diff options
| author | Martin Miller | 2017-04-04 14:07:24 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-04-04 14:07:24 -0500 |
| commit | 0c8912c5dc1ec3c752f8648ca2a8181275ba2654 (patch) | |
| tree | eb8265319362a9fafe8dc46575bc044dca5e0518 /src/feature.h | |
| parent | 623f94f85a49f1b720a025c680769ba2ba028ba0 (diff) | |
| download | refslam-0c8912c5dc1ec3c752f8648ca2a8181275ba2654.zip refslam-0c8912c5dc1ec3c752f8648ca2a8181275ba2654.tar.gz | |
Use STATESIZE for Feature
Replace FULLSTATE define with STATESIZE.
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, |