diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 37 |
1 files changed, 31 insertions, 6 deletions
diff --git a/src/state.h b/src/state.h index a7f7df5..f330b67 100644 --- a/src/state.h +++ b/src/state.h @@ -15,7 +15,7 @@ //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ //#define INLIERTEST /* */ - +#define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -44,14 +44,20 @@ class State int rowById(int id); int iById(int id); Feature *featureById(int id); +#if STATESIZE==13 + MatrixXd F(const Vector3d &w, double dt); + MatrixXd H ( const std::vector<measurement_t> &z ); + MatrixXd blockSI( const std::vector<measurement_t> &z ); +#else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); + MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q); +#endif int Hrows( const std::vector<measurement_t> &z ); MatrixXd Q(double dt); MatrixXd R( const std::vector<measurement_t> &z ); - MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q); - Matrix<double,9,9> Pxx(); - Matrix<double,9,3> Pxy(int id); + Matrix<double,STATESIZE,STATESIZE> Pxx(); + Matrix<double,STATESIZE,3> Pxy(int id); Matrix<double,3,3> Pyy(int id); Matrix<double,Dynamic,6> L(); @@ -59,16 +65,27 @@ class State void accelerometer_bias(const Vector3d &b); void asVector(const Matrix<double,Dynamic,1> &m); void pos(const UTM &utm); +#if STATESIZE==13 + void handle_measurements( const std::vector<measurement_t> & z); + void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector<measurement_t> &z); + void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt); + MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, + const std::vector<measurement_t> &z); + void Pkk1 ( const Vector3d &ang, const double dt); + void ransacUpdate(std::vector<measurement_t> &z); +#else void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); - void initializePi(int i, const Matrix<double,3,3> &Pi); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector<measurement_t> &z, const Quaterniond &q); - void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector<measurement_t> &z, const Quaterniond &q ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); + void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q); +#endif + void initializePi(int i, const Matrix<double,3,3> &Pi); void position_covariance(const Matrix<double,3,3> &p); std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, bool bl); std::list<Feature *>::iterator removeFeature ( int id, bool bl ); @@ -76,7 +93,11 @@ class State void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ +#if STATESIZE==13 + Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z); +#else Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); +#endif void unicsv(); protected: @@ -86,7 +107,11 @@ class State private: /* ==================== METHODS ======================================= */ +#if STATESIZE==13 + void addFeatures(std::vector<measurement_t> &F, double z); +#else void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z); +#endif void expandP ( const Matrix3d &Pi); void shrinkP( int i ); |