From 1c543489862cd0bf9c559cee6575baf20f84b2d0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 11 Apr 2017 10:57:04 -0500
Subject: Remove features after 5 timesteps without measurements

---
 src/state.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

(limited to 'src/state.cpp')

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) {
-- 
cgit v1.1