|
|
const int imageWidth = 640; //摄像头的分辨率
const int imageHeight = 480;
Size imageSize = Size(imageWidth, imageHeight);
Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;
Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
Mat xyz; //三维坐标
Point origin; //鼠标按下的起始点
Rect selection; //定义矩形选框
bool selectObject = false; //是否选择对象
//int blockSize = 3, uniquenessRatio = 10, numDisparities = 16;
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);
/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0 1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 730.9859, 0.1427 , 294.2559,
0 , 735.3010, 261.9521,
0 , 0 , 1 );
Mat distCoeffL = (Mat_<double>(5, 1) << -0.4740, 0.1766,0.0017, -0.0039,0.1152);
Mat cameraMatrixR = (Mat_<double>(3, 3) << 734.7534, 0.2424 , 283.8769,
0 , 737.4809, 342.1307,
0 , 0 , 1 );
Mat distCoeffR = (Mat_<double>(5, 1) << -0.5229, 0.6307,-0.0004, -0.0037,-0.7994);
Mat T = (Mat_<double>(3, 1) << 9.1664, 0.0753, -0.2054);//T平移向量
//Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量
Mat R = (Mat_<double>(3, 3) << 0.9997, 0.0175, -0.0140,
-0.0172, 0.9996, 0.0207,
0.0144, -0.0204, 0.9997);
/*****立体匹配*****/
void stereo_match(int, void*)
{
sgbm->setPreFilterCap(15);
int sgbmWinSize = 5;//根据实际情况自己设定
int NumDisparities = 624;//根据实际情况自己设定
int UniquenessRatio = 10;//根据实际情况自己设定
sgbm->setBlockSize(sgbmWinSize);
int cn = rectifyImageL.channels();
sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(NumDisparities);
sgbm->setUniquenessRatio(UniquenessRatio);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(10);
sgbm->setDisp12MaxDiff(1);
sgbm->setMode(StereoSGBM::MODE_SGBM);
Mat disp, dispf, disp8;
sgbm->compute(rectifyImageL, rectifyImageR, disp);
//去黑边
Mat img1p, img2p;
copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
dispf = disp.colRange(NumDisparities, img2p.cols - NumDisparities);
dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
reprojectImageTo3D(dispf, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
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 = abs(x - origin.x);
selection.height = 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(int argc, char** argv)
{
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
-1, imageSize, &validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);
/*
读取图片
*/
rgbImageL = imread("came04L.jpg", -1);
cvtColor(rgbImageL, grayImageL, COLOR_BGR2GRAY);
rgbImageR = imread("came04R.jpg", -1);
cvtColor(rgbImageR, grayImageR, COLOR_BGR2GRAY);
imshow("ImageL Before Rectify", grayImageL);
imshow("ImageR Before Rectify", grayImageR);
/*
经过remap之后，左右相机的图像已经共面并且行对准了
*/
remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
/*
把校正结果显示出来
*/
Mat rgbRectifyImageL, rgbRectifyImageR;
cvtColor(rectifyImageL, rgbRectifyImageL, COLOR_GRAY2BGR); //伪彩色图
cvtColor(rectifyImageR, rgbRectifyImageR, COLOR_GRAY2BGR);
//单独显示
//rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
//rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
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); //把图像缩放到跟canvasPart一样大小
Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf), //获得被截取的区域
cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8); //画上一个矩形
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));
rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
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", WINDOW_NORMAL);
setMouseCallback("disparity", onMouse, 0);
stereo_match(0,0);
waitKey(0);
return 0;
}
D:/18982/Pictures/94E02F691CB803306287A2309D13D459.jpg |
|
发表于 2018-11-20 18:44:50
楼主