双目立体匹配算法SGBM

semi-global matching（SGM）是一种用于计算双目视觉中视差（disparity）的半全局匹配算法，在OpenCV中的实现为semi-global block matching（SGBM）。

第一部分：SGBM算法原理：

一、预处理

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。

预处理实际上是得到图像的梯度信息，将预处理的图像保存起来，将会用于计算代价。

二、代价计算

代价有两部分组成：

1、经过预处理得到的图像的梯度信息经过基于采样的方法得到的梯度代价。

2、原图像经过基于采样的方法得到的SAD代价。

上述两个代价都会在SAD窗口内进行计算。

三、动态规划

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

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

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

式中L为当前路径累积的代价函数，P1、P2为像素点与相邻点视差存在较小和较大差异情况下的平滑惩罚，P1

默认4条路径，其中动态规划很重要两个参数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，可以自己设置。

OpencvSGBM计算右视差图的方式：

通过得到的左视察图计算右视差图

 

 

第二部分：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之间。

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

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

三 、动态规划参数

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

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

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

四、后处理参数

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

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

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

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

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

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

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

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

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

 

第三部分：C++实现

#include   
#include   

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 sgbm = StereoSGBM::create(0, 16, 3);

Mat cameraMatrixL = (Mat_(3, 3) << 350.11095, 0, 339.81480,
	0, 349.39869, 200.42205,
	0, 0, 1);
Mat distCoeffL = (Mat_(5, 1) << -0.16028, 0.00600, -0.00009, -0.00047, 0.00000);

Mat cameraMatrixR = (Mat_(3, 3) << 351.08207, 0, 343.68828,
	0, 350.19118, 210.18586,
	0, 0, 1);
Mat distCoeffR = (Mat_(5, 1) << -0.17678, 0.02713, -0.00033, -0.00109, 0.00000);
//左右目之间的R,t可通过stereoCalibrate()或matlab工具箱calib求得
Mat T = (Mat_(3, 1) << -119.61078, -0.06806, 0.08105);//T平移向量
Mat rec = (Mat_(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(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;
}

运行效果：

 

参考：

Accurate and Efficient Stereo Processing by Semi-Global Matching and Mutual Information

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

semi-global matching 算法总结

密集匹配之半全局匹配SGBM

【OpenCV】双目测距（双目标定、双目校正和立体匹配）

双摄像头立体成像(三)-畸变矫正与立体校正

双目立体匹配经典算法之Semi-Global Matching（SGM）概述：匹配代价计算之Census变换（Census Transform，CT）（附计算C代码）