remove debug code from Vision::measure

2 files changed, 7 insertions, 18 deletions

diff --git a/src/types.h b/src/types.h
index d3aeb01..d264cdc 100644
--- a/src/types.h
+++ b/src/types.h
@@ -23,6 +23,7 @@ typedef struct {
     double height;
     Vector2d Ssrc, Sref;
     cv::Mat patch;
+    double xcorrmax;
 } measurement_t;
 
 // A struct for storing PVA covariance.
diff --git a/src/vision.cpp b/src/vision.cpp
index dbd1f77..857bd0e 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -211,28 +211,16 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
     ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height);
     ellipserr.angle = 0;
     cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 );
-    cerr << searchrr.size << endl;
-    cerr << searchr.size() << endl;
     double minval, maxval;
     int minidx[2], maxidx[2];
     cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask);
-
-    // Display result
     cv::Point maxpt(maxidx[0], maxidx[1]);
-    Mat sr8, patch8;
-    sr.convertTo(sr8, CV_8UC1);
-    zin.patch.convertTo(patch8, 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(255),1);
-    cerr << maxval << endl;
-    cv::imshow("s. region", sr8);
-    cv::imshow("patch", patch8);
-    cv::imshow("result", result);
-    cv::imshow("mask", mask);
-    cv::imshow("display", display);
-    cv::waitKey(0);
+
+    cv::Point zpi = maxpt+searchr.tl();
+    Vector3d zi;
+    zi << zpi.x, zpi.y, 1;
+    zout.source = cam.img2body(zi);
+    zout.xcorrmax = maxval;
     return ;
 }		/* -----  end of method Vision::measure  ----- */