1.项目背景
使用两个型号和镜头一样的相机进行双目视觉算法开发，测量远距离下的物体距离
2.开发环境和使用设备
VS2019+opencv454 ，matlab2021a进行双目相机标定，相同型号的相机镜头2个，保证能够同时触发
3.详细设计方案
（1）双目设备安装要求及计算原理
使用的是平行式双目视觉测量方法，两个相机平行安装，安装距离间隔1米。
成像原理如下图所示：
建立空间直角坐标系，构建相似三角形，求视差：
图5-2 双目视觉测距原理
如图5-2所示，由相似三角形原理可知，PL1L2与PC1C2相似，建立方程求解公式如5-1所示：
化简后：
其中B为两个相机之间的距离，称为基线距离，f为相机焦距， 表示视差，用d表示。公式5-2中，基线距离B和焦距f已知，只需要求得视差d就可以得到物体距离。
（2）使用matlab进行双目相机标定
1）标定前需要对两个相机的图像进行编号处理，保证一一对应
2）标定的图像数量大于40张，左右相机各20张左右。
3）标定板移动范围尽可能的覆盖需要测量的视野范围。
4）标定板不要太小，远距离情况下，标定板太小标出来不准，投影误差会比较大，对精度要求高的时候需要更换更大的标定板。
（3）标定结果如下，其中平均的投影误差在1.3个像素左右。
（4）标定验证
保证图像的y相同，便于后面进行立体匹配，也是检查标定的结果是否准确
图中立体校正后结果基本上能对上，误差可接受，需要迁移到c++工程中，保存两个相机的内参、畸变系数和共同的R、T矩阵。
（5）matlab保存标定结果
以下matlab代码参考某位博主的：
rowName = cell(1,10);
rowName{
1,1} = '平移矩阵';
rowName{
1,2} = '旋转矩阵';
rowName{
1,3} = '相机1内参矩阵';
rowName{
1,4} = '相机1径向畸变';
rowName{
1,5} = '相机1切向畸变';
rowName{
1,6} = '相机2内参矩阵';
rowName{
1,7} = '相机2径向畸变';
rowName{
1,8} = '相机2切向畸变';
rowName{
1,9} = '相机1畸变向量';
rowName{
1,10} = '相机2畸变向量';
xlswrite('out.xlsx',rowName(1,1),1,'A1');
xlswrite('out.xlsx',rowName(1,2),1,'A2');
xlswrite('out.xlsx',rowName(1,3),1,'A5');
xlswrite('out.xlsx',rowName(1,4),1,'A8');
xlswrite('out.xlsx',rowName(1,5),1,'A9');
xlswrite('out.xlsx',rowName(1,6),1,'A10');
xlswrite('out.xlsx',rowName(1,7),1,'A13');
xlswrite('out.xlsx',rowName(1,8),1,'A14');
xlswrite('out.xlsx',rowName(1,9),1,'A15');
xlswrite('out.xlsx',rowName(1,10),1,'A16');
xlswrite('out.xlsx',stereoParams.TranslationOfCamera2,1,'B1'); % 平移矩阵
xlswrite('out.xlsx',stereoParams.RotationOfCamera2.',1,'B2'); % 旋转矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.IntrinsicMatrix.',1,'B5'); % 相机1内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.RadialDistortion,1,'B8'); % 相机1径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters1.TangentialDistortion,1,'B9'); % 相机1切向畸变(3,4)
xlswrite('out.xlsx',stereoParams.CameraParameters2.IntrinsicMatrix.',1,'B10'); % 相机2内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters2.RadialDistortion,1,'B13'); % 相机2径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters2.TangentialDistortion,1,'B14'); % 相机2切向畸变(3,4)
xlswrite('out.xlsx',[stereoParams.CameraParameters1.RadialDistortion(1:2), stereoParams.CameraParameters1.TangentialDistortion,...
stereoParams.CameraParameters1.RadialDistortion(3)],1,'B15'); % 相机1畸变向量
xlswrite('out.xlsx',[stereoParams.CameraParameters2.RadialDistortion(1:2), stereoParams.CameraParameters2.TangentialDistortion,...
stereoParams.CameraParameters2.RadialDistortion(3)],1,'B16'); % 相机2畸变向量
最后畸变那两行可能会报错，主要是标定的时候勾选的差异，选第一个就只输出2个，第三个参数是没有的，需要删掉matlab里面那一行。
保存的out.xls结果里面已经将参数进行转置了，可以直接在c++里面使用。
（6）工程化应用
立体匹配和立体校正：
实际使用过程中，存在安装角度不平行等问题，因此需要进行立体校正。
//2.双目的立体标定和立体校正
void ObstacleRecongnition::stereo_check(Mat grayImageL, Mat grayImageR, Mat &rectifyImageL, Mat &rectifyImageR, Size imageSize, Mat &Q, Rect &validROIL, Rect &validROIR) {
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Mat Rl, Rr, Pl, Pr; //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
//左边相机的内参矩阵
Mat cameraMatrixL = (Mat_<double>(3, 3) <<
left_fx, 0, left_cx,
0, left_fy,left_cy,
0, 0, 1);
//左边相机的畸变矩阵
Mat distCoeffL = (Mat_<double>(5, 1) << left_k1,left_k2,left_p1,left_p2,left_k3);
//右边相机的内参矩阵
Mat cameraMatrixR = (Mat_<double>(3, 3) <<
right_fx, 0, right_cx,
0, right_fy, right_cy,
0, 0, 1);
//右边相机的畸变矩阵
Mat distCoeffR = (Mat_<double>(5, 1) << right_k1,right_k2,right_p1,right_p2,right_k3);
//两个相机标定后的平移矩阵
Mat T = (Mat_<double>(3, 1) << double_py_x1,double_py_x2,double_py_x3);//T平移向量
//两个相机标定后的旋转矩阵
Mat rec = (Mat_<double>(3, 3) <<
double_xz_x1, double_xz_x2, double_xz_x3,
double_xz_x4, double_xz_x5, double_xz_x6,
double_xz_x7, double_xz_x8, double_xz_x9); //rec旋转向量
Mat R;//R 旋转矩阵
Rodrigues(rec, R); //Rodrigues变换，获得3*3的旋转矩阵
//立体校正
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);//mapL——输出的X坐标重映射参数
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);//mapR——输出的Y坐标重映射参数
//一幅图像中某位置的像素放置到另一个图片指定位置。
//经过remap之后，左右相机的图像已经共面并且行对准了
remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//最后一个参数是差值方式
remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
//输出的图像需和原图形一样的尺寸和类型
#if TEST_LOG
//把校正结果显示出来
Mat rgbRectifyImageL, rgbRectifyImageR;
cv::cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR); //伪彩色图
cv::cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(rectifyImageL.cols, rectifyImageL.rows);
w = cvRound(rectifyImageL.cols * sf);
h = cvRound(rectifyImageL.rows * 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一样大小
//右图像画到画布上
canvasPart = canvas(Rect(w, 0, w, h)); //获得画布的另一部分
resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
//画上对应的线条
for (int i = 0; i < canvas.rows; i += 16)
line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
imwrite("D:\\桌面文件\\save\\Rectifyimages.jpg", canvas);
waitKey(0);
#endif
}
双视觉算法c++：
//双目视觉算法
void ObstacleRecongnition::binocular_vision(Mat rgbImageL, Mat rgbImageR, Mat &rectifyImageL1,Mat &rectifyImageR1) {
Mat rectifyImageL,rectifyImageR;
Mat grayImageL, grayImageR;
const int imageWidth = rgbImageL.cols;
const int imageHeight = rgbImageL.rows;
Size imageSize = Size(imageWidth, imageHeight);
//1.图像获取
cv::cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
cv::cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
//2.立体校正--图像对齐了-在同一极线上检测左右摄像机图像的相同特征
stereo_check(grayImageL, grayImageR, rectifyImageL, rectifyImageR, imageSize, Q, validROIL, validROIR);
cv::cvtColor(rectifyImageL, rectifyImageL1, CV_GRAY2BGR);
cv::cvtColor(rectifyImageR, rectifyImageR1, CV_GRAY2BGR);
}
4.通过立体匹配算法计算距离
双目视觉的核心就是通过视差计算距离：z = bf/d
其中b表示两个相机之间的距离，f表示相机焦距，d表示视差，d=xr-xl，即左右图像中的x坐标差，d单位是像素pixel。
这个也是目前主流的算法，通过立体匹配，然后生成视差图，最后计算距离，代码很简单，基本上都是opencv封装好的，只需要根据图像修改几个参数。
缺点：速度慢，好的视差图不容易得到。
实现：
void ObstacleRecongnition::stereo_match(Mat rectifyImageL, Mat rectifyImageR, Mat Q, Mat &xyz, Rect validROIL, Rect validROIR,Mat &SC_Map)
{
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, StereoSGBM_width, StereoSGBM_height);
sgbm->setPreFilterCap(PreFilterCap);
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(SpeckleWindowSize);
sgbm->setSpeckleRange(SpeckleRange);
sgbm->setDisp12MaxDiff(Disp12MaxDiff);
sgbm->setMode(StereoSGBM::MODE_SGBM);
Mat disp,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);
//视差图滤波算法
cv::imwrite("D:\\桌面文件\\save\\原始视差图.jpg", disp);
disp.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
Mat color(disp.size(), CV_8UC3);
GenerateFalseMap(disp8, color);//转成彩图
imwrite("D:\\桌面文件\\save\\伪彩色视差图.jpg", color);
waitKey(0);
}
获得视差图以后，可以通过对视差图进行滤波等操作，不做进一步研究，下面可以通过在视差图中随意点几个点，输出的距离是否和该位置的实际距离差不多进行验证。
通过视差图计算距离：
//4.通过视差图计算距离
float ObstacleRecongnition::compute_distance(Mat xyz, float up_x, float up_y, float down_x, float down_y) {
//在视差图中点几个点
Point origin = Point(up_x, up_y);
//通过双目参数将图像坐标进行转换
Vec3f point3 = xyz.at<Vec3f>(origin);
d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2];
d = sqrt(d); //mm
return d;
}
5.通过特征点匹配算法算距离（推荐）
1）使用yolov5对两个相机的图像进行识别，获取对应的目标
2）将定位后的图像进行特征点匹配，最后需要传入定位前原始图像的定位x坐标x_before1和x_before2来计算视差值d。
3）通过公式z=bf/d计算距离。
可参考ORB、SIFT、SURF特征提取算法实现特征点提取。
以SURF为例：
用到了基于GPU加速的opencv编程，要求opencv454是带cuda版本的，输入左右相机的图像，进行特征点匹配
//surf特征匹配算法
float ObstacleRecongnition::surf_demo(Mat rectifyImageL,Mat rectifyImageR,int x_before1,int x_before2) {
cv::cvtColor(rectifyImageL, rectifyImageL, CV_BGR2GRAY);
cv::cvtColor(rectifyImageR, rectifyImageR, CV_BGR2GRAY);
cv::cuda::printShortCudaDeviceInfo(cv::cuda::getDevice());
cuda::SURF_CUDA surf(points_alg_surf);
// detecting keypoints & computing descriptors
cuda::GpuMat keypoints1GPU, keypoints2GPU;
cuda::GpuMat descriptors1GPU, descriptors2GPU;
cuda::GpuMat imageL, imageR;
imageL.upload(rectifyImageL);
imageR.upload(rectifyImageR);
float dis;
try {
surf(imageL, cuda::GpuMat(), keypoints1GPU, descriptors1GPU);
surf(imageR, cuda::GpuMat(), keypoints2GPU, descriptors2GPU);
// matching descriptors
Ptr<cv::cuda::DescriptorMatcher> matcher = cv::cuda::DescriptorMatcher::createBFMatcher(surf.defaultNorm());
vector<DMatch> matches;
matcher->match(descriptors1GPU, descriptors2GPU, matches);
// downloading results
vector<KeyPoint> keypoints1, keypoints2;
vector<float> descriptors1, descriptors2;
surf.downloadKeypoints(keypoints1GPU, keypoints1);
surf.downloadKeypoints(keypoints2GPU, keypoints2);
surf.downloadDescriptors(descriptors1GPU, descriptors1);
surf.downloadDescriptors(descriptors2GPU, descriptors2);
//筛选距离，不同特征点存在匹配错误的情况，可以根据实际情况调整
//calculate the max and min distance between key points
double maxdist = 0; double mindist = 100;
for (int i = 0; i < descriptors1GPU.rows; i++)
{
double dist = matches[i].distance;
if (dist < mindist)mindist = dist;
if (dist > maxdist)maxdist = dist;
}
//cout << "Matching quantity:" << matches.size() << endl;
//select the good match points
vector<DMatch>goodMatches;
for (int i = 0; i < descriptors1GPU.rows; i++)
{
if (matches[i].distance < 2 * mindist)
{
goodMatches.push_back(matches[i]);
}
}
//cout << "Good matching quantity:" << goodMatches.size() << endl;
//calculate parallax
vector<parallax>para;
for (int i = 0; i < goodMatches.size(); i++)
{
parallax temp;
temp.leftX = keypoints1[goodMatches[i].queryIdx].pt.x + x_before1;
temp.rightX = keypoints2[goodMatches[i].queryIdx].pt.x + x_before2;
temp.paraValue = temp.leftX - temp.rightX;
para.push_back(temp);
cout << "No." << i + 1 << ":\t l_X ";
cout << para[i].leftX << "\t r_X " << para[i].rightX;
cout << "\t surf parallax " << para[i].paraValue << endl;
}
//sort(para.begin(), para.end(), ascendPara);
int idxMedian = int(para.size() / 2);
double paraMedian = para[idxMedian].paraValue;
vector<parallax>::iterator it;
for (it = para.begin(); it != para.end(); )
{
if (it->paraValue<((1 - errorRange)*paraMedian) || it->paraValue>((1 + errorRange)*paraMedian))
it = para.erase(it);
else
it++;
}
cout << "Final data..." << endl;
double paraSum = 0;
double paraMean;
for (int i = 0; i < para.size(); i++)
{
paraSum = paraSum + para[i].paraValue;
cout << "No." << i << "\t" << para[i].paraValue << endl;
}
paraMean = paraSum / para.size();
cout << "surf Parallax is " << paraMean << " pixel." << endl;
//z = bf/d
dis = (cameradis * camera_f) / (paraMean);
// drawing the results
Mat img_matches;
drawMatches(Mat(imageL), keypoints1, Mat(imageR), keypoints2, matches, img_matches);
cv::imwrite("D:\\桌面文件\\save\\surf_img.jpg", img_matches);
}
catch (...) {
dis = 100;
}
//cout << "now dis：" << dis << endl;
return dis;
}
6.通过中点算距离
直接对校正后的图像进行识别，然后使用目标检测框的中点位置作为最终的结果，再得到视差，计算距离。
7.参考文献
1.https://blog.csdn.net/hhjoerrrr/article/details/124127143
2.https://blog.csdn.net/qq_40700822/article/details/124251201
3.https://blog.csdn.net/weixin_30072103/article/details/110549798
4.https://blog.csdn.net/Yong_Qi2015/article/details/107704269
5.https://blog.csdn.net/qinchang1/article/details/86934636
6.https://blog.csdn.net/dulingwen/article/details/104142149