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