diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/state.cpp | 10 |
1 files changed, 6 insertions, 4 deletions
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 |