Remove features after 5 timesteps without measurements

1 files changed, 20 insertions, 0 deletions

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) {