Nice quatviz-2 result

1 files changed, 3 insertions, 3 deletions

diff --git a/src/vision.cpp b/src/vision.cpp
index 07f292f..4b5ce98 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -149,9 +149,6 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z, const vec
 
     cv::goodFeaturesToTrack(gray, corners, 20, 0.1, 50, mask);
     for (auto i=corners.begin(); i!=corners.end(); ++i) {
-        measurement_t m;
-        m.id = _id++;
-        m.z_type = MONO;
         Vector3d xi;
         xi << i->x, i->y, 1.;
         bool tooClose = false;
@@ -163,6 +160,9 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z, const vec
             }
         }
         if (tooClose) continue;
+        measurement_t m;
+        m.id = _id++;
+        m.z_type = MONO;
         m.source = cam.img2body(xi);
         m.reflection << 0,0,1;
         getTemplate(m.patch,xi);