diff options
Diffstat (limited to 'src/feature.h')
| -rw-r--r-- | src/feature.h | 26 |
1 files changed, 7 insertions, 19 deletions
diff --git a/src/feature.h b/src/feature.h index 56e0835..53eef2d 100644 --- a/src/feature.h +++ b/src/feature.h @@ -17,8 +17,7 @@ #define RANSAC_LI_THRESHOLD 4e-5 /* */ #define RANSAC_HI_THRESHOLD 5e-2 /* */ #define INITDEPTH -#define FULLSTATE - +#define STATESIZE 13 /* */ using Eigen::Dynamic; using Eigen::Matrix; @@ -59,25 +58,14 @@ class Feature /* ==================== OPERATORS ======================================= */ bool sane(); -#ifdef FULLSTATE - bool isInlier(const measurement_t &z, const Matrix<double,13,13> &Pxx, - const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy, - const Vector3d &pos, const Quaterniond &q, double thr); - Matrix<double,3,13> Fx( double dt ); - Matrix<double,6,13> Hx( const Vector3d &pos, const Quaterniond &q); - Matrix<double,6,6> S ( const Matrix<double,13,13> &Pxx, - const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy, - const Vector3d &pos, const Quaterniond &q); -#else - bool isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx, - const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, + bool isInlier(const measurement_t &z, const Matrix<double,STATESIZE,STATESIZE> &Pxx, + const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q, double thr); - Matrix<double,3,9> Fx( double dt ); - Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); - Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, - const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, + Matrix<double,3,STATESIZE> Fx( double dt ); + Matrix<double,6,STATESIZE> Hx( const Vector3d &pos, const Quaterniond &q); + Matrix<double,6,6> S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx, + const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q); -#endif bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q); bool inFOV(); Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs, |