双目立体视觉匹配算法之视差图算法从理论到实践专题1

原创

HelloCVCG 2022-07-22 18:24:23 博主文章分类：OpenCV ©著作权

文章标签 OpenCV #include 像素点 scala 文章分类 数据结构与算法人工智能

©著作权归作者所有：来自51CTO博客作者HelloCVCG的原创作品，请联系作者获取转载授权，否则将追究法律责任

1、资源搜索

双目立体视觉匹配算法-----SAD匹配算法、BM算法、SGBM算法、GC算法

半全局块匹配（Semi-Global Block Matching）算法

基于BM算法的双目测距

opencvSGBM半全局立体匹配算法的研究(1)

2、本人总结

在讲具体实现之前，我们还是先想想为什么会此概念。我们要实现的是相机能够可以知道这个场景的深度信息，但是从计算成像角度使用单个相机是不能够得到当前场景的深度。当然，你可以说现在DeepLearning可以实现单张出现深度情况，深度学习确实可以得到从单张图像得到深度，但是它的精度以及复杂场景的适应还需要进一步提高。所以如何解决这个问题呢，这时候我们就想到我们人类自己，我们是怎么知道场景的深度呢。我们使用两只眼睛就可以准确知道当前场景的深度信息，所以很多研究员开始从这个角度作为出发点，来给机器人一双眼睛通过数学计算来得到场景深度。肯定还有人会反驳，我只用一只眼睛也可以知道啊，对确实是这样的，这是我们大脑进行了各种场景的学习，才实现单只眼睛看到场景深度。估计这获取是单张图像得到深度研究的出发点吧。好，既然我们知道双目从哪里借鉴了，那么咱们讲讲机器人在双目深度估计中最重要的匹配算法。下面从一种SAD算法、BM算法、SGBM算法、GC算法展开研究。

2.1 SAD算法

2.1.1 基本概念

该算法是一种图像匹配算法，其主要思想就是差的绝对值之和。

$SAD(u, v) = \sum \left | left(u, v) - Right(u, v) \right |$

并选择其中的最小值。即以左目图像的源匹配点为中心，并且定义一个

双目立体视觉匹配算法之视差图算法从理论到实践专题1_OpenCV_02

矩形大小的窗口D，统计该区域对应的灰度值之和。然后右目逐步计算左右窗口灰度和的差值，最后搜索差值最小区域的中心像素即为匹配点。

2.1.1 基本流程

图像输入：两张矫正之后的图像，对左图，依次扫描，选定一个锚点：

（1）首先构造一个小窗口,类似于卷积核；
（2）用窗口覆盖左边的图像，并选择出窗口覆盖区域内的所有像素点；
（3）用窗口覆盖右边的图像，并选择出覆盖区域的像素点；
（4）利用上述计算公式求出所有像素点灰度差的绝对值之和；
（5）移动右边图像的窗口，重复（3）-（4）的处理，记住这里会设置一个窗口滑动阈值；
（6）找到这个范围内SAD值最小的窗口，即找到了左图锚点的最佳匹配的像素块。

双目立体视觉匹配算法之视差图算法从理论到实践专题1_像素点_03

说明：图片中的灰色区域为创建的窗口大小（3*3），距离d会在设定的阈值条件里进行移动；

2.1.2 代码实现

//main.cpp
//
#include"SAD.h"
int main(int argc, char* argv[])
{
  namedWindow("Img_L", 0);
  namedWindow("Img_R", 0);
  namedWindow("Disparity", 0);
  Mat Img_L = imread("aloeL.jpg", 0);
  Mat Img_R = imread("aloeR.jpg", 0);
  Mat Disparity;    //视差图
            //SAD mySAD;
  SAD mySAD(7, 30);
  Disparity = mySAD.computerSAD(Img_L, Img_R);

  imshow("Img_L", Img_L);
  imshow("Img_R", Img_R);
  imshow("Disparity", Disparity);
  waitKey();
  return 0;
}

//SAD.h
#include"iostream"
#include"opencv2/opencv.hpp"
#include"iomanip"

using namespace std;
using namespace cv;

class SAD
{
public:
  SAD() :winSize(7), DSR(30) {}
  SAD(int _winSize, int _DSR) :winSize(_winSize), DSR(_DSR) {}
  Mat computerSAD(Mat &L, Mat &R); //计算SAD
private:
  int winSize; //卷积核的尺寸
  int DSR;     //视差搜索范围

};

Mat SAD::computerSAD(Mat &L, Mat &R)
{
  int Height = L.rows;
  int Width = L.cols;
  Mat Kernel_L(Size(winSize, winSize), CV_8U, Scalar::all(0));
  Mat Kernel_R(Size(winSize, winSize), CV_8U, Scalar::all(0));
  Mat Disparity(Height, Width, CV_8U, Scalar(0)); //视差图

  for (int i = 0;i<Width - winSize;i++)  //左图从DSR开始遍历
  {
    for (int j = 0;j<Height - winSize;j++)
    {
      Kernel_L = L(Rect(i, j, winSize, winSize));
      Mat MM(1, DSR, CV_32F, Scalar(0)); //

      for (int k = 0;k<DSR;k++)
      {
        int x = i - k;
        if (x >= 0)
        {
          Kernel_R = R(Rect(x, j, winSize, winSize));
          Mat Dif;
          absdiff(Kernel_L, Kernel_R, Dif);//
          Scalar ADD = sum(Dif);
          float a = ADD[0];
          MM.at<float>(k) = a;
        }

      }
      Point minLoc;
      minMaxLoc(MM, NULL, NULL, &minLoc, NULL);

      int loc = minLoc.x;
      //int loc=DSR-loc;
      Disparity.at<char>(j, i) = loc * 16;

    }

  }
  return Disparity;
}

双目立体视觉匹配算法之视差图算法从理论到实践专题1_scala_04

说明：实验使用的数据是OpenCV提供的

2.2 BM算法

2.2.1 简单说明
其实BM算法核心就是上面提到的SAD方法，所以这里就不就行特别说明了。但是大家看到效果后肯定会有疑问，为什么相同但实现的效果和速度上有很大差别呢！因为OpenCV在实现的时候，进行了滤波处理（使用sobel处理，关于这块讲解可以参考链接）和加速优化计算。

2.2.2 代码实现

//main.cpp 
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"

#include <stdio.h>

using namespace cv;

static void print_help()
{
    printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n");
    printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|sgbm3way] [--blocksize=<block_size>]\n"
           "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
           "[--no-display] [-o=<disparity_image>] [-p=<point_cloud_file>]\n");
}

static void saveXYZ(const char* filename, const Mat& mat)
{
    const double max_z = 1.0e4;
    FILE* fp = fopen(filename, "wt");
    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);
}

int main(int argc, char** argv)
{
    std::string img1_filename = "";
    std::string img2_filename = "";
    std::string intrinsic_filename = "";
    std::string extrinsic_filename = "";
    std::string disparity_filename = "./disparity.jpg";
    std::string point_cloud_filename = "";

    enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3, STEREO_3WAY=4 };
    int alg = STEREO_SGBM;
    int SADWindowSize, numberOfDisparities;
    bool no_display;
    float scale;

    Ptr<StereoBM> bm = StereoBM::create(16,9);
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);
    cv::CommandLineParser parser(argc, argv,
        "{@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{no-display||}{scale|1|}{i||}{e||}{o||}{p||}");
    /*if(parser.has("help"))
    {
        print_help();
        return 0;
    }*/
    //img1_filename = parser.get<std::string>(0);
    //img2_filename = parser.get<std::string>(1);
    img1_filename = "aloeL.jpg";
    img2_filename = "aloeR.jpg";

    if (parser.has("algorithm"))
    {
        std::string _alg = parser.get<std::string>("algorithm");
        alg = _alg == "bm" ? STEREO_BM :
            _alg == "sgbm" ? STEREO_SGBM :
            _alg == "hh" ? STEREO_HH :
            _alg == "var" ? STEREO_VAR :
            _alg == "sgbm3way" ? STEREO_3WAY : -1;
    }
    numberOfDisparities = 256;//即最大视差值与最小视差值之差, 窗口大小必须是 16 的整数倍
    SADWindowSize = 7;
    scale = 1.0;//图像尺度
    no_display = false;//不显示
    if( parser.has("i") )
        intrinsic_filename = parser.get<std::string>("i");
    if( parser.has("e") )
        extrinsic_filename = parser.get<std::string>("e");
    if( parser.has("o") )
        disparity_filename = parser.get<std::string>("o");
    if( parser.has("p") )
        point_cloud_filename = parser.get<std::string>("p");
    if (!parser.check())
    {
        parser.printErrors();
        return 1;
    }
    if( alg < 0 )
    {
        printf("Command-line parameter error: Unknown stereo algorithm\n\n");
        print_help();
        return -1;
    }
    if ( numberOfDisparities < 1 || numberOfDisparities % 16 != 0 )
    {
        printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
        print_help();
        return -1;
    }
    if (scale < 0)
    {
        printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
        return -1;
    }
    if (SADWindowSize < 1 || SADWindowSize % 2 != 1)
    {
        printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
        return -1;
    }
    if( img1_filename.empty() || img2_filename.empty() )
    {
        printf("Command-line parameter error: both left and right images must be specified\n");
        return -1;
    }
    if( (!intrinsic_filename.empty()) ^ (!extrinsic_filename.empty()) )
    {
        printf("Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified)\n");
        return -1;
    }

    if( extrinsic_filename.empty() && !point_cloud_filename.empty() )
    {
        printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
        return -1;
    }

    int color_mode = alg == STEREO_BM ? 0 : -1;
    Mat img1 = imread(img1_filename, color_mode);
    Mat img2 = imread(img2_filename, color_mode);

    if (img1.empty())
    {
        printf("Command-line parameter error: could not load the first input image file\n");
        return -1;
    }
    if (img2.empty())
    {
        printf("Command-line parameter error: could not load the second input image file\n");
        return -1;
    }

    if (scale != 1.f)
    {
        Mat temp1, temp2;
        int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
        resize(img1, temp1, Size(), scale, scale, method);
        img1 = temp1;
        resize(img2, temp2, Size(), scale, scale, method);
        img2 = temp2;
    }

    Size img_size = img1.size();

    Rect roi1, roi2;
    Mat Q;

    if( !intrinsic_filename.empty() )
    {
        // reading intrinsic parameters
        FileStorage fs(intrinsic_filename, FileStorage::READ);
        if(!fs.isOpened())
        {
            printf("Failed to open file %s\n", intrinsic_filename.c_str());
            return -1;
        }

        Mat M1, D1, M2, D2;
        fs["M1"] >> M1;
        fs["D1"] >> D1;
        fs["M2"] >> M2;
        fs["D2"] >> D2;

        M1 *= scale;
        M2 *= scale;

        fs.open(extrinsic_filename, FileStorage::READ);
        if(!fs.isOpened())
        {
            printf("Failed to open file %s\n", extrinsic_filename.c_str());
            return -1;
        }

        Mat R, T, R1, P1, R2, P2;
        fs["R"] >> R;
        fs["T"] >> T;

        stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );

        Mat map11, map12, map21, map22;
        initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
        initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);

        Mat img1r, img2r;
        remap(img1, img1r, map11, map12, INTER_LINEAR);
        remap(img2, img2r, map21, map22, INTER_LINEAR);

        img1 = img1r;
        img2 = img2r;
    }

    numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16;

    bm->setROI1(roi1);
    bm->setROI2(roi2);
    bm->setPreFilterCap(31);
    bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9);
    bm->setMinDisparity(0);
    bm->setNumDisparities(numberOfDisparities);
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(15);
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(1);

    sgbm->setPreFilterCap(63);
    int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
    sgbm->setBlockSize(sgbmWinSize);

    int cn = img1.channels();

    sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(numberOfDisparities);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);
    if(alg==STEREO_HH)
        sgbm->setMode(StereoSGBM::MODE_HH);
    else if(alg==STEREO_SGBM)
        sgbm->setMode(StereoSGBM::MODE_SGBM);
    else if(alg==STEREO_3WAY)
        sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);

    Mat disp, disp8;
    //Mat img1p, img2p, dispp;
    //copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
    //copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);

    int64 t = getTickCount();
    if( alg == STEREO_BM )
        bm->compute(img1, img2, disp);
    else if( alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_3WAY )
        sgbm->compute(img1, img2, disp);
    t = getTickCount() - t;
    printf("Time elapsed: %fms\n", t*1000/getTickFrequency());

    //disp = dispp.colRange(numberOfDisparities, img1p.cols);
    if( alg != STEREO_VAR )
        disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
    else
        disp.convertTo(disp8, CV_8U);
    if( !no_display )
    {
        namedWindow("left", 1);
        imshow("left", img1);
        namedWindow("right", 1);
        imshow("right", img2);
        namedWindow("disparity", 0);
        imshow("disparity", disp8);
        printf("press any key to continue...");
        fflush(stdout);
        waitKey();
        printf("\n");
    }

    if(!disparity_filename.empty())
        imwrite(disparity_filename, disp8);

    if(!point_cloud_filename.empty())
    {
        printf("storing the point cloud...");
        fflush(stdout);
        Mat xyz;
        reprojectImageTo3D(disp, xyz, Q, true);
        saveXYZ(point_cloud_filename.c_str(), xyz);
        printf("\n");
    }

    return 0;
}

说明：此代码为OpenCV官方提供

双目立体视觉匹配算法之视差图算法从理论到实践专题1_OpenCV_05

2.3 SGBM算法

这里讲解的SGBM主要是讲OpenCV中如何实现的，大家如果想了解作者提出的SGM，请看论文《《Stereo Processing by Semi-Global Matching and Mutual Information》》，当然后面有时间的话，我也会对论文进行说明。

2.3.1 基本概念

该算法是一个半全局匹配算法，其实现效果明显优于局部匹配算法，但是同时会增加算法的复杂度。

2.3.2 实现流程

该算法主要含有4部分组成：图像预处理、代价计算、动态规划、后处理。

2.3.2.1 图像预处理

（1）Sobel算子图像计算

使用Sobel对图像进行处理，来得到梯度信息：

$\small Sobel = 2\left [ P(x+1, y) - P(x-1, y) \right ] + P(x+1, y-1) - P(x-1, y-1) + P(x+1, y + 1) - P(x-1, y+1)$

（2）映射得到新图像

将经过步骤（1）处理后的图像，映射为一个新图像

$\small P_{new}$

；

双目立体视觉匹配算法之视差图算法从理论到实践专题1_scala_08

其中preFilterCap为一个常数，在程序实现时可以自行进行设定；将

$\small P_{new}$

（为图像的梯度信息）进行代价处理。

2.3.2.2 代价计算

该部分由两个代价组成：梯度图像、原图像。两幅图像分别使用基于采样的方法，进行SAD代价计算。关于为什么采用基于采样的方法，可以参考《DepthDiscontinuities by Pixel-to-Pixel Stereo》

2.3.2.3 动态规划

动态规划公式：

$\small L_{r}(p,d) = C(p,d)+min(L_{r}(p-r,d), L_{r}(p-r,d-1)+P_{1}, L_{r}(p-r,d+1)+P_{1}, \begin{matrix} min\\ i \end{matrix} L_{r}(p-r,i)+P_{2} )- \begin{matrix} min\\ k \end{matrix} L_{r}(p-r,k)$

默认4条路径，其中动态规划很重要两个参数

$\small P_{1}$

和

$\small P_{2}$

其计算公式：

$\small P1 =8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize$

;

$\small P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize$

; cn是图像的通道数, SADWindowSize是SAD窗口大小，数值为奇数。从公式可以看出，当SAD窗口确定时，SGBM规划参数

$\small P_{1}$

和

$\small P_{2}$

也就确定。

2.3.2.3 后处理

后处理主要由4部分组成：唯一性检测、亚像素插值、左右一致性检测、连通区域检测；

（1）唯一性检测

视差窗口范围内最低代价是次代价的

$\small (1+\frac{uniquenessRatio}{100})$

倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为0。其中

$\small uniquenessRatio$

为常数

（2）亚像素插值

$\small denom2=\frac{max(Sp[d-1]+Sp[d+1]-2*Sp[d], 1)}{16}$

$\small d=d+\frac{(Sp[d-1]-Sp[d+1]+denom2)}{denom2 * 2}$

（3）左右一致性检测

双目立体视觉匹配算法之视差图算法从理论到实践专题1_scala_21

遇见上图情况dispR[X-d]是d还是d+n?

实际上，dispR[X-d]确定方式是比较dispL中X和X+n出的最小代价，选代价最小对应的视差最为最优视差。

通过下面的方式来确定左视图中像素的视差是否为有效：

双目立体视觉匹配算法之视差图算法从理论到实践专题1_scala_22 disp12MaxDiff" title="\small \small |dispR[X-d]-dispL[X]|>disp12MaxDiff" style="width: 338px; visibility: visible; height: 17px;" data-type="block">

双目立体视觉匹配算法之视差图算法从理论到实践专题1_#include_23 disp12MaxDiff" title="\small | dispR[X-d-1]- dispL[X]-1|>disp12MaxDiff" style="width: 395px; visibility: visible; height: 17px;" data-type="block">

if( 0 <= _x && _x < width && disp2Buf[_x] >= minD && std::abs(disp2Buf[_x] - _d) > disp12MaxDiff &&
    0 <= x_ && x_ < width && disp2Buf[x_] >= minD && std::abs(disp2Buf[x_] - d_) > disp12MaxDiff )
    disp_row[x] = (short)INVALID_DISP_SCALED;

（注：根据OpenCV的源码可以看到应该是大于号）
满足上述条件，则就认为是误匹配点。

满足上述条件情况下，其实检测的大都是遮挡点。比如上图中，左视差图X和X+n按照规则都会映射到右视差图的X-d处，若再左视差图中，X位置是遮挡点，那么左差图X位置最小代价一定会比X+n处大（因为X处找不到匹配，所谓最小代价也是很大的），这样的话dispR[X-d] = d + n。

先看左边判别式：

检测到X处（遮挡误匹配点），发现dispR[x-d]=d+n，并不是d。因此就会符合条件1中

$\small |dispR[X-d]-dispL[X]|<disp12MaxDiff$