opencv双目测距(BM 与SGBM匹配)

1、引言
在一年之前小编写了一篇双目测距的博文,引入了大量的童鞋阅读,其博文介绍了详细的相机标定与双目测距过程和代码

https://blog.csdn.net/xiao__run/article/details/78900652

摄像头如前面文章所示,大家可自行购买,小编就是在这家购买

https://shop224405513.taobao.com/search.htm?spm=a1z10.1-c-s.0.0.751b3e49u0Kz6o&search=y

文章评论特别多,由此可见很多读者遇到了很多的问题,有标定不准的,测距距离不准,误差特别大,视差图很差的等各种问题。今天小编再写一篇博文,可能对您有所帮助。

双目测距流程如图

在这里插入图片描述

2 标定
首先准备一张标定板,标定板要比较大,不能反光等特性,小博的标定板如图
在这里插入图片描述接下来拍照,大约二十张,就可以了,我重新修改了下拍照程序

#include <iostream>
#include <opencv2/opencv.hpp>
 
using namespace std;
using namespace cv;
 
 
int main()
{

/*
    //双目摄像头,两个usb
    cv::VideoCapture capl(0);
    cv::VideoCapture capr(1);
    int i = 0;
 
    cv::Mat cam_left;
    cv::Mat cam_right;
 
    char filename_l[15];
    char filename_r[15];
    while(capl.read(cam_left) && capr.read(cam_right))
    {
        cv::imshow("cam_left", cam_left);
        cv::imshow("cam_right", cam_right);
	
        char c = cv::waitKey(1);
        char s[40];
        
        if(c==' ') //按空格采集图像
        {
	        sprintf(filename_l, "/home/lqx/ClionProjects/Calibration/left_img/left%d.jpg",i);
            imwrite(filename_l, cam_left);
	        sprintf(filename_r, "/home/lqx/ClionProjects/Calibration/right_img/right%d.jpg",i++);
            imwrite(filename_r, cam_right);
            //printf(s, "%s%d%s\n", "the ",i++,"th image");
            cout << "save the "<< i <<"th image\n"<< endl;
        }
        if(c=='q' || c=='Q') // 按q退出
        {
            break;
        }
    }
 
    return 0;
 */

    //双目摄像头,一个usb
    //图像大小为640*240,左右各为320*240
    cv::VideoCapture cap(0);
    int i = 0;
    cap.set(CV_CAP_PROP_FRAME_WIDTH,640);//可以

    cap.set(CV_CAP_PROP_FRAME_HEIGHT,240);
    cv::Mat cam, cam_left,cam_right;
    char filename_l[15];
    char filename_r[15];
    while(cap.read(cam) )
    {

        cam_right=cam(Rect(0,0,320,240));
        cam_left=cam(Rect(320,0,320,240));

        cv::imshow("cam_left", cam_left);
        cv::imshow("cam_right", cam_right);
        char c = cv::waitKey(1);
        char s[40];

        if(c==' ') //按空格采集图像
        {
            sprintf(filename_l, "left_img/left%d.jpg",i);
            imwrite(filename_l, cam_left);
            sprintf(filename_r, "right_img/right%d.jpg",i++);
            imwrite(filename_r, cam_right);
            //printf(s, "%s%d%s\n", "the ",i++,"th image");
            cout << "save the "<< i <<"th image\n"<< endl;
        }
        if(c=='q' || c=='Q') // 按q退出
        {
            break;
        }
    }

    return 0;
}

调用上述代码即可得到左右二十多张图像。

接下来我们使用matlab工具箱的标定工具进行标定,也可以使用我上面的博文进行标定。我建议使用matlab标定,结果会更准确点。
标定得到内参和外参如下:

//T 矩阵参数
    Mat T = Mat::zeros(3,1,CV_64F);
    T.at<double>(0,0)= 119.5815;
    T.at<double>(1,0)=-0.5328;
    T.at<double>(2,0)=-1.7296;

//R矩阵
   // Rodrigues(rec,R);
   // cout<<R<<endl;
    Mat R=Mat::eye(3,3,CV_64F);
    R.at<double>(0,1)= 0.000055498;
    R.at<double>(0,2)= -0.0184;
    R.at<double>(1,0)= -0.0001629;
    R.at<double>(1,2)= 0.0118;
    R.at<double>(2,0)= 0.0184;
    R.at<double>(2,1)= -0.0118;


    //*******************左相机参数**********************************
    //*******************左相机参数**********************************
    double fx=202.0386,fy=202.6377;
    double cx=156.0528,cy=116.9724;

    Size size(320,240);

    Rect left(0,0,320,240);
    Rect right(320,0,320,240);
    Mat cameraMatrixL=Mat ::eye(3,3,CV_64F);
    Mat cameraMatrixR=Mat ::eye(3,3,CV_64F);
   //左边相机内参 
    cameraMatrixL.at<double>(0,0)=fx;
    cameraMatrixL.at<double>(0,2)=cx;
    cameraMatrixL.at<double>(1,1)=fy;
    cameraMatrixL.at<double>(1,2)=cy;
   //左边相机畸变
 Mat distCoffsL=Mat::zeros(5,1,CV_64F);
    distCoffsL.at<double>(0,0)=-0.0446;
    distCoffsL.at<double>(1,0)=0.0597;
    distCoffsL.at<double>(2,0)=0;
    distCoffsL.at<double>(3,0)=0.;
    distCoffsL.at<double>(4,0)=0;

    //*******************右相机参数**********************************
    cameraMatrixR.at<double>(0,0)=204.1203;
    cameraMatrixR.at<double>(0,2)=164.9597;
    cameraMatrixR.at<double>(1,1)=204.5797;
    cameraMatrixR.at<double>(1,2)=117.9534;
    Mat distCoffsR=Mat::zeros(5,1,CV_64F);

    distCoffsR.at<double>(0,0)=-0.0352;
    distCoffsR.at<double>(1,0)=0.0345;
    distCoffsR.at<double>(2,0)=0.;
    distCoffsR.at<double>(3,0)=0;
    distCoffsR.at<double>(4,0)=0;

3、 BM与SGBM匹配算法
SGBM是一种立体匹配算法,准确度和速度适中,工程中比较常用

oid  stereo_SGBM_match(int, void*)
{
	int mindisparity = 0;
	int ndisparities = 64;  
	int SADWindowSize = 11; 
	cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);

	int P1 = 8 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
	int P2 = 32 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
	sgbm->setP1(P1);
	sgbm->setP2(P2);
	sgbm->setPreFilterCap(15);
	sgbm->setUniquenessRatio(6);
	sgbm->setSpeckleRange(2);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setDisp12MaxDiff(1);
        //sgbm->setNumDisparities(1);
	sgbm->setMode(cv::StereoSGBM::MODE_HH);
        Mat disp,disp8U;
	sgbm->compute(left_camera_calibration, right_camrera_calibration, disp);
	disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
	disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
	//normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
	disp.convertTo(disp8U,CV_8U,255/(numDisparities*16.));
	reprojectImageTo3D(disp,xyz,Q);
        xyz=xyz*16;
	imshow("disparity",disp8U);

}

BM算法之前博文已经提过

oid  stereo_match(int, void*)
{
    bm->setBlockSize(2*blockSize+5);
    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(uniquenessRation);
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp,disp8;
    bm->compute(left_camera_calibration,right_camrera_calibration,disp);
    disp.convertTo(disp8,CV_8U,255/((numDisparities*16+16)*16.));
    reprojectImageTo3D(disp,xyz,Q);
    xyz=xyz*16;
    imshow("disparity",disp8);

}

4、视差图测距
我们先看下效果图,效果蛮不错,误差1m以内大约1cm以内;
在这里插入图片描述在这里插入图片描述在这里插入图片描述可以看到本文的测距方法还是挺准,视差图效果也不错。

最后给出部分工程代码吧,此代码未经我优化,仅仅作为学习使用,其实经优化,在树莓派等设备上也能实时生成视差图测距。

#include <iostream>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;


int framewidth=320;
int frameheight=240;
Size imageSize=Size(framewidth,frameheight);
Mat left_camera,right_camera,left_camera_calibration,right_camrera_calibration,xyz;
Point origin;
Rect selection;
bool selectObject= false;
int blockSize=0,uniquenessRation=0,numDisparities=0;
Rect validROIL,validROIR;

Mat view,rview,mapL1,map_L2,mapR1,mapR2,RL,PL,RR,PR,Q;
Ptr<StereoBM>bm=StereoBM::create(16,9);
int mindisparity = 0;
int ndisparities = 64;  
int SADWindowSize = 11; 
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
//*******************bm**********************************
//*******************bm**********************************

/*
void  stereo_match(int, void*)
{
    bm->setBlockSize(2*blockSize+5);
    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(uniquenessRation);
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp,disp8;
    bm->compute(left_camera_calibration,right_camrera_calibration,disp);
    disp.convertTo(disp8,CV_8U,255/((numDisparities*16+16)*16.));
    reprojectImageTo3D(disp,xyz,Q);
    xyz=xyz*16;
    imshow("disparity",disp8);

}

*/
//*******************bm**********************************
//*******************bm**********************************

void  stereo_SGBM_match(int, void*)
{

	int P1 = 8 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
	int P2 = 32 * left_camera_calibration.channels() * SADWindowSize* SADWindowSize;
	sgbm->setP1(P1);
	sgbm->setP2(P2);
	sgbm->setPreFilterCap(15);
	sgbm->setUniquenessRatio(6);
	sgbm->setSpeckleRange(2);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setDisp12MaxDiff(1);
        //sgbm->setNumDisparities(1);
	sgbm->setMode(cv::StereoSGBM::MODE_HH);
        Mat disp,disp8U;
	sgbm->compute(left_camera_calibration, right_camrera_calibration, disp);
	disp.convertTo(disp, CV_32F, 1.0 / 16);                //除以16得到真实视差值
	disp8U = Mat(disp.rows, disp.cols, CV_8UC1);       //显示
	//normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);
	disp.convertTo(disp8U,CV_8U,255/(numDisparities*16.));
	reprojectImageTo3D(disp,xyz,Q);
        xyz=xyz*16;
	imshow("disparity",disp8U);

}



static void onMouse(int envent,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 (envent)
    {
        case EVENT_LBUTTONDOWN:
            origin=Point(x,y);
            selection=Rect(x,y,0,0);
            selectObject= true;
            cout<<origin<<"in the world coordinate is: "<<xyz.at<Vec3f>(origin)<<endl;
            break;
        case EVENT_LBUTTONUP:
            selectObject= false;
            if(selection.width>0 && selection.height>0)
            break;

    }
}



int main()
{
//T 矩阵参数
    Mat T = Mat::zeros(3,1,CV_64F);
    T.at<double>(0,0)= 119.5815;
    T.at<double>(1,0)=-0.5328;
    T.at<double>(2,0)=-1.7296;


    //Mat rec = Mat::zeros(3,1,CV_64F);
    //rec.at<double>(0,0)= 0.0191;
   // rec.at<double>(1,0)=0.03125;
    //rec.at<double>(2,0)=-0.00960;


    //Mat R;

//R矩阵
   // Rodrigues(rec,R);
   // cout<<R<<endl;
    Mat R=Mat::eye(3,3,CV_64F);
    R.at<double>(0,1)= 0.000055498;
    R.at<double>(0,2)= -0.0184;
    R.at<double>(1,0)= -0.0001629;
    R.at<double>(1,2)= 0.0118;
    R.at<double>(2,0)= 0.0184;
    R.at<double>(2,1)= -0.0118;


    //*******************左相机参数**********************************
    //*******************左相机参数**********************************
    double fx=202.0386,fy=202.6377;
    double cx=156.0528,cy=116.9724;

    Size size(320,240);

    Rect left(0,0,320,240);
    Rect right(320,0,320,240);
    Mat cameraMatrixL=Mat ::eye(3,3,CV_64F);
    Mat cameraMatrixR=Mat ::eye(3,3,CV_64F);
   //左边相机内参 
    cameraMatrixL.at<double>(0,0)=fx;
    cameraMatrixL.at<double>(0,2)=cx;
    cameraMatrixL.at<double>(1,1)=fy;
    cameraMatrixL.at<double>(1,2)=cy;
   //左边相机畸变
 Mat distCoffsL=Mat::zeros(5,1,CV_64F);
    distCoffsL.at<double>(0,0)=-0.0446;
    distCoffsL.at<double>(1,0)=0.0597;
    distCoffsL.at<double>(2,0)=0;
    distCoffsL.at<double>(3,0)=0.;
    distCoffsL.at<double>(4,0)=0;
    //*******************做相机参数**********************************
    //*******************右相机参数**********************************
    cameraMatrixR.at<double>(0,0)=204.1203;
    cameraMatrixR.at<double>(0,2)=164.9597;
    cameraMatrixR.at<double>(1,1)=204.5797;
    cameraMatrixR.at<double>(1,2)=117.9534;
    Mat distCoffsR=Mat::zeros(5,1,CV_64F);

    distCoffsR.at<double>(0,0)=-0.0352;
    distCoffsR.at<double>(1,0)=0.0345;
    distCoffsR.at<double>(2,0)=0.;
    distCoffsR.at<double>(3,0)=0;
    distCoffsR.at<double>(4,0)=0;
    //*******************有相机参数**********************************
    //*******************有相机参数**********************************


    cout<<"calibration ......."<<endl;
    //Mat new_cameraMatrix_L=getOptimalNewCameraMatrix(cameraMatrix1, distCoeffs1, image_size, 1, image_size, 0);

    stereoRectify(cameraMatrixL,distCoffsL,cameraMatrixR,distCoffsR,size,R,T,RL,RR,PL,PR,Q,CALIB_ZERO_DISPARITY,0,size,&validROIL,&validROIR);

    initUndistortRectifyMap(cameraMatrixL,distCoffsL,RL,PL ,size,CV_16SC2,mapL1,map_L2);

    initUndistortRectifyMap(cameraMatrixR,distCoffsR,RR,PR ,size,CV_16SC2,mapR1,mapR2);
    VideoCapture cap (0);
    Mat rectifyL,rectifyR;
    cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 320);

    while (cap.isOpened())
    {
        Mat frame,grayL,grayR;
        cap>>frame;
        left_camera=frame(left);
        right_camera=frame(right);
        imshow("left_org",left_camera);
        imshow("right_org",right_camera);
        imshow("org",frame);

        cvtColor(left_camera,grayL,CV_RGB2GRAY);
        cvtColor(right_camera,grayR,CV_RGB2GRAY);

        remap(grayL,left_camera_calibration,mapL1,map_L2,INTER_LINEAR);
        remap(grayR,right_camrera_calibration,mapR1,mapR2,INTER_LINEAR);
/*
        Mat canvas;
        double sf;
        int w ,h;
        sf=240. /320;
        w=cvRound(240*sf);
        h=cvRound(320*sf);
        canvas.create(h,w*2,CV_8UC3);

        Mat canvasPart=canvas(Rect(0,0,w,h));
        resize(left_camera_calibration,canvasPart,canvasPart.size(),0,0,INTER_AREA);
        Rect vroiL(cvRound(validROIL.x*sf),cvRound(validROIL.y*sf),cvRound(validROIL.width*sf),cvRound(validROIL.height*sf));

        canvasPart=canvas(Rect(w,0,w,h));
        resize(right_camrera_calibration,canvasPart,canvasPart.size(),0,0,INTER_AREA);
        Rect vroiR(cvRound(validROIR.x*sf),cvRound(validROIR.y*sf),cvRound(validROIR.width*sf),cvRound(validROIR.height*sf));

        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("rectify",canvas);
*/
        namedWindow("disparity",CV_WINDOW_AUTOSIZE);

//        createTrackbar("blocksize:\n","disparity",&blockSize,16,stereo_match);

//        createTrackbar("UniquenessRatio:\n","disparity",&uniquenessRation,50,stereo_match);

        createTrackbar("NumDisparities:\n","disparity",&numDisparities,16,stereo_SGBM_match);

        setMouseCallback("disparity",onMouse,0);

//        stereo_match(0,0);
	
	stereo_SGBM_match(0, 0);
	// Mat output;
	//sgbm->compute(left_camera_calibration,right_camrera_calibration,output);
	//imshow("SGBM",output);
        //cvtColor(left_camera_calibration,left_camera_calibration,CV_GRAY2BGR);

       // cvtColor(right_camrera_calibration,right_camrera_calibration,CV_GRAY2BGR);

        imshow("cali_right",right_camrera_calibration);

        imshow("cali_left",left_camera_calibration);

        int key=waitKey(1);
        if (key==27)
        {
            break;
        }




    }


    return 0;
}

若测得距离为负数,可将T向量的三个值变换个符号即可,OK ,先讲这么多,觉得不错的点个赞哦。

评论 18
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

xiao__run

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值