在这之前需要已经完成双目标定，这里是利用双目标定结果利用SGBM算法获取深度图，以及转伪彩图。
目录
StereoSGBM用到的参数：
一、 预处理参数
二 、代价参数
三 、动态规划参数
四、后处理参数
reprojectImageTo3D函数
获取真实距离
联合YOLOv8的思路
畸变矫正、立体矫正 --> 视差图---> 深度图 代码
OpenCV有三种立体匹配算法：BM算法、SGBM算法、GC算法，这里 我选择了SGBM算法，其速度精度都还不错。
StereoSGBM用到的参数：
这些参数的调整是很痛苦 的事情，但是如果你的参数设置不好，深度图呈现的效果就会很不好，会导致没法获取图像物体离相机的距离!!
opencv中SGBM算法的参数含义及数值选取
一、 预处理参数
1：preFilterCap：水平sobel预处理后，映射滤波器大小。默认为15
int ftzero =max(params.preFilterCap, 15) | 1;
opencv测试例程test_stereomatching.cpp中取63。
二 、代价参数
2：SADWindowSize:计算代价步骤中SAD窗口的大小。由源码得，此窗口默认大小为5。
SADWindowSize.width= SADWindowSize.height = params.SADWindowSize > 0 ?params.SADWindowSize : 5;
注：窗口大小应为奇数，一般应在3x3到21x21之间。
3：minDisparity：最小视差，默认为0。此参数决定左图中的像素点在右图匹配搜索的起点。int 类型
4：numberOfDisparities：视差搜索范围，其值必须为16的整数倍（CV_Assert( D % 16 == 0 );）。最大搜索边界= numberOfDisparities+ minDisparity。int 类型
三 、动态规划参数
动态规划有两个参数，分别是P1、P2，它们控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。需要指出，在动态规划时，P1和P2都是常数。
5：opencv测试例程test_stereomatching.cpp中，P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
6：opencv测试例程test_stereomatching.cpp中，P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
四、后处理参数
7：uniquenessRatio：唯一性检测参数。对于左图匹配像素点来说，先定义在numberOfDisparities搜索区间内的最低代价为mincost，次低代价为secdmincost。如果满足
即说明最低代价和次第代价相差太小，也就是匹配的区分度不够，就认为当前匹配像素点是误匹配的。
opencv测试例程test_stereomatching.cpp中，uniquenessRatio=10。int 类型
8：disp12MaxDiff：左右一致性检测最大容许误差阈值。int 类型
opencv测试例程test_stereomatching.cpp中，disp12MaxDiff =1。
9：speckleWindowSize：视差连通区域像素点个数的大小。对于每一个视差点，当其连通区域的像素点个数小于speckleWindowSize时，认为该视差值无效，是噪点。
opencv测试例程test_stereomatching.cpp中，speckleWindowSize=100。
10：speckleRange：视差连通条件，在计算一个视差点的连通区域时，当下一个像素点视差变化绝对值大于speckleRange就认为下一个视差像素点和当前视差像素点是不连通的。
reprojectImageTo3D函数
函数描述：
该函数将视差图，通过投影矩阵Q，得到一幅映射图，图像大小与视差图相同，且每个像素具有三个 通道，分别存储了该像素位置在相机坐标系下的三维点坐标在x,y，z三个轴上的值，即每个像素的在相机坐标系下的三维坐标
函数原型：
void cv::reprojectImageTo3D(
InputArray disparity, //视差图像
OutputArray _3dImage, //映射后存储三维坐标的图像
InputArray Q, //重投影矩阵 通过stereoRectify得到
bool handleMissingValues = false, //计算得到的非正常值是否给值，如果为true则给值10000
int ddepth = -1 //输出类型 -1 即默认为CV_32FC3 还可以是 CV_16S, CV_32S, CV_32F
)
获取真实距离
代码中通过鼠标回调函数 onMouse可以在视差图上左键点击即可获得真实 距离
联合YOLOv8的思路
如果我想把双目相机与YOLOv8结合，检测目标的同时还能返回目标离相机距离。那么输入目标检测网络是某个相机的原图？还是经过畸变矫正及立体矫正之后的某相机图像？
我觉得可能选后者会好点，因为立体矫正对原图有个剪裁的操作（stereRectify()函数的第14的参数我设置为-1）。
所以联合YOLOv8的思路是：
- 输入检测网络的图像是经过畸变矫正及立体矫正之后的左相机图像（也可以是右相机图像）
- 检测到的目标，用其中心点坐标在深度度上面索取像素值，即左相机坐标系下的三维坐标
另外，我也想过直接从某个相机原图上进行预测目标，但是如何让目标中心点映射到深度图上我暂时想明白
畸变矫正、立体矫正 --> 视差图---> 深度图 代码
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/videoio/videoio.hpp>
#include <vector>
#include <iostream>
using namespace cv;
using namespace std;
// 双目摄像机内参
Mat cameraMatrix_L,cameraMatrix_R;
Mat discoeff_L, discoeff_R;
// 双目摄像机外参
Mat R, T;
// 立体校正用
Mat Rl, Rr, Pl, Pr, Q;
Mat mapLx, mapLy, mapRx, mapRy;
Rect validROIL, validROIR;
// 图像
Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;
// 图像尺寸
const int imageWidth = 1280/2;
const int imageheight = 360;
Size imageSize = Size(imageWidth, imageheight);
Mat xyz; //三维坐标
Point origin; //鼠标按下的起始点
Rect selection; //定义矩形选框
bool selectObject = false; //是否选择对象
int blockSize = 0, uniquenessRatio =0, numDisparities=0;
Ptr<StereoBM> bm = StereoBM::create();
// SGBM
/* 参数说明链接：https://blog.csdn.net/wwp2016/article/details/86080722*/
/*Opencv官方文档参数说明写得太模糊了，还得自己上网找解释看*/
int minDisparity; /*最小视差，默认为0。此参数决定左图中的像素点在右图匹配搜索的起点*/
int numDisParitiey; /*视差搜索范围，其值必须为16的整数倍， must be divisible by 16*/
//int blockSize; /* 匹配的块大小*/
int P1; /* 控制视差平滑度的第一个参数，8*number_of_image_channels*blockSize*blockSize */
int P2;/*第二个参数控制视差平滑度，32*number_of_image_channels*blockSize*blockSize*/
int disp12MaxDiff;/* 左右视差检查中允许的最大差异*/
int preFilterCap;/* 水平sobel预处理后，映射滤波器大小。 */
//int uniquenessRatio;/* 唯一性检测参数。 5-15 range is good enough*/
int speckleWindowSize;/* 视差连通区域像素点个数的大小。对于每一个视差点，当其连通区域的像素点个数小于speckleWindowSize时，认为该视差值无效，是噪点, set it somewhere in the 50-200 range. */
int speckRange;/* 视差连通条件，在计算一个视差点的连通区域时，当下一个像素点视差变化绝对值大于speckleRange就认为下一个视差像素点和当前视差像素点是不连通的。，1 or 2 is good enough.*/
int mode=StereoSGBM::MODE_SGBM;/*o StereoSGBM::MODE_HH to run the full-scale two-pass dynamic programming algorithm*/
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create();
// 设置相机内外参数文件路径
void setParameterPath(string path)
{
string parameterPath1 = path + "intrisics.yml";
FileStorage fs(parameterPath1, FileStorage::READ);
if(fs.isOpened())
{
fs["cameraMatrixL"] >> cameraMatrix_L;
fs["cameradistCoeffsL"] >> discoeff_L;
fs["cameraMatrixR"] >> cameraMatrix_R;
fs["cameradistCoeffsR"] >> discoeff_R;
fs.release();
cout<<"*****左右摄像机 内参 已读取"<<endl;
}
else
{
cout << "******" << parameterPath1
<< " can not open" << endl;
}
string parameterPath2 = path + "extrinsics.yml";
fs.open(parameterPath2, FileStorage::READ);
if(fs.isOpened())
{
fs["R"] >> R;
fs["T"] >> T;
fs["Rl"] >> Rl;
fs["Rr"] >> Rr;
fs["Pl"] >> Pl;
fs["Pr"] >> Pr;
fs["Q"] >> Q;
fs["mapLx"] >> mapLx;
fs["mapLy"] >> mapLy;
fs["mapRx"] >> mapRx;
fs["mapRy"] >> mapRy;
fs.release();
cout<<"*****左右摄像机 外参 已读取"<<endl<<endl<<endl;
}
else
{
cout << "******" << parameterPath2
<< " can not open" << endl;
}
}
// 视差图孔洞填充
void insertDepth32f(cv::Mat& depth)
{
/*https://blog.csdn.net/qq_41641044/article/details/120693644*/
for (int i = 0; i < depth.rows; i++)
{
for (int j = 0; j < depth.cols; j++)
{
if (depth.channels() == 1)
{
//图像数组是逐行逐列顺序排列的，也就是第一行，全列，第二行全列的走
int a,b,indexs = i * depth.cols + j;
//cout << typeid(depth.data[indexs]).name() << endl;
a = (int)depth.data[indexs];
//cout << a << endl;
if (a == 0 && j > 50 && j< depth.cols-50 && i>50 && i< depth.rows-50)
{
//vector<int> testarray = { (int)depth.data[indexs - 50], (int)depth.data[indexs + 50], (int)depth.data[indexs - 50 * depth.cols], (int)depth.data[indexs + 50 * depth.cols] };
b = max({ (int)depth.data[indexs - 50], (int)depth.data[indexs + 50], (int)depth.data[indexs - 50* depth.cols], (int)depth.data[indexs + 50 * depth.cols] });//accumulate(testarray.begin(),testarray.end(),0)/4;
//cout << "a的值为："<<a<<"b的值为："<<b << endl;
depth.data[indexs] = b;
}
}
else if (depth.channels() == 3)
{
}
}
}
}
// 深度图转伪彩
void convertColor(Mat & image)
{
/*https://blog.csdn.net/qq_51639530/article/details/124462762*/
//读取16位深度图（像素范围0～65535），并将其转化为8位（像素范围0～255）
double minValue, maxValue; // 最大值，最小值
cv::Point minIdx, maxIdx; // 最小值坐标，最大值坐标
cv::minMaxLoc(image, &minValue, &maxValue, &minIdx, &maxIdx);
image -= minValue;
// image = image / (maxValue - minValue);
image =image/((numDisparities * 16 + 16)*16.);
image *= 255;
//使得越近的地方深度值越大，越远的地方深度值越小，以达到伪彩色图近蓝远红的目的。
image = 255 - image;
// cv2 中的色度图有十几种，其中最常用的是 cv2.COLORMAP_JET，蓝色表示较高的深度值，红色表示较低的深度值。
// cv.convertScaleAbs() 函数中的 alpha 的大小与深度图中的有效距离有关，如果像我一样默认深度图中的所有深度值都在有效距离内，并已经手动将16位深度转化为了8位深度，则 alpha 可以设为1。
convertScaleAbs(image,image,1);
applyColorMap(image,image, COLORMAP_JET);
}
/*****立体匹配*****/
void stereo_match(int,void*)
{
bm->setBlockSize(2*blockSize+5); //SAD窗口大小，5~21之间为宜
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0); //最小视差，默认值为0, 可以是负值，int型
bm->setNumDisparities(numDisparities*16+16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);//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.));//计算出的视差是CV_16S格式
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
imshow("disparity", disp8);
}
void Stereo_SGBM_match(int,void*)
{
P1 = 8 * grayImageL.channels() * blockSize*blockSize;
P2 = 4*P1;
sgbm->setP1(P1);
sgbm->setP2(P2);
sgbm->setMinDisparity(0);//此参数决定左图中的像素点在右图匹配搜索的起点
sgbm->setSpeckleRange(32);
// 下面6个参数用按钮来调节
sgbm->setNumDisparities(numDisparities*16+16); // 必须能被16整除； 视差搜索范围
sgbm->setBlockSize(blockSize); // > = 1的奇数
sgbm->setDisp12MaxDiff(1); //左右一致性检测最大容许误差阈值
sgbm->setPreFilterCap(9);
sgbm->setUniquenessRatio(uniquenessRatio); // 5~15
sgbm->setSpeckleWindowSize(100); // 50～200
Mat disp, disp8;
// 对左右视图的左边进行边界延拓，以获取与原始视图相同大小的有效视差区域
/* 加大numDisparities值，深度图左边会变黑，但是整图尺寸变大！很有效哦*/
/*https://blog.csdn.net/u011574296/article/details/87546622*/
copyMakeBorder(rectifyImageL,rectifyImageL,0,0,numDisparities,0,cv::BORDER_REPLICATE);
copyMakeBorder(rectifyImageR,rectifyImageR,0,0,numDisparities,0,cv::BORDER_REPLICATE);
sgbm->compute(rectifyImageL, rectifyImageR, disp); // //得出的结果是以16位符号数的形式的存储的，出于精度需要，所有的视差在输出时都扩大了16倍(2^4)
disp = disp.colRange(numDisparities, rectifyImageL.cols).clone();
// insertDepth32f(disp); 没有用,不能呢消除空洞
// 可以试一试闭合运算或者开启运算
// cv::Mat element(7, 7, disp.type(), cv::Scalar(1));
// morphologyEx(disp, disp, MORPH_CLOSE, element);
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式，
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
imshow("disparity", disp8);
applyColorMap(disp8, disp8, COLORMAP_RAINBOW);
imshow("color", 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()
{
/* 读取相机内外参数*/
setParameterPath("/home/jason/work/my--camera/calibration/");
/*立体校正*/
stereoRectify(cameraMatrix_L, discoeff_L,
cameraMatrix_R, discoeff_R,
imageSize, R, T,
Rl, Rr, Pl, Pr, Q,
CALIB_ZERO_DISPARITY, -1,
imageSize,
&validROIL, &validROIR);
initUndistortRectifyMap(cameraMatrix_L, discoeff_L,
Rl, Pl, imageSize,
CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrix_R, discoeff_R,
Rr, Pr, imageSize,
CV_32FC1, mapRx, mapRy);
/*读取图片*/
VideoCapture cap(2);
if (!cap.isOpened())
{
cout << "***** camera can't open!!"<< endl <<endl;
return 1;
}
else
{
cout << "*****camera is open"<< endl;
}
cap.set(CAP_PROP_FRAME_WIDTH, imageWidth*2);
cap.set(CAP_PROP_FRAME_HEIGHT, imageheight);
cap.set(CAP_PROP_FPS, 30);
namedWindow("disparity", cv::WINDOW_AUTOSIZE);
// /*BM算法用*/
// // 创建SAD窗口 Trackbar
// createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);
// // 创建视差唯一性百分比窗口 Trackbar
// createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);
// // 创建视差窗口 Trackbar
// createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
/*SGBM算法用*/
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, Stereo_SGBM_match);
createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, Stereo_SGBM_match);
// createTrackbar("disp12MaxDiff:\n", "disparity", &disp12MaxDiff, -1, Stereo_SGBM_match);
// createTrackbar("preFilterCap:\n", "disparity", &preFilterCap, 50, Stereo_SGBM_match);
createTrackbar("uniquenessRatio:\n", "disparity", &uniquenessRatio, 50, Stereo_SGBM_match);
// createTrackbar("speckleWindowSize:\n", "disparity", &speckleWindowSize, 200, Stereo_SGBM_match);
//鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
setMouseCallback("disparity", onMouse, 0);
Mat doubleImage;
while (1)
{
cap >> doubleImage;
rgbImageL = doubleImage(Rect(Point(0,0), Point(imageWidth, imageheight))).clone();
rgbImageR = doubleImage(Rect(Point(imageWidth, 0), Point(imageWidth*2, imageheight))).clone();
cvtColor(rgbImageL, grayImageL, COLOR_BGR2GRAY);
cvtColor(rgbImageR, grayImageR, COLOR_BGR2GRAY);
imshow("before Rectify", doubleImage);
/*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);
//显示在同一张图上
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);
/*
获得深度
*/
// stereo_match(0,0);
Stereo_SGBM_match(0,0);
if ((char)waitKey(1) == 'q')
{
break;
}
}
return 0;
}其实做这个项目目前有一个一直未解决的问题，即深度图中空洞问题，如果您有解决办法，烦请给个机会交流一下
参考：
sgbm参数设置_wwp2016的博客-CSDN博客
视觉深度图后处理中空洞填充问题_深度图空洞填充_乘风破浪的混子的博客-CSDN博客
关于深度图/视差图转为伪彩色的方法_深度图转伪彩图_via无心的博客-CSDN博客双目立体匹配方法：BM、SGBM、GC算法的实现及性能对比(附代码)_bm立体匹配_slam02∞的博客-CSDN博客
双目立体匹配算法SGBM_sgbm算法_Alan Lan的博客-CSDN博客
【OpenCV】reprojectImageTo3D函数_魏Gordon的博客-CSDN博客
【OpenCV】reprojectImageTo3D函数_魏Gordon的博客-CSDN博客
