Remove features after 5 timesteps without measurements

4 files changed, 40 insertions, 1 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index fbb1376..84575d1 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -36,6 +36,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
     xb0w = xbw;
     q0 = q;
     _patch = p;
+    lastseen = 0;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
 /*
@@ -61,6 +62,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
     xb0w = xbw;
     q0 = q;
     _patch = p;
+    lastseen = 0;
 }
 
 
@@ -1023,6 +1025,7 @@ Feature::sane ( )
 void
 Feature::dx ( const Vector3d &del )
 {
+    lastseen += 1;
     X += del;
     return ;
 }		/* -----  end of method Feature::dx  ----- */
@@ -1131,3 +1134,16 @@ Feature::reflectedPatch( const Camera &cam, const Quaterniond &q1 )
     return cam.reflectPatch(_patch, q0, q1);
 }
 
+void
+Feature::seen (  )
+{
+    lastseen = 0;
+    return ;
+}		/* -----  end of method Feature::seen  ----- */
+
+bool
+Feature::since ( int N ) const
+{
+    return (lastseen<N) ;
+}		/* -----  end of method Feature::since  ----- */
+
diff --git a/src/feature.h b/src/feature.h
index 65850af..c0d3ac6 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -60,6 +60,8 @@ class Feature
         void asVector(const Vector3d &m);
         void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
         void dx( const Vector3d &del);
+        void seen();
+        bool since(int N) const;
 
         /* ====================  OPERATORS     ======================================= */
         bool sane();
@@ -98,6 +100,7 @@ class Feature
         /* ====================  DATA MEMBERS  ======================================= */
         int _id;
         Mat _patch;
+        unsigned int lastseen;
         Quaterniond q0;
         Vector3d X;
         Vector3d X0;
diff --git a/src/state.cpp b/src/state.cpp
index 055a3d1..10b3ad3 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1164,6 +1164,26 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
         kalmanUpdate(h,S,z,q);
 #endif
 #endif
+    // Reset lastseen counter
+    for (auto i=z.begin(); i!=z.end(); ++i) {
+        if (i->z_type==HEIGHT) continue;
+        Feature *ft = featureById(i->id);
+        if (ft==NULL) {
+            cerr << "id: " << i->id << "is null." << endl;
+        } else {
+            ft->seen();
+        }
+    }
+
+    // Remove features that haven't been measured recently
+    auto i=features.begin();
+    while (i!=features.end()) {
+        if ((*i)->since(5)) {
+            ++i;
+        } else {
+            i=removeFeature(i,false);
+        }
+    }
 
     // Initialize new features
     if (features.size()<MINFEATURES) {
diff --git a/src/state.h b/src/state.h
index 3ec5f10..70c73fe 100644
--- a/src/state.h
+++ b/src/state.h
@@ -11,7 +11,7 @@
 #include "types.h"
 #include "vision.h"
 
-#define MAXFEATURES 70
+#define MAXFEATURES 40
 #define MINFEATURES 30
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once