VS2017+OpenCV3.3基于SGBM算法的双目立体视觉、双目测距（双目校正和立体匹配）...

mediatv 2021-12-14

展开全文

前些日子做了一个关于双目立体视觉的入门作业，现在在这里总结一下学到的一些知识（写的可能会有很多欠缺的地方，还望海涵！）
本篇博客不涉及双目标定的知识，关于双目标定网上资料很多，大家可以自行查找学习。
先说一下本博客的双目立体视觉的实现基础，已知以下信息：
（1）双目采集图像分辨率为1920X1024；

（2）双目相机相对于虚拟焦平面的外参及各自内参如下：

 左视相机：

 内参：

 像元大小 = 5.86微米；
 焦距     = [ 4334.09568   4334.09568 ]（像素）;
 主点坐标 = [ 959.50000   511.50000 ]  （像素）;

 相对于虚拟焦平面的外参：

 旋转向量 = [ 0.04345   -0.05236  -0.01810 ];
 平移向量 = [ -518.97666   01.20629  9.14632 ](毫米）

 右视相机：

 内参：

 像元大小 = 5.86微米；
 焦距     = [ 4489.55770   4507.13412 ]（像素）;
 主点坐标 = [ 801.86552   530.72579 ]  （像素）;

 相对于虚拟焦平面的外参：

 旋转向量 = [ 0.04345   -0.05236  -0.01810 ];
 平移向量 = [ 518.97666   -01.20629  -9.14632 ](毫米）

注：不考虑畸变校正；

通过以上信息，首先确定解决问题的技术思路如下图：
这里写图片描述
由上图可以看到在已知内外参数的前提下，只需要读取图片直接进行校正，接下来介绍关于双目校正的知识。
双目校正
要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。校正的目的是对左右两幅图像平面重投影，使得它们精确落在同一个平面上，而且图像的行完全地对准到前向平行的结构上。
题目已经给出了旋转矩阵Ｒ和平移向量 T，立体校正Bouguet 算法能简单地使左右图像中的每幅重投影次数最小且重投影畸变最大，所以使立体匹配更加准确和快速，并使左右图像的观测面积最大。
通过投影矩阵P把三维点变换成可以在平面上显示的二维点
[■(x@y@w)]=P[■(X@Y@■(Z@1))] (1) （1）
投影平面上的点坐标为(x /w，y /w) 。同理，二维点也可通过重投影矩阵Q 重投影为三维点
（2）（3）
其中 (c_x,c_y ) 为主点在左图像上的坐标，f 为焦距， T_x 为双目间距， c_x^' 为主点在右图像的x坐标。根据式(2)得到三维坐标为: (X /W，Y/W，Z /W) 。在 OpenCV中可通过stereoRectify() 函数完成以上校正功能，该函数输入参数为摄像机矩阵，畸变向量，左右旋转矩阵Ｒ和平移向量T。输出参数有式 (1)中投影矩阵P，分别为 P_left 和 P_right ，以及重投影矩阵Q。可调用函数InitUndistortRectifyMap( ) 生成图像校正所需的映射矩阵。最后经过remap（）函数，使左右相机的图像共面并且行对准。效果如下图：
这里写图片描述

立体匹配与生成深度信息
立体匹配完成匹配左右摄像机视图的相同特征，并得到视差图，视差值是匹配时相同特征点在x坐标轴上的差值 x_l-x_r 。得到视差图后可通过三角相似的原理得到目标物体的距离。
2.2.1立体成像原理
假设摄像机没有畸变，左右摄像机的成像平面已经严格对准，左右主点已经校准，主光线也是平行的。理想立体摄像机模型如下图:
这里写图片描述
设两个摄像机分别移动到世界坐标系的原点，可分别得到各自独立的W点相对像平面点的X和Y坐标式：

由式(6) 易知视差d和距离Z成反比，当视差很小时，视差的变化对距离Z的影响较大; 当视差较大时，视差的变化对距离Z的影响较小，因此，测距系统仅当距离较近时精度较高。

立体匹配算法介绍
SGBM算法介绍
在OpenCV中使用函数StereoSGBM ( ) 实现了SGBM算法。SGBM 算法核心步骤为：选取匹配基元；构建基于多个方向的扫描线的代价能量和函数；求取能量代价和函数的最优解。OpenCV中SGMB算法的实现主要分为以下四个步骤：
①预处理
SGBM采用水平Sobel算子，把图像做处理，然后用一个函数将经过水平Sobel算子处理后的图像上每个像素点（P表示其像素值）映射成一个新的图像， P_NEW 表示新图像上的像素值。映射函数如下：
这里写图片描述
preFilterCap为一个常数参数，openCv默认取15。预处理实际上是得到图像的梯度信息。经预处理的图像保存起来，将会用于计算代价。
②代价计算
代价有两部分组成：经过预处理得到的图像的梯度信息经过基于采样的方法得到的梯度代价；原图像经过基于采样的方法得到的SAD代价。
③动态规划
用一维约束近似二维约束。在P的周围，以 45°为间隔设置了8个路径。通过8个路径计算最小代价路径 L_r (p,d) ，以此来近似二维约束匹配计算.
其中动态规划很重要两个参数P1，P2是这样设定的：
P1 =8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
cn是图像的通道数, SADWindowSize是SAD窗口大小，数值为奇数。可以看出，当图像通道和SAD窗口确定下来，SGBM的规划参数P1和P2是常数。
④后处理
openCvSGBM的后处理包含以下几个步骤：
Step1：唯一性检测：视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为0。其中uniquenessRatio是一个常数参数。
Step2：亚像素插值
Step3：左右一致性检测：误差阈值disp12MaxDiff默认为1，可以自己设置。

获得深度信息
经过sgbm->compute(rectifyImageL, rectifyImageR, disp)获得视差映射后，利用式(2)，式(3) ，通过简单的矩阵相乘就可提取深度信息。三维坐标就是( X /W，Y/W，Z /W) 。OpenCV中使用reprojectImageTo3D( )函数实现该功能，该函数输入上面得到的视差数据，输出所需的三维点阵，然后提取深度信息。

SGBM参数设置：（在下面的程序中已经标明）
MinDisparity设置为0，因为两个摄像头是前向平行放置，相同的物体在左图中一定比在右图中偏右。如果为了追求更大的双目重合区域而将两个摄像头向内偏转的话，这个参数是需要考虑的。
UniquenessRatio主要可以防止误匹配，此参数对于最后的匹配结果是有很大的影响。立体匹配中，宁愿区域无法匹配，也不要误匹配。如果有误匹配的话，碰到障碍检测这种应用，就会很麻烦。该参数不能为负值，一般5-15左右的值比较合适，int型。
BlockSize：SAD窗口大小，容许范围是[5,255]，一般应该在 5x5..21x21 之间，参数必须为奇数值, int型。
NumDisparities：视差窗口，即最大视差值与最小视差值之差,窗口大小必须是 16的整数倍，int型。
在SGBM算法的参数中，对视差生成效果影响较大的主要参数是BlockSize、NumDisparities和UniquenessRatio三个，一般只需对这三个参数进行调整，其余参数按默认设置即可。
具体实现代码如下：（SGBM算法匹配效果较好，但是时间较长，程序运行时请耐心等待！）

#include <opencv2/opencv.hpp>  
#include <iostream>  
#include <stdio.h>

using namespace std;
using namespace cv;

const int imageWidth = 1920;                             //摄像头的分辨率  
const int imageHeight = 1024;
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域  
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表  
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
Mat xyz;              //三维坐标

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象


Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);

/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 4334.09568, 0, 959.50000,
    0, 4334.09568, 511.50000,
    0, 0, 1.0);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.0, 0.0,0.0, 0.0, 0.0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 4489.55770, 0, 801.86552,
    0, 4507.13412, 530.72579,
    0, 0, 1.0);
Mat distCoeffR = (Mat_<double>(5, 1) << 0.0, 0.0, 0.0, 0.0, 0.0);

Mat T = (Mat_<double>(3, 1) << -518.97666, 01.20629,9.14632);//T平移向量
Mat rec = (Mat_<double>(3, 1) <<0.04345, -0.05236, -0.01810);//rec旋转向量
Mat R;//R 旋转矩阵


static void saveXYZ(const char* filename, const Mat& mat)
{
    const double max_z = 16.0e4;
    FILE* fp = fopen(filename, "wt");
    printf("%d %d \n", mat.rows, mat.cols);
    for (int y = 0; y < mat.rows; y++)
    {
        for (int x = 0; x < mat.cols; x++)
        {
            Vec3f point = mat.at<Vec3f>(y, x);
            if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);

        }
    }
    fclose(fp);
}

/*给深度图上色*/
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
    // color map  
    float max_val = 255.0f;
    float map[8][4] = { { 0,0,0,114 },{ 0,0,1,185 },{ 1,0,0,114 },{ 1,0,1,174 },
    { 0,1,0,114 },{ 0,1,1,185 },{ 1,1,0,114 },{ 1,1,1,0 } };
    float sum = 0;
    for (int i = 0; i<8; i++)
        sum += map[i][3];

    float weights[8]; // relative   weights  
    float cumsum[8];  // cumulative weights  
    cumsum[0] = 0;
    for (int i = 0; i<7; i++) {
        weights[i] = sum / map[i][3];
        cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
    }

    int height_ = src.rows;
    int width_ = src.cols;
    // for all pixels do  
    for (int v = 0; v<height_; v++) {
        for (int u = 0; u<width_; u++) {

            // get normalized value  
            float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

            // find bin  
            int i;
            for (i = 0; i<7; i++)
                if (val<cumsum[i + 1])
                    break;

            // compute red/green/blue values  
            float   w = 1.0 - (val - cumsum[i])*weights[i];
            uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
            uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
            uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
            //rgb内存连续存放  
            disp.data[v*width_ * 3 + 3 * u + 0] = b;
            disp.data[v*width_ * 3 + 3 * u + 1] = g;
            disp.data[v*width_ * 3 + 3 * u + 2] = r;
        }
    }
}

      /*****立体匹配*****/
void stereo_match(int, void*)
{
    sgbm->setPreFilterCap(63);
    int sgbmWinSize =  5;//根据实际情况自己设定
    int NumDisparities = 416;//根据实际情况自己设定
    int UniquenessRatio = 6;//根据实际情况自己设定
    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(100);
    sgbm->setSpeckleRange(10);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(StereoSGBM::MODE_SGBM);
    Mat disp, dispf, 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);

    dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
    reprojectImageTo3D(dispf, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    imshow("disparity", disp8);
    Mat color(dispf.size(), CV_8UC3);
    GenerateFalseMap(disp8, color);//转成彩图
    imshow("disparity", color);
    saveXYZ("xyz.xls", xyz);
}



/*****描述：鼠标操作回调*****/
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()
{
    /*  立体校正    */
    Rodrigues(rec, R); //Rodrigues变换
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
        0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

    /*  读取图片    */
    rgbImageL = imread("left_cor.bmp", CV_LOAD_IMAGE_COLOR);//CV_LOAD_IMAGE_COLOR
    rgbImageR = imread("right_cor.bmp", -1);


    /*  经过remap之后，左右相机的图像已经共面并且行对准了 */
    remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//INTER_LINEAR
    remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

    /*  把校正结果显示出来*/

    //显示在同一张图上
    Mat canvas;
    double sf;
    int w, h;
    sf = 700. / 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(rectifyImageL, 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(rectifyImageR, 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);

    /*  立体匹配    */
    namedWindow("disparity", CV_WINDOW_NORMAL);
    //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
    setMouseCallback("disparity", onMouse, 0);//disparity
    stereo_match(0, 0);

    waitKey(0);
    return 0;
}

效果图如下：
这里写图片描述

可以看到广告牌距离大概为10米，车身距离大概为7米多，符合实际。

源程序下载地址（包含测试图片）