双目测距--4 双目立体匹配获取深度图

爱钓鱼的歪猴

已于 2023-05-28 00:13:26 修改

阅读量2.7k

点赞数

分类专栏： OpenCV实战文章标签： opencv 计算机视觉人工智能

于 2023-05-07 00:10:35 首次发布

本文链接：https://blog.csdn.net/weixin_45824067/article/details/130530167

版权

OpenCV实战专栏收录该内容

18 篇文章 6 订阅

订阅专栏

在这之前需要已经完成双目标定，这里是利用双目标定结果利用SGBM算法获取深度图，以及转伪彩图。

畸变矫正、立体矫正 --> 视差图---> 深度图代码

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 = 480;
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博客

爱钓鱼的歪猴

关注

0
点赞
踩
35

收藏

觉得还不错? 一键收藏
2
评论
双目测距--4 双目立体匹配获取深度图

在这之前需要已经完成双目标定，这里是利用双目标定结果利用SGBM算法获取深度图，以及转伪彩图。OpenCV有三种立体匹配算法：BM算法、SGBM算法、GC算法，这里我选择了SGBM算法，其速度精度都还不错。
复制链接

扫一扫