From e5c3b5117c2f3936967319223e353ac6c7a0b1ca Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 9 Apr 2017 13:25:13 -0500
Subject: warp source and reflection patches

---
 src/vision.cpp | 88 ++++++++++++++++++++++++++++++++++++++--------------------
 1 file changed, 58 insertions(+), 30 deletions(-)

(limited to 'src/vision.cpp')

diff --git a/src/vision.cpp b/src/vision.cpp
index 857bd0e..5d46f02 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -93,8 +93,12 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam,
             // Create rotated rect for ellipse
             cv::RotatedRect rrs(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.);
             cv::RotatedRect rrr(pr, cv::Size(2*INLIER_THRESHOLD*xr, 2*INLIER_THRESHOLD*yr), 0.);
-            cv::ellipse(display, rrs, ellcol );
-            cv::ellipse(display, rrr, ellcol);
+            try {
+                cv::ellipse(display, rrs, ellcol );
+                cv::ellipse(display, rrr, ellcol);
+            } catch (...) {
+                cerr << "caught and error drawing ellipse" << endl;
+            }
         }
     }
     return ;
@@ -169,7 +173,8 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
     for (auto i=zin.begin(); i!=zin.end(); ++i) {
         measurement_t z;
         measure(cam,*i, z);
-        zout.push_back(z);
+        if (z.xcorrmax>0.90)
+            zout.push_back(z);
     }
     return ;
 }		/* -----  end of method Vision::measurements  ----- */
@@ -184,8 +189,6 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
 void
 Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zout )
 {
-    zout.id = zin.id;
-
     // Create ROIs
     RotatedRect searchrr; // Rotated rect describing search ellipse
     Rect searchr; // Rect describing search ellipse
@@ -193,34 +196,59 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
     searchr = searchrr.boundingRect();
 
     // Add margin around search region
+    Rect imrect(cv::Point(0,0), gray.size());
     cv::Point offset(PATCHSIZE/2,PATCHSIZE/2);
     cv::Size enlarge(2*(PATCHSIZE/2),2*(PATCHSIZE/2));
     Rect roi = (searchr-offset) + enlarge;
-    Mat sr = gray(roi);
-
-    // Match template
-    Mat result;
-    result.create( sr.cols-zin.patch.cols+1, sr.rows-zin.patch.rows+1, CV_32FC1 );
-    matchTemplate( sr, zin.patch, result, CV_TM_CCORR_NORMED);
-
-    // Mask the ellipse around the result.
-    Mat mask = Mat::zeros(result.size(), CV_8UC1);
-    RotatedRect ellipserr;
-    cv::Size sz = searchr.size();
-    ellipserr.size = sz;
-    ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height);
-    ellipserr.angle = 0;
-    cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 );
-    double minval, maxval;
-    int minidx[2], maxidx[2];
-    cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask);
-    cv::Point maxpt(maxidx[0], maxidx[1]);
-
-    cv::Point zpi = maxpt+searchr.tl();
-    Vector3d zi;
-    zi << zpi.x, zpi.y, 1;
-    zout.source = cam.img2body(zi);
-    zout.xcorrmax = maxval;
+    if ((roi & imrect)==roi) {
+        Mat sr = gray(roi);
+
+        // Match template
+        Mat result;
+        result.create( sr.cols-zin.patch.cols+1, sr.rows-zin.patch.rows+1, CV_32FC1 );
+        matchTemplate( sr, zin.patch, result, CV_TM_CCOEFF_NORMED);
+
+        // Mask the ellipse around the result.
+        Mat mask = Mat::zeros(result.size(), CV_8UC1);
+        RotatedRect ellipserr;
+        cv::Size sz = searchr.size();
+        ellipserr.size = sz;
+        ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height);
+        ellipserr.angle = 0;
+        cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 );
+        double minval, maxval;
+        int minidx[2], maxidx[2];
+        cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask);
+        cv::Point maxpt(maxidx[1], maxidx[0]);
+
+        cv::Point zpi = maxpt+searchr.tl();
+        Vector3d zi;
+        zi << zpi.x, zpi.y, 1;
+
+        zout.source = cam.img2body(zi);
+        zout.xcorrmax = maxval;
+
+        Mat sr8, patch8, rpatch8;
+        sr.convertTo(sr8, CV_8UC1);
+        zin.patch.convertTo(patch8, CV_8UC1);
+        zin.refpatch.convertTo(rpatch8, CV_8UC1);
+        cv::circle(sr8, maxpt+offset, 4, cv::Scalar(255), 1);
+        cv::Point spt(PATCHSIZE/2, PATCHSIZE/2);
+        cv::circle(patch8, spt, 4, cv::Scalar(255), 1);
+        cv::circle(display, maxpt+searchr.tl(), 4, cv::Scalar(20,240,0),2);
+        cv::imshow("s region", sr8);
+        cv::imshow("patch", patch8);
+        cv::imshow("rpatch", rpatch8);
+        cv::imshow("result", result);
+        cv::imshow("mask", mask);
+        cv::imshow("display2", display);
+        cerr << maxval << endl;
+        cv::waitKey(0);
+    } else {
+        zout.xcorrmax = -1.;
+    }
+    zout.id = zin.id;
+    zout.z_type = MONO;
     return ;
 }		/* -----  end of method Vision::measure  ----- */
 
-- 
cgit v1.1