opencv中四种立体匹配算法示例（StereoBM,StereoSGBM，StereoBinaryBM, StereoBinarySGBM）

Posted 2023-02-03 ZealCV

OpenCV3.4.4
StereoBM,StereoSGBM在正式库中
StereoBinaryBM, StereoBinarySGBM在contrib模块中

1 StereoBM

// 预处理滤波参数

preFilterType：预处理滤波器的类型，主要是用于降低亮度失真（photometric distortions）、消除噪声和增强纹理等, 有两种可选类型：CV_STEREO_BM_NORMALIZED_RESPONSE（归一化响应） 或者 CV_STEREO_BM_XSOBEL（水平方向Sobel算子，默认类型）, 该参数为 int 型；
preFilterSize：预处理滤波器窗口大小，容许范围是[5,255]，一般应该在 5x5…21x21 之间，参数必须为奇数值, int 型
preFilterCap：预处理滤波器的截断值，预处理的输出值仅保留[-preFilterCap, preFilterCap]范围内的值，参数范围：1 - 31（文档中是31，但代码中是 63）, int
// SAD 参数

SADWindowSize：SAD窗口大小，容许范围是[5,255]，一般应该在 5x5 至 21x21 之间，参数必须是奇数，int 型
minDisparity：最小视差，默认值为 0, 可以是负值，int 型
numberOfDisparities：视差窗口，即最大视差值与最小视差值之差, 窗口大小必须是 16 的整数倍，int 型
// 后处理参数

textureThreshold：低纹理区域的判断阈值。如果当前SAD窗口内所有邻居像素点的x导数绝对值之和小于指定阈值，则该窗口对应的像素点的视差值为 0（That is, if the sum of absolute values of x-derivatives computed over SADWindowSize by SADWindowSize pixel neighborhood is smaller than the parameter, no disparity is computed at the pixel），该参数不能为负值，int 型
uniquenessRatio：视差唯一性百分比， 视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0 （the minimum margin in percents between the best (minimum) cost function value and the second best value to accept the computed disparity, that is, accept the computed disparity d^ only if SAD(d) >= SAD(d^) x (1 + uniquenessRatio/100.) for any d != d*+/-1 within the search range ），该参数不能为负值，一般5-15左右的值比较合适，int 型
speckleWindowSize：检查视差连通区域变化度的窗口大小, 值为 0 时取消 speckle 检查，int 型
speckleRange：视差变化阈值，当窗口内视差变化大于阈值时，该窗口内的视差清零，int 型
// OpenCV2.1 新增的状态参数

roi1, roi2：左右视图的有效像素区域，一般由双目校正阶段的 cvStereoRectify 函数传递，也可以自行设定。一旦在状态参数中设定了 roi1 和 roi2，OpenCV 会通过cvGetValidDisparityROI 函数计算出视差图的有效区域，在有效区域外的视差值将被清零。
disp12MaxDiff：左视差图（直接计算得出）和右视差图（通过cvValidateDisparity计算得出）之间的最大容许差异。超过该阈值的视差值将被清零。该参数默认为 -1，即不执行左右视差检查。int 型。注意在程序调试阶段最好保持该值为 -1，以便查看不同视差窗口生成的视差效果。具体请参见《使用OpenGL动态显示双目视觉三维重构效果示例》一文中的讨论。
在上述参数中，对视差生成效果影响较大的主要参数是 SADWindowSize、numberOfDisparities 和 uniquenessRatio 三个，一般只需对这三个参数进行调整，其余参数按默认设置即可。

在OpenCV2.1中，BM算法有C和C++ 两种实现模块。

2 StereoSGBM

SGBM算法的状态参数大部分与BM算法的一致，下面只解释不同的部分：

SADWindowSize：SAD窗口大小，容许范围是[1,11]，一般应该在 3x3 至 11x11 之间，参数必须是奇数，int 型
P1, P2：控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。OpenCV2.1提供的例程 stereo_match.cpp 给出了 P1 和 P2 比较合适的数值。
fullDP：布尔值，当设置为 TRUE 时，运行双通道动态编程算法（full-scale 2-pass dynamic programming algorithm），会占用O(WHnumDisparities)个字节，对于高分辨率图像将占用较大的内存空间。一般设置为 FALSE。
注意OpenCV2.1的SGBM算法是用C++ 语言编写的，没有C实现模块。与H. Hirschmuller提出的原算法相比，主要有如下变化：

算法默认运行单通道DP算法，只用了5个方向，而fullDP使能时则使用8个方向（可能需要占用大量内存）。
算法在计算匹配代价函数时，采用块匹配方法而非像素匹配（不过SADWindowSize=1时就等于像素匹配了）。
匹配代价的计算采用BT算法（“Depth Discontinuities by Pixel-to-Pixel Stereo” by S. Birchfield and C. Tomasi），并没有实现基于互熵信息的匹配代价计算。
增加了一些BM算法中的预处理和后处理程序。

原理解释

目前立体匹配算法是计算机视觉中的一个难点和热点，算法很多，但是一般的步骤是：

A、匹配代价计算

匹配代价计算是整个立体匹配算法的基础，实际是对不同视差下进行灰度相似性测量。常见的方法有灰度差的平方SD（squared intensity differences），灰度差的绝对值AD（absolute intensity differences）等。另外，在求原始匹配代价时可以设定一个上限值，来减弱叠加过程中的误匹配的影响。以AD法求匹配代价为例，可用下式进行计算，其中T为设定的阈值。

这就是在参数设置中阈值的作用，在视差图中经常有黑色区域，就是和阈值的设置关。

B、 匹配代价叠加

一般来说，全局算法基于原始匹配代价进行后续算法计算。而区域算法则需要通过窗口叠加来增强匹配代价的可靠性，根据原始匹配代价不同，可分为：

此图是核心算法的解释，就是计算区域内像素差值，可以为单个像素也可以为一定区域内，主要看SAD的窗口大小的设置，同时SAD设置决定误匹配的多少和运算效率问题，所以大小设置一定要很慎重。

C、 视差获取

对于区域算法来说，在完成匹配代价的叠加以后，视差的获取就很容易了，只需在一定范围内选取叠加匹配代价最优的点（SAD和SSD取最小值，NCC取最大值）作为对应匹配点，如胜者为王算法WTA（Winner-take-all）。而全局算法则直接对原始匹配代价进行处理，一般会先给出一个能量评价函数，然后通过不同的优化算法来求得能量的最小值，同时每个点的视差值也就计算出来了。

D、视差细化（亚像素级）

大多数立体匹配算法计算出来的视差都是一些离散的特定整数值，可满足一般应用的精度要求。但在一些精度要求比较高的场合，如精确的三维重构中，就需要在初始视差获取后采用一些措施对视差进行细化，如匹配代价的曲线拟合、图像滤波、图像分割等。

亚像素级的处理就是涉及到BMState参数设置后后续参数的设置了。

有关立体匹配的介绍和常见匹配算法的比较，推荐大家看看Stefano Mattoccia 的讲义 Stereo Vision: algorithms and applications，190页的ppt，讲解得非常形象详尽。

用于立体匹配的图像可以是彩色的吗？

在OpenCV3.4.4中，BM算法只能对8位灰度图像计算视差，SGBM算法则可以处理24位（8bits*3）彩色图像。所以在读入图像时，应该根据采用的算法来处理图像：

怎样获取与原图像有效像素区域相同的视差图？

在OpenCV2.0及以前的版本中，所获取的视差图总是在左侧和右侧有明显的黑色区域，这些区域没有有效的视差数据。视差图有效像素区域与视差窗口（ndisp，一般取正值且能被16整除）和最小视差值（mindisp，一般取0或负值）相关，视差窗口越大，视差图左侧的黑色区域越大，最小视差值越小，视差图右侧的黑色区域越大。其原因是为了保证参考图像（一般是左视图）的像素点能在目标图像（右视图）中按照设定的视差匹配窗口匹配对应点，OpenCV 只从参考图像的第 (ndisp - 1 + mindisp) 列开始向右计算视差，第 0 列到第 (ndisp - 1 + mindisp) 列的区域视差统一设置为 (mindisp - 1) *16；视差计算到第 width + mindisp 列时停止，余下的右侧区域视差值也统一设置为 (mindisp - 1) *16。

static const int DISPARITY_SHIFT = 4;
…
int ndisp = state->numberOfDisparities;
int mindisp = state->minDisparity;
int lofs = MAX(ndisp - 1 + mindisp, 0);
int rofs = -MIN(ndisp - 1 + mindisp, 0);
int width = left->cols, height = left->rows;
int width1 = width - rofs - ndisp + 1;
short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
initialize the left and right borders of the disparity map
for( y = 0; y < height; y++ )

for( x = 0; x < lofs; x++ )
dptr[ydstep + x] = FILTERED;
for( x = lofs + width1; x < width; x++ )
dptr[ydstep + x] = FILTERED;

dptr += lofs;
for( x = 0; x < width1; x++, dptr++ )

这样的设置很明显是不符合实际应用的需求的，它相当于把摄像头的视场范围缩窄了。因此，OpenCV2.1 做了明显的改进，不再要求左右视图和视差图的大小（size）一致，允许对视差图进行左右边界延拓，这样，虽然计算视差时还是按上面的代码思路来处理左右边界，但是视差图的边界得到延拓后，有效视差的范围就能够与对应视图完全对应。具体的实现代码范例如下：

//
// 对左右视图的左边进行边界延拓，以获取与原始视图相同大小的有效视差区域
copyMakeBorder(img1r, img1b, 0, 0, m_nMaxDisp, 0, IPL_BORDER_REPLICATE);
copyMakeBorder(img2r, img2b, 0, 0, m_nMaxDisp, 0, IPL_BORDER_REPLICATE);

//
// 计算视差
if( alg == STEREO_BM )

bm(img1b, img2b, dispb);
// 截取与原始画面对应的视差区域（舍去加宽的部分）
displf = dispb.colRange(m_nMaxDisp, img1b.cols);

else if(alg == STEREO_SGBM)

sgbm(img1b, img2b, dispb);
displf = dispb.colRange(m_nMaxDisp, img1b.cols);

4． StereoBM和StereoSGBM的输出结果好像不是以像素点为单位的视差？

BM函数得出的结果是以16位符号数的形式的存储的，出于精度需要，所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下：

dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);

可以看到，原始视差在左移8位(256)并且加上一个修正值之后又右移了4位，最终的结果就是左移4位。

因此，在实际求距离时，cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16 (也就是W除以16)，才能得到正确的三维坐标信息。”

如何实现视差图的伪彩色显示？

首先要将16位符号整形的视差矩阵转换为8位无符号整形矩阵，然后按照一定的变换关系进行伪彩色处理。我的实现代码如下：

// 转换为 CV_8U 格式，彩色显示
dispLfcv = displf, dispRicv = dispri, disp8cv = disp8;
if (alg == STEREO_GC)

cvNormalize( &dispLfcv, &disp8cv, 0, 256, CV_MINMAX );

else

displf.convertTo(disp8, CV_8U, 255/(m_nMaxDisp*16.));

F_Gray2Color(&disp8cv, vdispRGB);
灰度图转伪彩色图的代码，主要功能是使灰度图中 亮度越高的像素点，在伪彩色图中对应的点越趋向于 红色；亮度越低，则对应的伪彩色越趋向于 蓝色；总体上按照灰度值高低，由红渐变至蓝，中间色为绿色。其对应关系如

void Gray2Color(CvMat* gray_mat, CvMat* color_mat)

	if(color_mat)
		cvZero(color_mat);
		
	int stype = CV_MAT_TYPE(gray_mat->type), dtype = CV_MAT_TYPE(color_mat->type);
	int rows = gray_mat->rows, cols = gray_mat->cols;

	// 判断输入的灰度图和输出的伪彩色图是否大小相同、格式是否符合要求
	if (CV_ARE_SIZES_EQ(gray_mat, color_mat) && stype == CV_8UC1 && dtype == CV_8UC3)
	
		CvMat* red = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* green = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* blue = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* mask = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);

		// 计算各彩色通道的像素值
		cvSubRS(gray_mat, cvScalar(255), blue);	// blue(I) = 255 - gray(I)
		cvCopy(gray_mat, red);			// red(I) = gray(I)
		cvCopy(gray_mat, green);			// green(I) = gray(I),if gray(I) < 128
		cvCmpS(green, 128, mask, CV_CMP_GE );	// green(I) = 255 - gray(I), if gray(I) >= 128
		cvSubRS(green, cvScalar(255), green, mask);
		cvConvertScale(green, green, 2.0, 0.0);

		// 合成伪彩色图
		cvMerge(blue, green, red, NULL, color_mat);

		cvReleaseMat( &red );
		cvReleaseMat( &green );
		cvReleaseMat( &blue );
		cvReleaseMat( &mask );
	

示例代码

#include <opencv2/opencv.hpp>
#include "opencv2/stereo.hpp"
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <io.h>  //对系统文件进行操作的头文件 11.26 by zww

using namespace std;
using namespace cv;
using namespace cv::stereo;

/* --------------------------------------------
* 功  能：获取文件夹下所有的文件名
* 输  入：File_Directory 为文件夹目录
*         FileType 为需要查找的文件类型
*         FilesName 为存放文件名的容器
----------------------------------------------*/
void getFilesName(std::string &File_Directory, std::string &FileType, std::vector<std::string>&FilesName)

    std::string buffer = File_Directory + "\\\\*" + FileType;

    _finddata_t c_file;   // 存放文件名的结构体,需要包含头文件#include <io.h>  
    long hFile;
    hFile = _findfirst(buffer.c_str(), &c_file);   //找第一个文件名

    if (hFile == -1L)   // 检查文件夹目录下存在需要查找的文件
        printf("No %s files in current directory!\\n", FileType.c_str());
    else
    
        std::string fullFilePath;
        do
        
            fullFilePath.clear();
            fullFilePath = File_Directory + "\\\\" + c_file.name;
            FilesName.push_back(fullFilePath);

         while (_findnext(hFile, &c_file) == 0);  //如果找到下个文件的名字成功的话就返回0,否则返回-1  
        _findclose(hFile);
    


void CensusTransform(Mat src_image, Mat &dst_image, int window_sizex, int window_sizey)

    int image_height = src_image.rows;
    int image_width = src_image.cols;

    dst_image = Mat::zeros(image_height, image_width, CV_64F);

    //-----------census变换  --------------------------------- 
    int offsetx = (window_sizex - 1) / 2;
    int offsety = (window_sizey - 1) / 2;
    for (int j = 0; j < image_width - window_sizex; j++)
    
        for (int i = 0; i < image_height - window_sizey; i++)
        
            unsigned long census = 0;
            uchar current_pixel = src_image.at<uchar>(i + offsety, j + offsetx); //窗口中心像素
            Rect roi(j, i, window_sizex, window_sizey); //方形窗口
            Mat window(src_image, roi);

            for (int a = 0; a < window_sizey; a++)
            
                for (int b = 0; b < window_sizex; b++)
                
                    if (!(a == offsety && b == offsetx))//中心像素不做判断
                    
                        census = census << 1;//左移1位 
                    
                    uchar temp_value = window.at<uchar>(a, b);
                    if (temp_value <= current_pixel) //当前像素小于中心像素 01
                    
                        census += 1;
                    
                
            
            dst_image.at<double>(i + offsety, j + offsetx) = census;
        
    


// Fast Hamming distance algorithm
unsigned char Hammingdst(long long PL, long long PR)

    unsigned char number = 0;
    long long v;
    v = PL^PR;            /* ^ 异或运算 不同为1 相同为0*/

    while (v)
    
        v &= (v - 1);            /* & 与运算*/
        number++;
    
    return number;


float SAD(Mat lImg, Mat rImg, Point2i pl, Point2i pr, int size)

    float diff = 0.0;
    for (int i = -size; i <= size; i++)
    
        for (int j = -size; j <= size; j++)
        
            Point2i pl_temp = pl + Point2i(i, j);
            Point2i pr_temp = pr + Point2i(i, j);
            diff += abs(lImg.at<uchar>(pl_temp) - rImg.at<uchar>(pr_temp));
        
    
    return diff;


void insertDepth32f(cv::Mat& depth)

    const int width = depth.cols;
    const int height = depth.rows;
    float* data = (float*)depth.data;
    cv::Mat integralMap = cv::Mat::zeros(height, width, CV_64F);
    cv::Mat ptsMap = cv::Mat::zeros(height, width, CV_32S);
    double* integral = (double*)integralMap.data;
    int* ptsIntegral = (int*)ptsMap.data;
    memset(integral, 0, sizeof(double) * width * height);
    memset(ptsIntegral, 0, sizeof(int) * width * height);
    for (int i = 0; i < height; ++i)
    
        int id1 = i * width;
        for (int j = 0; j < width; ++j)
        
            int id2 = id1 + j;
            if (data[id2] > 1e-3) 
                integral[id2] = data[id2];
                ptsIntegral[id2] = 1;
            
        
    
    // 积分区间
    for (int i = 0; i < height; ++i)
    
        int id1 = i * width;
        for (int j = 1; j < width; ++j) 
            int id2 = id1 + j;
            integral[id2] += integral[id2 - 1];
            ptsIntegral[id2] += ptsIntegral[id2 - 1];
        
    
    for (int i = 1; i < height; ++i)
    
        int id1 = i * width;
        for (int j = 0; j < width; ++j) 
            int id2 = id1 + j;
            integral[id2] += integral[id2 - width];
            ptsIntegral[id2] += ptsIntegral[id2 - width];
        
    
    int wnd;
    double dWnd = 2;
    while (dWnd > 1)
    
        wnd = int(dWnd);
        dWnd /= 2;
        for (int i = 0; i < height; ++i)
        
            int id1 = i * width;
            for (int j = 0; j < width; ++j)
            
                int id2 = id1 + j;
                int left = j - wnd - 1;
                int right = j + wnd;
                int top = i - wnd - 1;
                int bot = i + wnd;
                left = max(0, left);
                right = min(right, width - 1);
                top = max(0, top);
                bot = min(bot, height - 1);
                int dx = right - left;
                int dy = (bot - top) * width;
                int idLeftTop = top * width + left;
                int idRightTop = idLeftTop + dx;
                int idLeftBot = idLeftTop + dy;
                int idRightBot = idLeftBot + dx;
                int ptsCnt = ptsIntegral[idRightBot] + ptsIntegral[idLeftTop] - (ptsIntegral[idLeftBot] + ptsIntegral[idRightTop]);
                double sumGray = integral[idRightBot] + integral[idLeftTop] - (integral[idLeftBot] + integral[idRightTop]);
                if (ptsCnt <= 0) 
                    continue;
                
                data[id2] = float(sumGray / ptsCnt);
            
        
        int s = wnd / 2 * 2 + 1;
        if (s > 201) 
            s = 201;
        
        cv::GaussianBlur(depth, depth, cv::Size(s, s), s, s);
    


/*------------------------------------------------------
* 功  能：视差图转深度图
* 输  入：dispMap  ----视差图，8位单通道，CV_8UC1
*       　K        ----内参矩阵，float类型
* 输  出：depthMap ----深度图，16位无符号单通道，CV_16UC1
-------------------------------------------------------*/
void disp2Depth(cv::Mat dispMap, cv::Mat &depthMap, cv::Mat K)

    int type = dispMap.type();

    float fx = K.at<float>(0, 0);
    float fy = K.at<float>(1, 1);
    float cx = K.at<float>(0, 2);
    float cy = K.at<float>(1, 2);
    float baseline = 65;               //基线距离65mm

    if (type == CV_8U) 
        const float PI = 3.14159265358;
        int height = dispMap.rows;
        int width = dispMap.cols;

        uchar* dispData = (uchar*)dispMap.data;
        double* depthData = (double*)depthMap.data;
        for (int i = 0; i < height; i++)
        
            for (int j = 0; j < width; j++)
            
                int id = i*width + j;
                if (!dispData[id])  continue;  //防止0除
                depthData[id] = double((float)fx *baseline / ((float)dispData[id]));
            
        
     
    else 
        cout << "please confirm dispImg's type!" << endl;
        cv::waitKey(0);
    


/*------------------------------------------------------
* 功  能：将图片保存为ppm格式
* 输  入：output      ----ppm文件的存储路径
* 输  出：img         ----存储ppm文件的矩阵
-------------------------------------------------------*/
void writeppm(cv::Mat img, char* output)

    ofstream ppm(output, ios::app | ios::binary);
    ppm << "P6" << endl << img.cols << " " << img.rows << endl << 255 << endl;
    for (int j = 0; j < img.rows; j++) 
        for (int i = 0; i < img.cols; i++) 
            uchar* temp_ptr = &((uchar*)(img.data + img.channels()*img.cols*j))[i * 3];
            ppm << temp_ptr[2] << temp_ptr[1] << temp_ptr[0];
        
    
    ppm.close();


enum  STEREO_BM, STEREO_BINARY_BM, STEREO_SGBM, STEREO_BINARY_SGM ;

int main()

    //----------------------   读取图像路径   --------------------------------//
    //std::string File_Directory = ".\\\\data";   //文件夹目录   
    //std::string FileType = ".BMP";            // 需要查找的文件类型

    std::string File_Directory = ".\\\\img_stereo";   //文件夹目录   
    std::string FileType = ".ppm";                  // 需要查找的文件类型

    std::string result_File_Directory = "..\\\\result";  //文件夹目录

    std::vector<std::string> imgpaths;
    getFilesName(File_Directory, FileType, imgpaths);
    int img_pair_num = imgpaths.size() / 2;

    cv::Mat rectifyImageL, rectifyImageR;
    for (int i = 0; i < img_pair_num; i++)
    
        rectifyImageL = cv::imread(imgpaths[2 * i], 0);
        rectifyImageR = cv::imread(imgpaths[2 * i + 1], 0);
        const int imageWidth = rectifyImageL.cols;
        const int imageHeight = rectifyImageL.rows;
        const int channel = rectifyImageL.channels();
        Size imageSize = Size(imageWidth, imageHeight);

        Mat imgDisparity16S = Mat(imageHeight, imageWidth, CV_16S);
        Mat imgDisparity32F = Mat(imageHeight, imageWidth, CV_32F);
        Mat imgDisparity8U = Mat(imageHeight, imageWidth, CV_8U);

        Mat disp, disp8;

        int number_of_disparities = 64;   //视差搜索范围
        int minDisparity = 0;             //最小视差
        int maxDisparity = 0;             //最大视差

        double matching_time;
        int stereo_algorithm_type = STEREO_SGBM;
        if (stereo_algorithm_type == STEREO_BM)
        
            cvtColor(rectifyImageL, rectifyImageL, COLOR_BGR2GRAY);
            cvtColor(rectifyImageR, rectifyImageR, COLOR_BGR2GRAY);

            int wsize = 9;
            cv::Ptr<cv::StereoBM> left_matcher = StereoBM::create(maxDisparity, wsize);

            matching_time = (double)getTickCount();
            left_matcher->compute(rectifyImageL, rectifyImageR, disp);

            matching_time = ((double)getTickCount() - matching_time) / getTickFrequency();

        
        else if (stereo_algorithm_type == STEREO_SGBM)
        
            int SADWindowSize = 9;

            cv::Ptr<cv::StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
            sgbm->setPreFilterCap(15);

            sgbm->setBlockSize(SADWindowSize);
            sgbm->setP1(8 * channel*SADWindowSize*SADWindowSize);
            sgbm->setP2(32 * channel*SADWindowSize*SADWindowSize);
            sgbm->setMinDisparity(minDisparity);
            sgbm->setNumDisparities(number_of_disparities);

            sgbm->setUniquenessRatio(10);
            sgbm->setSpeckleWindowSize(50);
            sgbm->setSpeckleRange(32);
            sgbm->setDisp12MaxDiff(1);
            sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
            sgbm->compute(rectifyImageL, rectifyImageR, imgDisparity16S);

            imgDisparity16S.convertTo(imgDisparity32F, CV_32F, 1.0 / 16);  //除以16得到真实视差值

            //reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
            //xyz = xyz * 16;

            cv::Mat color;
            double max, min;
            Point minLoc, maxLoc;
            minMaxLoc(disp8, &min, &max, &maxLoc, &minLoc);

            cv::Mat grayImage;
            double alpha = 255.0 / (max - min);
            disp8.convertTo(grayImage, CV_8UC1, alpha, -alpha * min);// expand your range to 0..255. Similar to histEq();
            cv::applyColorMap(grayImage, color, cv::COLORMAP_JET);

            imshow("disparity", disp8);

        
        if (stereo_algorithm_type == STEREO_BINARY_BM)
        
            int binary_descriptor_type = 0;

            int kernel_size;
            if (binary_descriptor_type == 0)
                kernel_size = 5;
            else if (binary_descriptor_type == 2 || binary_descriptor_type == 3)
                kernel_size = 7;
            else if (binary_descriptor_type == 1)
                kernel_size = 11;
            else
                kernel_size = 9;

            int aggregation_window;
            if (binary_descriptor_type == 3)
                aggregation_window = 13;
            else
                aggregation_window = 11;

            Ptr<StereoBinaryBM> sbm = StereoBinaryBM::create(number_of_disparities, kernel_size);
            sbm->setPreFilterCap(31);
            sbm->setMinDisparity(0);
            sbm->setTextureThreshold(10);
            sbm->setUniquenessRatio(0);
            sbm->setSpeckleWindowSize(400); // speckle size
            sbm->setSpeckleRange(200);
            sbm->setDisp12MaxDiff(0);
            sbm->setScalleFactor(16); // the scaling factor
            sbm->setBinaryKernelType(binary_descriptor_type); // binary descriptor kernel
            sbm->setAgregationWindowSize(aggregation_window);
            sbm->setSpekleRemovalTechnique(CV_SPECKLE_REMOVAL_AVG_ALGORITHM);
            sbm->setUsePrefilter(false);

            sbm->compute(rectifyImageL, rectifyImageR, imgDisparity8U);
            imshow("Disparity", imgDisparity8U);
        
        else if (stereo_algorithm_type == STEREO_BINARY_SGM)
        
            int kernel_size = 0;
            
            int aggregation_window = 0;
            int P1 = 10;
            int P2 = 100;
            int binary_descriptor_type = 4;  //census变换类型，只能赋值4，其他类型有bug

            Ptr<StereoBinarySGBM> sgbm = StereoBinarySGBM::create(0, number_of_disparities, kernel_size);
            sgbm->setP1(P1);
            sgbm->setP2(P2);
            sgbm->setMinDisparity(0);
            sgbm->setUniquenessRatio(5);
            sgbm->setSpeckleWindowSize(400);
            sgbm->setSpeckleRange(0);
            sgbm->setDisp12MaxDiff(1);
            sgbm->setBinaryKernelType(binary_descriptor_type);
            sgbm->setSpekleRemovalTechnique(CV_SPECKLE_REMOVAL_AVG_ALGORITHM);
            sgbm->setSubPixelInterpolationMethod(CV_SIMETRICV_INTERPOLATION);
            sgbm->compute(rectifyImageL, rectifyImageR, imgDisparity16S2);
            Alternative for scalling
            //imgDisparity16S2.convertTo(imgDisparity8U2, CV_8UC1, scale);

            double minVal; double maxVal;
            minMaxLoc(imgDisparity16S2, &minVal, &maxVal);
            imgDisparity16S2.convertTo(imgDisparity8U2, CV_8UC1, 255 / (maxVal - minVal));

            imshow("Windowsgm", imgDisparity8U2);
        
       

    
    return 0;