关于双目测距中立体匹配算法的研究(一)

1.概述

目前常用的立体匹配算法有BM（blocking match块匹配），SGBM（semi-global BM），GC（graph cut图割）；传统的立体匹配算法中，全局匹配算法都无法满足实时性的要求，因此此处只对BM和SGBM这种局部匹配法进行分析和优化，GC是全局匹配不做介绍。

2.BM算法

2.1 BM算法的原理

BM算法是一种基于SAD窗的局部匹配算法，思想是以左目图像的源匹配点为中心，定义一个窗口D，其大小为（2m+1X2n+1)，统计其窗口的灰度值的和，然后在右目图像中逐步计算其左右窗口的灰度和的差值，最后搜索到的差值最小的区域的中心像素即为匹配点。基于SAD窗口的BM算法的处理速度很快，理论上一副320X240的灰度图匹配时间为30ms。实验使用640X480的镜头，测算出平均耗时约为90ms。但是这种算法只能用于处理简单场景，对于稍微复杂的场景会存在边缘匹配关键点缺失的信息丢失问题，严重影响测距性能。

2.2 BM算法的代码实现

2.2.1 BM算法在OpenCV中的源码分析

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

using namespace std;
using namespace cv;

Mat ImageL, ImageR;

Ptr<StereoBM> bm = StereoBM::create(32, 15);

int main()
{
	long  start = GetTickCount();
	ImageL = imread("ll.jpg",0);
	ImageR = imread("rr.jpg",0);

	bm->setBlockSize(15); 
	bm->setPreFilterCap(31);
	bm->setMinDisparity(0);  
	bm->setNumDisparities(32);
	bm->setTextureThreshold(10);
	bm->setUniquenessRatio(10);
	bm->setSpeckleWindowSize(100);
	bm->setSpeckleRange(32);
	bm->setDisp12MaxDiff(-1);

	Mat disp, disp8;
	bm->compute(ImageL, ImageR, disp);
	disp.convertTo(disp8, CV_8U, 255 / ((32)*16.));
	long stop = GetTickCount();
	cout << (stop - start) << "ms" << endl;
	imshow("disparity", disp8);
	
	waitKey(0);
	return 0;
}

下面依次给出视差图和ll.jpg以及rr,jpg;可以看出效果一般，基本不实用。同时可以看到除了轮廓信息丢失之外，左侧有一条明显的黑色部分：信息完全丢失。这是一个明显的错误，接下来我们分析一下产生这个黑条的原因。

这里给出OpenCV中stereoBM算法的源码的位置：D:\opencv\sources\modules\calib3d\src（依据个人安装路径查找，代码太多不便粘贴展示）
BM算法一共1278行，看起来蛮吓人的，但是仔细一分析其实没有多么恐怖，算法主要由以下部分组成：

1. 定义结构体StereoBMParams；
2. 使用opencl和不适用opencl情况下prefilterNorm函数的实现，这个函数的功能是对图片预处理；
3. 使用opencl和不适用opencl情况下prefilterXSobel函数的实现，这个函数的功能是对图片预处理；
4. 在CPU支持CV_SSE2指令集优化的情况下，findStereoCorrespondenceBM_SSE2的函数实现；
5. 在CPU不支持CV_SSE2指令集优化的情况下，findStereoCorrespondenceBM的函数实现；
6. 构造循环体进行并行计算PrefilterInvoker；
7. 定义StereoBM的子类StereoBMImpl；
8. 在子类StereoBMImpl类里实现所有需要调用的函数，如compute，setMinDisparity，setPreFilterCap等等。

2.2.2 上一节中的代码的具体实现

接下来我们研究一下本文给出的代码的运行过程：
1.创建对象：Ptr<StereoBM> bm = StereoBM::create(32, 15);

首先创建一个智能指针指向StereoBM的bm对象，这里调用了create函数，这个函数会调用下面的代码：

Ptr<StereoBM> StereoBM::create(int _numDisparities, int _SADWindowSize)
{
    return makePtr<StereoBMImpl>(_numDisparities, _SADWindowSize);
}

关于makePtr函数的定义（创建智能指针的基本原理和智能指针的优点不再赘述）

template<typename T, typename A1, typename A2>
Ptr<T> makePtr(const A1& a1, const A2& a2)
{
    return Ptr<T>(new T(a1, a2));
}

因此creat函数最后返回的结果为
Ptr<StereoBMImpl> bm指向StereoBMImpl的指针；StereoBMImpl是StereoBM的子类，因此bm是父类指针指向了子类对象。
2. 读取图片设置参数；
3. 调用compute函数：bm->compute(ImageL, ImageR, disp);
函数定义为：void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr )，输入的形参为左图，右图和用来保存输出的视差图。

void compute(InputArray leftarr, InputArray rightarr, OutputArray disparr)
{
	int dtype = disparr.fixedType() ? disparr.type() :params.dispType;
	Size leftsize = leftarr.size();

	if (leftarr.size() != rightarr.size())
		CV_Error(Error::StsUnmatchedSizes, "All the images must have the same size");

	if (leftarr.type() != CV_8UC1 || rightarr.type() != CV_8UC1)
		CV_Error(Error::StsUnsupportedFormat, "Both input images must have CV_8UC1");

	if (dtype != CV_16SC1 && dtype != CV_32FC1)
		CV_Error(Error::StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format");

	if (params.preFilterType != PREFILTER_NORMALIZED_RESPONSE &&
		params.preFilterType != PREFILTER_XSOBEL)
		CV_Error(Error::StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE");

	if (params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0)
		CV_Error(Error::StsOutOfRange, "preFilterSize must be odd and be within 5..255");

	if (params.preFilterCap < 1 || params.preFilterCap > 63)
		CV_Error(Error::StsOutOfRange, "preFilterCap must be within 1..63");

	if (params.SADWindowSize < 5 || params.SADWindowSize > 255 || params.SADWindowSize % 2 == 0 ||
		params.SADWindowSize >= (min(leftsize.width, leftsize.height)))
		CV_Error(Error::StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height");

	if (params.numDisparities <= 0 || params.numDisparities % 16 != 0)
		CV_Error(Error::StsOutOfRange, "numDisparities must be positive and divisble by 16");

	if (params.textureThreshold < 0)
		CV_Error(Error::StsOutOfRange, "texture threshold must be non-negative");

	if (params.uniquenessRatio < 0)
		CV_Error(Error::StsOutOfRange, "uniqueness ratio must be non-negative");

	int FILTERED = (params.minDisparity - 1) <<4;
	//to be continued

进入compute函数，计算dtype为CV_16S即3，左图size为384*288；并判断与右图是否相同；
接下来判断左图和右图是否都是CV_8UC1类型等等一系列判断，满足所有条件后继续；

Mat left0 = leftarr.getMat(), right0 = rightarr.getMat();//将左右两幅InputArray类型的图像转化成Mat类型
	disparr.create(left0.size(), dtype);
	Mat disp0 = disparr.getMat();

	preFilteredImg0.create(left0.size(), CV_8U);//创建一个和left0一样大的CV_8U类型的图像矩阵的矩阵体
	preFilteredImg1.create(left0.size(), CV_8U);//创建一个和left0一样大的CV_8U类型的图像矩阵的矩阵体
	cost.create(left0.size(), CV_16S);//创建一个和left0一样大的CV_16S类型的图像矩阵的矩阵体

	Mat left = preFilteredImg0, right = preFilteredImg1;

	int mindisp = params.minDisparity;//最小视差，默认值为0, 可以是负值，int型
	int ndisp = params.numDisparities;//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型

	int width = left0.cols;//左图的宽度
	int height = left0.rows;//左图的高度
	int lofs = max(ndisp - 1 + mindisp, 0);//找最大视差减一和0的最大值
	int rofs = -min(ndisp - 1 + mindisp, 0);//找最大视差减一和0的最小值
	int width1 = width - rofs - ndisp + 1;//最终生成的视差图的有效宽度，因为左图最左侧的点在右图中没有对应点；故视差图中没有这一部分

	if (lofs >= width || rofs >= width || width1 < 1)//特殊情况，如果lofs或者rofs大于原图宽度或者视差图有信息的部分宽度小于1的情况
	{
		disp0 = Scalar::all(FILTERED * (disp0.type() < CV_32F ? 1 : 1. / (1 << 4)));
		return;
	}

	Mat disp = disp0;//定义disp
	if (dtype == CV_32F)//此处类型为CV_16S，如果是32F则在此处更改为16S
	{
		dispbuf.create(disp0.size(), CV_16S);
		disp = dispbuf;
	}

	int wsz = params.SADWindowSize;//SAD窗口大小
	int bufSize0 = (int)((ndisp + 2) * sizeof(int));
	bufSize0 += (int)((height + wsz + 2)*ndisp * sizeof(int));
	bufSize0 += (int)((height + wsz + 2) * sizeof(int));
	bufSize0 += (int)((height + wsz + 2)*ndisp*(wsz + 2) * sizeof(uchar) + 256);

	int bufSize1 = (int)((width + params.preFilterSize + 2) * sizeof(int) + 256);
	int bufSize2 = 0;
	if (params.speckleRange >= 0 && params.speckleWindowSize > 0)
		bufSize2 = width*height*(sizeof(Point_<short>) + sizeof(int) + sizeof(uchar));
	//to be continued

这部分首先用getMat()函数将左右两幅InputArray类型的图像转化成Mat类型；同时disp0也是CV_16S类型的Mat图，然后preFilteredImg0创建一个和left0一样大的CV_8U类型的图像矩阵的矩阵体，preFilteredImg1与之相同；cost则是和left0一样大的CV_16S类型的图像矩阵的矩阵体。定义left和right图像，分别为
preFilteredImg0和preFilteredImg1；
width1用来表示最终生成的视差图的有效宽度，因为左图最左侧的点在右图中没有对应点；故视差图中没有这一部分，这也解释了之前我们的疑问，为什么最终的视差图左侧有一条明显的额黑色的信息缺失；这是必然出现的，而且这一部分的宽度随着视差窗口的大小变化，视差窗口越大，则黑带越宽。
定义wsz为SAD窗口大小；