From c1d17dff9d89735cb2fa5e6ae95c9ad9e8a973cd Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 11 Apr 2017 13:52:08 -0500
Subject: Update State for Vision::acquireFeatures

---
 src/state.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 10b3ad3..cba336e 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1130,10 +1130,7 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
     measurement_t hgt = z[0];
     assert (hgt.z_type==HEIGHT);
     double hgtmeas = hgt.height;
-    vector<measurement_t> newFeatures;
-    if (features.size()<MINFEATURES) {
-        viz.acquireFeatures(cam, newFeatures);
-    }
+    
     // Measure existing features
     vector<measurement_t> currentFeatures;
 #if STATESIZE==13
@@ -1141,6 +1138,11 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
 #else
     featuresAsMeasurements(currentFeatures,cam,q);
 #endif
+
+    vector<measurement_t> newFeatures;
+    if (features.size()<MINFEATURES) {
+        viz.acquireFeatures(cam, newFeatures,currentFeatures);
+    }
     viz.measurements(cam, currentFeatures, z);
 
 #ifdef DORANSAC
-- 
cgit v1.1