diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 8 |
1 files changed, 5 insertions, 3 deletions
diff --git a/src/state.h b/src/state.h index 67b235f..a0ebc45 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,7 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 50 +#define MAXFEATURES 30 #define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once using Eigen::Matrix; using Eigen::MatrixXd; @@ -45,10 +45,11 @@ class State void initializePi(int i, const Matrix<double,3,3> &Pi); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); + void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); void position_covariance(const Matrix<double,3,3> &p); + std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j); void velocity_covariance(const Matrix<double,3,3> &p); void vel(const Matrix<double,3,1> &v); - void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); void update ( const Matrix<double,1,1> &z); /* ==================== OPERATORS ======================================= */ @@ -61,8 +62,9 @@ class State private: /* ==================== METHODS ======================================= */ - void expandP(); void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q); + void expandP(); + void shrinkP( int i ); /* ==================== DATA MEMBERS ======================================= */ Body *body; |