From f409d322fd400b412c632b74f76c6c9794fbf6c0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 25 Mar 2017 17:22:44 -0500
Subject: 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.
---
 src/state.cpp | 59 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 56 insertions(+), 3 deletions(-)

(limited to 'src/state.cpp')

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  ----- */
+
-- 
cgit v1.1