Add code to State for removing features.

After updating the state according to the motion model we check if
features are still in the field of view. If they are not they are
cleanly removed. Removal requires deletion of the feature, removal from
the features list and shrinkng the P matrix.

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;