diff options
| -rw-r--r-- | src/types.h | 1 | ||||
| -rw-r--r-- | src/vision.cpp | 24 |
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 ----- */ |