1 files changed, 56 insertions, 3 deletions

diff --git a/src/state.cpp b/src/state.cpp
index a5a09fd..dae6a4d 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -80,9 +80,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 {
     // Add new features
     for (auto i=F.begin(); i!=F.end(); ++i) {
-        // Estimate initial depth
-        Vector3d xib;
-
+        if (features.size()>MAXFEATURES) break;
         // Create feature
         Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
         if (!f->sane()) {
@@ -230,12 +228,46 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang,
         (*i)->motionModel(ang,vel,dt);
     }
 #endif     /* -----  not FASTMOTIONMODEL  ----- */
+
+    // Remove features that leave FOV
+    {
+        auto i = features.begin();
+        int j=0;
+        while (i!=features.end()) {
+            if ((*i)->inFOV()) {
+                ++i;
+                ++j;
+            } else {
+                i=removeFeature(i,j);
+            }
+        }
+    }
+
+
     return ;
 }		/* -----  end of method State::motionModel  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
+ *      Method:  State :: removeFeature
+ * Description:  Remove the Feature i and Pj in a clean fashion.
+ *--------------------------------------------------------------------------------------
+ */
+std::list<Feature *>::iterator
+State::removeFeature ( std::list<Feature *>::iterator &i, int j )
+{
+    // It is important to shrink P first, because it depends on knowing the
+    // current size of the feature vector.
+    shrinkP(j);
+    delete *i;
+    i = features.erase(i);
+    return i;
+}		/* -----  end of method State::removeFeature  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
  *      Method:  State :: L
  * Description:  Return the composition of all Li's of the features
  *--------------------------------------------------------------------------------------
@@ -319,3 +351,24 @@ State::Pxx ( )
     return P.topLeftCorner<9,9>() ;
 }		/* -----  end of method State::Pxx  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: shrinkP
+ * Description:  Delete the ith feature from P
+ *--------------------------------------------------------------------------------------
+ */
+void
+State::shrinkP ( int i )
+{
+    int N = 9 + 3*features.size();
+    int I = 9 + 3*i;
+
+    P.block(I,0,N-I-3,I) = P.bottomLeftCorner(N-I-3,I);
+    P.block(0,I,I,N-I-3) = P.topRightCorner(I,N-I-3);
+    P.block(I,I,N-I-3,N-I-3) = P.bottomRightCorner(N-I-3,N-I-3);
+    P.conservativeResize(N-3,N-3);
+
+    return ;
+}		/* -----  end of method State::shrinkP  ----- */
+