diff options
Diffstat (limited to 'src/vision.cpp')
| -rw-r--r-- | src/vision.cpp | 135 |
1 files changed, 126 insertions, 9 deletions
diff --git a/src/vision.cpp b/src/vision.cpp index 7d1931d..0227053 100644 --- a/src/vision.cpp +++ b/src/vision.cpp @@ -41,14 +41,20 @@ Vision::Vision () void Vision::open ( const char *fn, const Camera &cam ) { - Mat bayered ; + Mat bayered, unbayered ; bayered = cv::imread(fn, -1); if (!bayered.data) { fprintf(stderr, "Could not read %s.\n", fn); exit(1); } - cv::cvtColor(bayered, original, CV_BayerBG2BGR ,3); - display = original.clone(); + cv::cvtColor(bayered, unbayered, CV_BayerBG2BGR ,3); + unbayered.convertTo(original, CV_32FC3); + Mat luv; + cv::cvtColor(original, gray, CV_BGR2GRAY); + //vector<Mat> luv_channels; + //cv::split(luv,luv_channels); + //gray = luv_channels[0]; + display = unbayered.clone(); return ; } /* ----- end of method Vision::open ----- */ @@ -66,7 +72,6 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam, cv::Scalar ellcol(0,0,200); for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) continue; - cerr << i->id << endl; Vector3d xis, xir; xis = cam.body2img(i->source); xis /= xis[2]; @@ -86,8 +91,8 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam, yr = 2*sqrt(i->Sref(1)); // Create rotated rect for ellipse - cv::RotatedRect rrs(ps, cv::Size(2*5.9*xs, 2*5.9*ys), 0.); - cv::RotatedRect rrr(pr, cv::Size(2*5.9*xr, 2*5.9*yr), 0.); + 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); } @@ -121,9 +126,12 @@ void Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z ) { vector<cv::Point2f> corners; - Mat gray; - cv::cvtColor(original, gray, CV_BGR2GRAY); - cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20); + + Mat mask = Mat::zeros(gray.size(),CV_8UC1); + cv::Rect roi(50,50,gray.cols-100, gray.rows-100); + mask(roi) = 255*Mat::ones(roi.size(),CV_8UC1); + + cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20, mask); for (auto i=corners.begin(); i!=corners.end(); ++i) { measurement_t m; m.id = _id++; @@ -132,9 +140,118 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z ) xi << i->x, i->y, 1.; m.source = cam.img2body(xi); m.reflection << 0,0,1; + getTemplate(m.patch,xi); + if (m.patch.cols==0 || m.patch.rows==0) continue; z.push_back(m); } return ; } /* ----- end of method Vision::acquireFeatures ----- */ +void +Vision::getTemplate( Mat &p, const Vector3d &pt) +{ + cv::Rect imrect(0,0,gray.cols,gray.rows); + assert (PATCHSIZE%2==1); // patch size must be odd + cv::Point pti; + pti.x = (int) pt(0); + pti.y = (int)pt(1); + cv::Point topLeft = pti - cv::Point(PATCHSIZE/2,PATCHSIZE/2); + cv::Size sz(PATCHSIZE,PATCHSIZE); + cv::Rect roi(topLeft, sz); + roi &= imrect; + p = gray(roi).clone(); +} + +void +Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin, + vector<measurement_t> &zout) +{ + for (auto i=zin.begin(); i!=zin.end(); ++i) { + measurement_t z; + measure(cam,*i, z); + zout.push_back(z); + } + return ; +} /* ----- end of method Vision::measurements ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Vision + * Method: Vision :: measure + * Description: Performs a measurement on zin and stores it in zout. + *-------------------------------------------------------------------------------------- + */ +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 + searchRegion(cam, zin, searchrr); + searchr = searchrr.boundingRect(); + + // Add margin around search region + 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 ); + 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,2,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::waitKey(0); + return ; +} /* ----- end of method Vision::measure ----- */ + +void +Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRect &rr ) +{ + Vector3d xis; + xis = cam.body2img(z.source); + //xis /= xis[2]; + cv::Point ps; + ps.x = xis[0]; + ps.y = xis[1]; + + double xs, ys; + xs = 2*sqrt(z.Ssrc(0)); + ys = 2*sqrt(z.Ssrc(1)); + + // Create rotated rect for ellipse + rr = cv::RotatedRect(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.); + + return ; +} /* ----- end of method Vision::searchRegion ----- */ + int Vision::_id = 1; |