diff options
Diffstat (limited to 'src/vision.cpp')
| -rw-r--r-- | src/vision.cpp | 30 |
1 files changed, 21 insertions, 9 deletions
diff --git a/src/vision.cpp b/src/vision.cpp index 0f89b35..4d4ba2e 100644 --- a/src/vision.cpp +++ b/src/vision.cpp @@ -41,19 +41,16 @@ Vision::Vision () void Vision::open ( const char *fn, const Camera &cam ) { - Mat bayered, unbayered ; + Mat bayered, unbayered, undistorted ; bayered = cv::imread(fn, -1); if (!bayered.data) { fprintf(stderr, "Could not read %s.\n", fn); exit(1); } cv::cvtColor(bayered, unbayered, CV_BayerBG2BGR ,3); - unbayered.convertTo(original, CV_32FC3); - Mat luv; + cv::undistort( unbayered, undistorted, cam.K(1), cam.d(1)); + undistorted.convertTo(original, CV_32FC3); 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 ----- */ @@ -173,6 +170,7 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin, for (auto i=zin.begin(); i!=zin.end(); ++i) { measurement_t z; z = *i; + measurement_type mt = z.z_type; if (i->z_type==MONO || i->z_type==BOTH) { measurement_t zins, zouts; zins = *i; @@ -180,6 +178,13 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin, measure(cam,zins, zouts); z.xcorrmax = zouts.xcorrmax; z.source = zouts.source; + if (z.xcorrmax<0.9) { //downgrade + if (mt==BOTH) { + mt=REFLECTION; + } else if (mt==MONO) { + continue; + } + } } if (i->z_type==REFLECTION || i->z_type==BOTH) { measurement_t zinr, zoutr; @@ -187,9 +192,16 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin, zinr.z_type = REFLECTION; measure(cam, zinr, zoutr); z.reflection = zoutr.reflection; + if (zoutr.xcorrmax<0.4) { //downgrade + if (mt==BOTH) { + mt = MONO; + } else if (mt==REFLECTION) { + continue; + } + } } - if (z.xcorrmax>0.90) - zout.push_back(z); + z.z_type = BOTH; + zout.push_back(z); } return ; } /* ----- end of method Vision::measurements ----- */ @@ -287,7 +299,7 @@ Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRec if (z.z_type==MONO) { xis = cam.body2img(z.source); } else { - xis = cam.body2img(z.source); + xis = cam.body2img(z.reflection); } //xis /= xis[2]; cv::Point ps; |