双目相机测距代码演示

双目测距的操作流程有四步：相机标定——双目校正——双目匹配——计算深度，具体内容参考： https://blog.csdn.net/qq_38236355/article/details/88933839

其中相机标定通常用matlab现成的工具箱进行标定，具体操作参考： https://blog.csdn.net/qq_38236355/article/details/89280633

我们接下来在完成相机标定的基础上，用标定得到的数据，按上述流程对双目深度进行计算。如果用的是任意的两个单目摄像头拼成的双目相机，则需要按上述所说进行双目校正；如果用的是成品的双目相机，则不需要进行双目校正，商家已经对相机校正好了。

一、双目校正+BM算法双目匹配生成视差图+生成深度图

#include <opencv2\opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);Mat grayImageL;
Mat grayImageR;
Mat rectifyImageL, rectifyImageR;
Mat disp, disp8;//视差图Rect vaildROIL;
Rect vaildROIR;//图像校正后，会对图像进行裁剪，这里的vaildROIR就是指裁剪之后的区域Mat maplx, maply, mapRx, mapRy;//映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P，重投影矩阵Q
Mat xyz;
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM>bm = StereoBM::create(16, 9);//事先用matlab标定好的相机参数
//matlab里的左相机标定参数矩阵
Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);
//matlab里左相机畸变参数
Mat distCoeffL = (Mat_<double>(5, 1) << -0.0218, -0.0014, -0.0104, -0.0025, -0.000024286);
//matlab右相机标定矩阵
Mat cameraMatrixR = (Mat_<double>(3, 3) << 168.2781, 0.1610, 413.2010, 0, 167.7710, 304.7487, 0, 0, 1);
//matlab右相机畸变参数
Mat distCoffR = (Mat_<double>(5, 1) << -0.0332, 0.0033, -0.0090, -0.0029, -0.00038324);
//matlab T 平移参数
Mat T = (Mat_<double>(3, 1) << -117.2789, -0.8970, 0.9281);
//旋转矩阵
Mat R = (Mat_<double>(3, 3) << 1.0000, -0.0040, -0.000052, 0.0040, 1.0000, -0.0041, 0.0000683, 0.0041, 1.0000);
//立体匹配 BM算法
void stereo_match(int, void*)
{bm->setBlockSize(2 * blockSize + 5);//SAD窗口大小bm->setROI1(vaildROIL);bm->setROI2(vaildROIR);bm->setPreFilterCap(31);bm->setMinDisparity(0);//最小视差，默认值为0bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，最大视差值与最小视差值之差bm->setTextureThreshold(10);bm->setUniquenessRatio(uniquenessRatio);//可以防止误匹配bm->setSpeckleWindowSize(100);bm->setSpeckleRange(32);bm->setDisp12MaxDiff(-1);bm->compute(rectifyImageL, rectifyImageR, disp);//生成视差图disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式,将其转化为CV_8U的形式reprojectImageTo3D(disp, xyz, Q, true);xyz = xyz * 16;//在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。imshow("disp image", disp8);
}
//生成深度图
void disp2Depth(int, void*)
{Mat depthMap = Mat(disp.size(), CV_16UC1, 255);int type = disp.type();float fx = cameraMatrixL.at<float>(0, 0);float fy = cameraMatrixL.at<float>(1, 1);float cx = cameraMatrixL.at<float>(0, 2);float cy = cameraMatrixL.at<float>(1, 2);float baseline = 40; //基线距离40mmif (type == CV_16S){int height = disp.rows;int width = disp.cols;short* dispData = (short*)disp.data;ushort* depthData = (ushort*)depthMap.data;for (int i = 0; i < height; i++){for (int j = 0; j < width; j++){int id = i * width + j;if (!dispData[id]){continue;}depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));}}}else{cout << "please onfirm dispImage's type" << endl;}Mat depthMap8;depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式imshow("depth image", depthMap8);
}
int main()
{//双目校正stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &vaildROIL, &vaildROIR);initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl , imageSize, CV_32FC1, maplx, maply);initUndistortRectifyMap(cameraMatrixR, distCoffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);//读取图像grayImageL = imread("D:/opencv learning/diannao_left.png", 0);grayImageR = imread("D:/opencv learning/diannao_right.png", 0);if (grayImageR.empty() || grayImageL.empty()){printf("can not load image...");return -1;}imshow("imageL before rectify", grayImageL);imshow("imageR bedore rectify", grayImageR);//经过remap之后，左右相机的图像已经共面并且行对准了remap(grayImageL, rectifyImageL, maplx, maply, INTER_LINEAR);remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);//把校正结果显示出来imshow("imageL after rectify", rectifyImageL);imshow("imageR after rectify", rectifyImageR);//显示在同一张图上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_8UC1);//左图像画到画布上Mat canvaspart = canvas(Rect(w * 0, 0, w, h));resize(rectifyImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));cout << "Painted imagel" << endl;//右图像画到画布上canvaspart = canvas(Rect(w, 0, w, h));resize(rectifyImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.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("disp image", CV_WINDOW_AUTOSIZE);createTrackbar("blocksize:\n", "disp image", &blockSize, 8, stereo_match);createTrackbar("UniquenessRatio:\n", "disp image", &uniquenessRatio, 50, stereo_match);createTrackbar("NumDisparities:\n", "disp image", &numDisparities, 16, stereo_match);stereo_match(0, 0);disp2Depth(0, 0);waitKey(0);return 0;
}

二、成品双目无双目校正，BM算法双目匹配生成视差图+生成深度图

#include <opencv2/opencv.hpp>
#include <iostream>
using namespace std;
using namespace cv;const int imagewidth = 752;
const int imageheigh = 480; // 摄像头分辨率752*480
Size imageSize = Size(imagewidth, imageheigh);int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9); //用Block Matching算法,实现双目匹配Mat grayImageL, grayImageR , disp, disp8;Rect vaildROIL = Rect(0, 0, 752, 480);
Rect vaildROIR = Rect(0, 0, 752, 480);Mat cameraMatrixL = (Mat_<float>(3, 3) << 165.9419, -0.2471, 372.8349, 0, 164.8281, 325.8182, 0, 0, 1);void stereo_match(int, void*); //双目匹配,生成视差图（xl-xr）
void disp2Depth(int, void*); //视差图转为深度图int main()
{grayImageL = imread("D:/opencv learning/liangdui_left.png", 0);grayImageR = imread("D:/opencv learning/liangdui_right.png", 0);if (grayImageL.empty() || grayImageR.empty()){printf("can not load image...");}imshow("input ImageL", grayImageL);imshow("input ImageR", grayImageR);//显示在同一张图上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_8UC1);//左图像画到画布上Mat canvaspart = canvas(Rect(w * 0, 0, w, h));resize(grayImageL, canvaspart, canvaspart.size(), 0, 0, INTER_AREA);Rect vroil(cvRound(vaildROIL.x*sf), cvRound(vaildROIL.y*sf), cvRound(vaildROIL.width*sf), cvRound(vaildROIL.height*sf));cout << "Painted imagel" << endl;//右图像画到画布上canvaspart = canvas(Rect(w, 0, w, h));resize(grayImageR, canvaspart, canvaspart.size(), 0, 0, INTER_LINEAR);Rect vroiR(cvRound(vaildROIR.x*sf), cvRound(vaildROIR.y*sf), cvRound(vaildROIR.width*sf), cvRound(vaildROIR.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);//创建SAD窗口TrackbarcreateTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);//创建视差唯一性百分比窗口 TrackbarcreateTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);// 创建视差窗口 Trackbarstereo_match(0, 0);disp2Depth(0, 0);waitKey(0);return 0;
}
void stereo_match(int, void*)
{bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5-21之间为宜bm->setROI1(vaildROIL);bm->setROI2(vaildROIR);bm->setPreFilterCap(31);bm->setMinDisparity(0); //最小视差bm->setNumDisparities(numDisparities * 16 + 16);bm->setTextureThreshold(10);bm->setUniquenessRatio(uniquenessRatio); //uniquenessRatio主要可以防止误匹配bm->setSpeckleRange(32);bm->setSpeckleWindowSize(100);bm->setDisp12MaxDiff(-1);bm->compute(grayImageL, grayImageR, disp); //输入图像必须为灰度图,输出视差图disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式imshow("disparity", disp8);
}
void disp2Depth(int, void*)
{Mat depthMap = Mat(disp.size(), CV_16UC1, 255);int type = disp.type();float fx = cameraMatrixL.at<float>(0, 0);float fy = cameraMatrixL.at<float>(1, 1);float cx = cameraMatrixL.at<float>(0, 2);float cy = cameraMatrixL.at<float>(1, 2);float baseline = 40; //基线距离40mmif (type == CV_16S){int height = disp.rows;int width = disp.cols;short* dispData = (short*)disp.data;ushort* depthData = (ushort*)depthMap.data;for (int i = 0; i < height; i++){for (int j = 0; j < width; j++){int id = i * width + j;if (!dispData[id]){continue;}depthData[id] = ushort((float)fx*baseline / ((float)dispData[id]));}}}else{cout << "please onfirm dispImage's type" << endl;}Mat depthMap8;depthMap.convertTo(depthMap8, CV_8U);//计算出的深度图是CV_16U格式imshow("depth image", depthMap8);
}

我拍了一幅电脑的图片，效果如下图所示：

左右原图：
在这里插入图片描述
校正后放到同一平面：

视差图：

深度图：