[双目视差] 立体匹配-SGBM半全局立体匹配算法

立体匹配-SGBM半全局立体匹配算法

一、SGBM算法实现过程

1、预处理

预处理目的是得到图像的梯度信息

Step1：SGBM采用水平Sobel算子，对图像做处理，公式为：

Sobel(x,y)=2[P(x+1,y)-P(x-1,y)]+ P(x+1,y-1)-P(x-1,y-1)+ P(x+1,y+1)-P(x-1,y+1)

Step2：用一个函数将经过水平Sobel算子处理后的图像上每个像素点（P表示其像素值）映射成一个新的图像；
PNEW表示新图像上的像素值。

映射函数：

其中preFilterCap 为一个常数参数，opencv缺省情况下取15，例程中取63。

2、代价计算

Step1：经过预处理得到的图像的梯度信息，经过基于采样的方法得到的梯度代价

Step2：原图像经过基于采样的方法得到的SAD代价

上述两个代价都会在SAD窗口内进行计算
关于什么是基于采样的方法，参考论文：DepthDiscontinuities by Pixel-to-Pixel Stereo 在计算代价的时候，用基于采样的方法效果会好一些。

3、动态规划

动态规划算法本身存在拖尾效应，视差突变处易产生错误的匹配，利用态规划进行一维能量累积，会将错误的视差信息传播给后面的路径上。半全局算法利用多个方向上的信息，试图消除错误信息的干扰，能明显减弱动态规划算法产生的拖尾效应。

半全局算法试图通过影像上多个方向上一维路径 的约束，来建立一个全局的马尔科夫能量方程，每个像素最终的匹配代价是所有路径信息的叠加，每个像素的视差选择都只是简单通过 WTA（Winner Takes All）决定的。多方向能量聚集如下图所示：

在每个方向上按照动态规划的思想进行能量累积，然后将各个方向上的匹配代价相加得到总的匹配代价，如下式所示：

式中L为当前路径累积的代价函数，P1、P2为像素点与相邻点视差存在较小和较大差异情况下的平滑惩罚，P1<P2，第三项无意义，仅仅是为了消除各个方向路径长度不同造成的影响。将所有r方向的匹配代价相加得到总的匹配代价，如下：

默认4条路径，其中动态规划很重要两个参数P1，P2是这样设定的：
P1 =8cnsgbm.SADWindowSizesgbm.SADWindowSize;
P2 = 32cnsgbm.SADWindowSizesgbm.SADWindowSize;
cn是图像的通道数, SADWindowSize是SAD窗口大小，数值为奇数。

4、后处理

opencvSGBM的后处理包含以下几个步骤：

Step1：唯一性检测：视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为0。其中uniquenessRatio是一个常数参数。

Step2：亚像素插值，插值公式：

Step3：左右一致性检测：误差阈值disp12MaxDiff默认为1，可以自己设置。

OpencvSGBM计算右视差图的方式：通过得到的左视察图计算右视差图，如图：

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

确定右图视差后，通过这样一个措施来确定左视察图中的像素视差是否有效：

|dispR[X-d]-dispL[X]|<disp12MaxDiff&&|dispR[X-d-1]-dispL[X]-1|<disp12MaxDiff （条件1）

如果这个条件成立，就是误匹配点：

• 实际上这个条件（LRcheck）检测的大都是遮挡点，比如图中，左视差图X和X+n处按照图一规则都会映射到右视差图的X-d处,若在左视差图中，X位置是遮挡点，那么左视差图X位置的最小代价一定会比X+n处大（因为X处根本找不到匹配，所谓最小代价也是很大的），这样dispR[X-d]=d+n。

用条件1来检测左视差图的误匹配点：

• 条件1左侧的条件：

  • 检测到X处（遮挡误匹配点），发现dispR[x-d]=d+n，并不是d。因此就会符合条件1中的dispR[X-d]-dispL[X]|<disp12MaxDiff，实际上这个条件是想解决一个这样的事情：当左图多个点同时匹配到右图中同一个点时，如何选择正确的匹配关系，毕竟左右图像素应该是一对一的。
  • 解决方法：认为多对匹配关系中，匹配代价最低的是正确匹配关系，然后用条件1选出正确的匹配关系。
    亲测把条件1变成dispR[X-d]-dispL[X]|<disp12MaxDiff检测效果和条件1几乎无差别。

• 条件1右侧的条件：

  • （| dispR[X-d-1]-dispL[X]-1|<disp12MaxDiff）是用来检测右视差图在X-d处的视差值是否连续
  • 如果左视差图X处是遮挡误匹配点，那么dispL[X]中的d是不准确的，因此dispR[X-d]=d也是不准确的，即不能保证dispR[X-d-1]处视差值和d很相近，故右视差图在X-d处的视差值是不连续的。

综上，在左视察图中，如果一个像素点是遮挡误匹配点，那么就会符合条件1，因而就会被检测出来。

Step4：连通区域的检测：简述：对左右一致性检测后的视差图再一次检测误匹配点，根据与当前处理的视差点满足连通条件的像素点个数来判断当前处理的视差点是否是误匹配点，个数小于一个阈值就认为是误匹配点。

• 方法：循环遍历每一个像素点，对每一个视差像素点d而言，检测其周围（上下左右）的视差是否满足这样的条件（称为视差连通条件）：

  • 1、 首先是LRcheck后，视差有效的点
  • 2、 和中心视差值的（变化）绝对值不超过speckleRange。注：speckleRange是一个常数参数，可以自己设定。Opencv例程中speckleRange=10

• 对于一个视差点：

  • Step1：当上下左右（以下简称周围）点至少有一个视差点满足视差连通条件后，再分别以它们为起点（称为传播），检测其周围（前向传播的点不算，比如，Pixel2是通过Pixel1传播过来的，即Pixel2肯定是Pixel1周围的点。再以Pixel2为起点检测周围的视差点是否满足视差连通条件时，Pixel1虽然也是Pixel2周围的视差点，但不算满足视差连通条件）的视差点是否满足视差连通条件。
  • Step2：每检测到一个新的连通点，其相应点的标志位置1,，计数器加一，直到对于每一个新的连通点,其周围的点（标志位置1的点也不算满足视差连通条件）都不满足视差连通条件。停止计数。
  • Step3：判断计数值（即和当前处理的视差点的连通区域的像素点个数）>speckleWindowSize?（注：speckleWindowSize是一个常数参数，可以自己设定。Opencv例程中speckleRange=100。）若大于，视差值认为有效，否则认为当前视差值是噪点。

• 连通区域检测有助于去除经LR和唯一性检测后残余的噪点，效果比较理想。

二、Opencv-SGBM算法的参数含义及数值选取

1、 预处理参数

• preFilterCap：水平sobel预处理后，映射滤波器大小。默认为15

  int ftzero =max(params.preFilterCap, 15) | 1;

  opencv测试例程test_stereomatching.cpp中取63。

2、代价参数

• SADWindowSize:计算代价步骤中SAD窗口的大小。由源码得，此窗口默认大小为5。

  SADWindowSize.width= SADWindowSize.height = params.SADWindowSize > 0 ?params.SADWindowSize : 5;

  注：窗口大小应为奇数，一般应在3x3到21x21之间。

• minDisparity：最小视差，默认为0。此参数决定左图中的像素点在右图匹配搜索的起点。int 类型

• numberOfDisparities：视差搜索范围，其值必须为16的整数倍（CV_Assert( D % 16 == 0 );）。最大搜索边界= numberOfDisparities+ minDisparity。int 类型

3、动态规划参数

• 动态规划有两个参数，分别是P1、P2，它们控制视差变化平滑性的参数：
  P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数，P2必须大于P1；需要指出，在动态规划时，P1和P2都是常数。

  opencv测试例程test_stereomatching.cpp中，P1 = 8cnsgbm.SADWindowSize*sgbm.SADWindowSize;

  opencv测试例程test_stereomatching.cpp中，P2 = 32cnsgbm.SADWindowSize*sgbm.SADWindowSize;

4、后处理参数

• uniquenessRatio：唯一性检测参数。对于左图匹配像素点来说，先定义在numberOfDisparities搜索区间内的最低代价为mincost，次低代价为secdmincost。如果满足

  即说明最低代价和次第代价相差太小，也就是匹配的区分度不够，就认为当前匹配像素点是误匹配的。

  opencv测试例程test_stereomatching.cpp中，uniquenessRatio=10。int 类型

• disp12MaxDiff：左右一致性检测最大容许误差阈值。int 类型

  opencv测试例程test_stereomatching.cpp中，disp12MaxDiff =1。

• speckleWindowSize：视差连通区域像素点个数的大小。对于每一个视差点，当其连通区域的像素点个数小于speckleWindowSize时，认为该视差值无效，是噪点。

  opencv测试例程test_stereomatching.cpp中，speckleWindowSize=100。

• speckleRange：视差连通条件，在计算一个视差点的连通区域时，当下一个像素点视差变化绝对值大于speckleRange就认为下一个视差像素点和当前视差像素点是不连通的。

  opencv测试例程test_stereomatching.cpp中，speckleWindowSize=10。

三、C++实现

#include <opencv2/opencv.hpp>  
#include <iostream>  
 
using namespace std;
using namespace cv;
 
const int imageWidth = 672;  //摄像头的分辨率  
const int imageHeight = 376;
Size imageSize = Size(imageWidth, imageHeight);
 
Mat grayImageL,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;    //是否选择对象
 
int numberOfDisparities = ((imageSize.width / 8) + 15) & -16;
int numDisparities = 6;
 
cv::Ptr<cv::StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
 
Mat cameraMatrixL = (Mat_<double>(3, 3) << 350.11095, 0, 339.81480,
	0, 349.39869, 200.42205,
	0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << -0.16028, 0.00600, -0.00009, -0.00047, 0.00000);
 
Mat cameraMatrixR = (Mat_<double>(3, 3) << 351.08207, 0, 343.68828,
	0, 350.19118, 210.18586,
	0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.17678, 0.02713, -0.00033, -0.00109, 0.00000);
//左右目之间的R,t可通过stereoCalibrate()或matlab工具箱calib求得
Mat T = (Mat_<double>(3, 1) << -119.61078, -0.06806, 0.08105);//T平移向量
Mat rec = (Mat_<double>(3, 1) << 0.00468, 0.02159, 0.00015);//rec旋转向量
Mat R;//R 旋转矩阵
Mat frame, f1, f2;
Mat disp, disp8;
 
/*****立体匹配*****/
void stereo_match(int, void*)
{
	sgbm->setPreFilterCap(32);
	int SADWindowSize = 9;
	int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
	sgbm->setBlockSize(sgbmWinSize);
	int cn = rectifyImageL.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);
	sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
	sgbm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
 
	disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
	reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	imshow("disparity", disp8);
}
 
/*****描述：鼠标操作回调*****/
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变换
	//经过双目标定得到摄像头的各项参数后，采用OpenCV中的stereoRectify(立体校正)得到校正旋转矩阵R、投影矩阵P、重投影矩阵Q
	//flags-可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
	//alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
	stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
		0, imageSize, &validROIL, &validROIR);
	//再采用映射变换计算函数initUndistortRectifyMap得出校准映射参数,该函数功能是计算畸变矫正和立体校正的映射变换
	initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
 
	VideoCapture cap(0);
	cap.set(CAP_PROP_FRAME_HEIGHT, 376);
	cap.set(CAP_PROP_FRAME_WIDTH, 1344);
	namedWindow("disparity", CV_WINDOW_AUTOSIZE);
	namedWindow("paramemnt", CV_WINDOW_NORMAL);
	createTrackbar("numDisparities:\n", "paramemnt", &numDisparities, 20, stereo_match);
	setMouseCallback("disparity", onMouse, 0);
	
	while (1)
	{
		cap >> frame;
		imshow("video", frame);
		f1 = frame.colRange(0, 672);
		f2 = frame.colRange(672, 1344);
 
		cvtColor(f1, grayImageL, CV_BGR2GRAY);
		cvtColor(f2, grayImageR, CV_BGR2GRAY);
		//然后用remap来校准输入的左右图像
		//interpolation-插值方法，但是不支持最近邻插值
		remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
		remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
 
		stereo_match(0, 0);
		waitKey(1);
	}
	waitKey(0);
	return 0;
}