opencv雙目測距(BM 與SGBM匹配）

１、引言
在一年之前小編寫了一篇雙目測距的博文，引入了大量的童鞋閱讀，其博文介紹了詳細的相機標定與雙目測距過程和代碼

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

文章評論特別多，由此可見很多讀者遇到了很多的問題，有標定不準的，測距距離不準，誤差特別大，視差圖很差的等各種問題。今天小編再寫一篇博文，可能對您有所幫助。

雙目測距流程如圖

２　標定
首先準備一張標定板，標定板要比較大，不能反光等特性，小博的標定板如圖
接下來拍照，大約二十張，就可以了，我重新修改了下拍照程序

#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);

}

４、視差圖測距
我們先看下效果圖，效果蠻不錯，誤差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;
}

若測得距離爲負數，可將Ｔ向量的三個值變換個符號即可，OK ,先講這麼多，覺得不錯的點個贊哦。

opencv雙目測距(BM 與SGBM匹配）

多目標追蹤近幾年論文及代碼

多激光雷達與攝像頭的融合算法（一）

Umat Mat 相互轉換(apexcv學習心得一）

ros opencv 之三(雙目視覺圖像發佈與接收)

自動泊車（之三）車位線定位（視覺定位）

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結