立体匹配和测距
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;
const int imageWidth = 800;
const int imageHeight = 600;
Size imageSize = Size(imageWidth, imageHeight);
Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;
Rect validROIL;
Rect validROIR;
Mat mapLx, mapLy, mapRx, mapRy;
Mat Rl, Rr, Pl, Pr, Q;
Mat xyz;
Point origin;
Rect selection;
bool selectObject = false;
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);
Mat cameraMatrixL = (Mat_<double>(3, 3) << 682.55880, 0, 384.13666,
0, 682.24569, 311.19558,
0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << -0.51614, 0.36098, 0.00523, -0.00225, 0.00000);
Mat cameraMatrixR = (Mat_<double>(3, 3) << 685.03817, 0, 397.39092,
0, 682.54282, 272.04875,
0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.46640, 0.22148, 0.00947, -0.00242, 0.00000);
Mat T = (Mat_<double>(3, 1) << -61.34485, 2.89570, -4.76870);
Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);
Mat R;
void stereo_match(int, void*)
{
bm->setBlockSize(2 * blockSize + 5);
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0);
bm->setNumDisparities(numDisparities * 16 + 16);
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp, disp8;
bm->compute(rectifyImageL, rectifyImageR, disp);
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));
reprojectImageTo3D(disp, xyz, Q, true);
xyz = xyz * 16;
imshow("disparity", disp8);
}
static void onMouse(int event, int x, int y, int, void*)
{
if (selectObject)
{
selection.x = MIN(x, origin.x);
selection.y = MIN(y, origin.y);
selection.width = std::abs(x - origin.x);
selection.height = std::abs(y - origin.y);
}
switch (event)
{
case EVENT_LBUTTONDOWN:
origin = Point(x, y);
selection = Rect(x, y, 0, 0);
selectObject = true;
cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
break;
case EVENT_LBUTTONUP:
selectObject = false;
if (selection.width > 0 && selection.height > 0)
break;
}
}
int main()
{
Rodrigues(rec, R);
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
0, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
rgbImageL = imread("left.png", CV_LOAD_IMAGE_COLOR);
cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
rgbImageR = imread("right.png", CV_LOAD_IMAGE_COLOR);
cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
imshow("ImageL Before Rectify", grayImageL);
imshow("ImageR Before Rectify", grayImageR);
remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
Mat rgbRectifyImageL, rgbRectifyImageR;
cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);
cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
imshow("ImageL After Rectify", rgbRectifyImageL);
imshow("ImageR After Rectify", rgbRectifyImageR);
Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width * sf);
h = cvRound(imageSize.height * sf);
canvas.create(h, w * 2, CV_8UC3);
Mat canvasPart = canvas(Rect(w * 0, 0, w, h));
resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),
cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
cout << "Painted ImageL" << endl;
canvasPart = canvas(Rect(w, 0, w, h));
resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
cout << "Painted ImageR" << endl;
for (int i = 0; i < canvas.rows; i += 16)
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
namedWindow("disparity", CV_WINDOW_AUTOSIZE);
createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match);
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
setMouseCallback("disparity", onMouse, 0);
stereo_match(0, 0);
waitKey(0);
return 0;
}