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