diff options
Diffstat (limited to 'src/feature.h')
| -rw-r--r-- | src/feature.h | 21 |
1 files changed, 16 insertions, 5 deletions
diff --git a/src/feature.h b/src/feature.h index db4f05e..6260d71 100644 --- a/src/feature.h +++ b/src/feature.h @@ -58,16 +58,30 @@ 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, 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, + 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, const Vector3d &xr); - Matrix<double,3,9> Fx( double dt ); Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt); - Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); Matrix<double,3,6> L(); @@ -76,9 +90,6 @@ class Feature Matrix<double,3,3> Q(const double dt); Matrix<double,6,6> R(); - Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, - const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, - const Vector3d &pos, const Quaterniond &q); Vector3d p2x(const Vector3d &p); Vector3d x2p(const Vector3d &x); |