Acquire features will ignore new points near existing features.

2 files changed, 30 insertions, 5 deletions

diff --git a/src/vision.cpp b/src/vision.cpp
index 2b190c0..07f292f 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -120,25 +120,49 @@ Vision::show (  )
  *--------------------------------------------------------------------------------------
  *       Class:  Vision
  *      Method:  Vision :: acquireFeatures
- * Description:  Finds new features, assigns IDs, extracts descriptors.
+ * Description:  Finds new features, assigns IDs, extracts descriptors. The
+ * vector curz contains a list of existing features i.e. areas to mask off when
+ * runing goodfeaturestotrack.
  *--------------------------------------------------------------------------------------
  */
 void
-Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z )
+Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z, const vector<measurement_t> &curz )
 {
     vector<cv::Point2f> corners;
 
     Mat mask = Mat::zeros(gray.size(),CV_8UC1);
+    // mask off the margins of the image
     cv::Rect roi(50,50,gray.cols-100, gray.rows-100);
     mask(roi) = 255*Mat::ones(roi.size(),CV_8UC1);
 
-    cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20, mask);
+    // mask off the existing points
+    for (auto i=curz.begin(); i!=curz.end(); ++i) {
+        Vector3d xi;
+        xi = cam.body2img(i->source);
+        cv::Point tl;
+        tl.x = xi(0);
+        tl.y = xi(1);
+        cv::Size sz(50,50);
+        cv::Rect box(tl,sz);
+        cv::rectangle(mask,box,cv::Scalar(0),-1);
+    }
+
+    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;
+        for (auto j=curz.begin(); j!=curz.end(); ++j) {
+            Vector3d xci = cam.body2img(j->source);
+            if ((xci-xi).norm()<25) {
+                tooClose = true;
+                break;
+            }
+        }
+        if (tooClose) continue;
         m.source = cam.img2body(xi);
         m.reflection << 0,0,1;
         getTemplate(m.patch,xi);
@@ -178,7 +202,7 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
             measure(cam,zins, zouts);
             z.xcorrmax = zouts.xcorrmax;
             z.source = zouts.source;
-            if (z.xcorrmax<0.9) { //downgrade
+            if (z.xcorrmax<0.90) { //downgrade
                 if (mt==BOTH) {
                     mt=REFLECTION;
                 } else if (mt==MONO) {
diff --git a/src/vision.h b/src/vision.h
index aaa982b..a0490d1 100644
--- a/src/vision.h
+++ b/src/vision.h
@@ -39,7 +39,8 @@ class Vision
 
         /* ====================  OPERATORS     ======================================= */
         void open(const char *fn, const Camera &cam);
-        void acquireFeatures(const Camera &cam, vector<measurement_t> &z);
+        void acquireFeatures(const Camera &cam, vector<measurement_t> &z,
+                const vector<measurement_t> &curz);
         void getTemplate( Mat &p, const Vector3d &pt);
         void measure( const Camera &cam, const measurement_t &zin, measurement_t &zout);
         void measurements( const Camera &cam, const vector<measurement_t> &zin,