局部立体匹配算法

局部立体匹配

一、什么是局部匹配算法？优势如何？

   局部(Local)立体匹配是相对于半全局以及全局(Non-Local)立体匹配算法而言的，它不构建能量函数，而是利用某种代价函数(或称做相似性度量)，仅仅通过比较左右视图中相同大小的图像块来确定视差，它的基本流程一般为代价计算、代价聚合、视差计算、视差细化。虽然非局部立体匹配算法在性能上可能优于局部算法，但是它们也有很多难点，并非是所有情况下的最好选择，例如：

   全局算法或半全局算法由于需要相当多的计算量，因此运算耗时可能很长，特别是对于高分辨率的图像如1080x720、1920x1080等，即使使用硬件加速也难以做到实时。然而局部算法占用计算资源很少，同时运算效率很高，利用滑动窗口的数据复用策略再结合并行编程，完全可以做到实时，若能实现到硬件上则更能发挥其无可比拟的速度优势，特别适合算力较低的计算平台。

   对于包含结构光发射器的双目或单目传感器，全局匹配算法的和局部匹配算法的匹配效果相比，差距可能并不大，在对视差图的精度和完整性没有太高要求的情况下，这时选择具有更高运算效率的局部匹配算法可能会是一种更好的选择。

二、代价函数

   常用来做匹配代价的方法有以下几种：SAD/SSD/ZSAD/BT、NCC/ZNCC、Census/StarCensus、rank等，关于这些代价方法的原理这里就不详细描述了。下面通过OpenCV来实现局部立体匹配算法，为了简化代码的复杂性，代价聚合部分会略去不写，视差细化部分仅包括亚像素插值、唯一性检测、左右一致性检测和中值滤波。算法假定输入图像都是经过极线校正的。

三、代码实现

下面我们把代码放在这里供大家参考，其中核心的匹配部分代码也就100行左右。完整代码可以到这里来下载：
https://download.csdn.net/download/dulingwen/83023125?spm=1001.2014.3001.5503

1.头文件LocalStereoMatch.h

#include <opencv2/opencv.hpp>

typedef char int8;
typedef unsigned char uint8;
typedef short int16;
typedef int int32;
typedef float float32;

enum
{
	LSM_SAD = 1,
	LSM_NCC = 2,
	LSM_CENSUS = 3,
	LSM_RANK = 4,
};

enum
{
	LSM_LINEAR = 1,
	LSM_PARABOLA = 2,
	LSM_EQUALIZE = 3,
};

/**
* @brief    局部立体匹配算法
* @param           left                左图像
* @param           right               右图像
* @param[out]      disp                视差图
* @param           window_size         匹配窗口尺寸
* @param           max_disparities     最大搜索视差
* @param           mode                匹配代价类型
* @param           ip_mode             亚像素插值方式：线性插值、抛物线插值、直方图均衡化的线性插值
* @param           uinqueness_ratio    唯一性检测比率
* @param           lrc_threshold       左右一致性检测阈值
*/
void LocalStereoMatching(const cv::Mat& left, const cv::Mat& right, cv::Mat& disp, 
int window_size = 11, int max_disparities = 64, int mode = LSM_SAD, int ip_mode = LSM_PARABOLA,int uinqueness_ratio = 5, int lrc_threshold = 1);

2.源文件LocalStereoMatch.cpp

#include "LocalStereoMatch.h"


// ----------------------------------------------------------------------------------------------------------------------------------
template<typename T>
inline float32 parabola_ip(T c1, T c2, T c, float32 d)
{
	float32 denom = std::max((float32)1, (float32)(c1 - c + c2 - c));
	float32 diff = (float32)(c1 - c2) / (denom * 2.f);
	float32 ds = d + diff;
	return ds;
}

template<typename T>
inline float32 linear_ip(T c1, T c2, T c, float32 d)
{
	float32 ldiff = c1 - c;
	float32 rdiff = c2 - c;
	float32 diff = 0;
	if (ldiff <= rdiff)
		diff = -0.5f + 0.5f * (ldiff / rdiff);
	else
		diff = 0.5f - 0.5f * (rdiff / ldiff);

	float32 ds = d + diff;
	return ds;
}

template<typename T>
inline float32 equiangular_equalize_ip(T c1, T c2, T c, float32 d)
{
	float32 ldiff = c1 - c;
	float32 rdiff = c2 - c;
	float32 diff = 0;
	if (ldiff <= rdiff)
	{
		float32 x = ldiff / rdiff;
		diff = -0.5f + 0.25f * (x * x + x);
	}
	else
	{
		float32 x = rdiff / ldiff;
		diff = 0.5f - 0.25f * (x * x + x);
	}

	float32 ds = d + diff;
	return ds;
}
// ----------------------------------------------------------------------------------------------------------------------------------


// ----------------------------------------------------------------------------------------------------------------------------------
static int32 compute_cost_sad(const uint8* lptr, const uint8* rptr, int x, int y, int w, int rad, int d)
{
	int32 sad = 0;
	for (int i = x - rad; i < x + rad; i++)
	{
		for (int j = y - rad; j < y + rad; j++)
		{
			uint8 val1 = lptr[i * w + j];
			uint8 val2 = rptr[i * w + std::max(j - d, 0)];
			sad += std::abs(val1 - val2);
		}
	}
	return sad;
}

static float32 compute_cost_ncc(const uint8* lptr, const uint8* rptr, int x, int y, int w, int rad, int d)
{
	int32 a = 0, b = 0, c = 0;
	for (int i = x - rad; i < x + rad; i++)
	{
		for (int j = y - rad; j < y + rad; j++)
		{
			uint8 val1 = lptr[i * w + j];
			uint8 val2 = rptr[i * w + std::max(j - d, 0)];
			a += val1 * val1;
			b += val2 * val2;
			c += val1 * val2;
		}
	}
	float32 ncc = FLT_MAX;
	if (c != 0) {
		ncc = (float32)(std::sqrt(a) * std::sqrt(b)) / (float32)c;
	}

	return ncc;
}

static int16 compute_cost_census(const uint8* lptr, const uint8* rptr, int x, int y, int w, int rad, int d)
{
	int16 hamming = 0;
	const uint8& cval1 = lptr[x * w + y];
	const uint8& cval2 = rptr[x * w + std::max(y - d, 0)];

	for (int i = x - rad; i < x + rad; i++)
	{
		for (int j = y - rad; j < y + rad; j++)
		{
			if (i == x && j == y) continue;
			uint8 val1 = lptr[i * w + j];
			uint8 val2 = rptr[i * w + std::max(j - d, 0)];
			if (val1 > cval1 && val2 <= cval2)
				hamming += 1;
			else if (val1 <= cval1 && val2 > cval2)
				hamming += 1;
		}
	}
	return hamming;
}

static int16 compute_cost_rank(const uint8* lptr, const uint8* rptr, int x, int y, int w, int rad, int d)
{
	int16 F = 0;
	const uint8& cval1 = lptr[x * w + y];
	const uint8& cval2 = rptr[x * w + std::max(y - d, 0)];
	int16 u = 2;
	int16 v = 9;
	int16 iu = -u;
	int16 iv = -v;
	int8 rank1 = 0;
	int8 rank2 = 0;

	for (int i = x - rad; i < x + rad; i++)
	{
		for (int j = y - rad; j < y + rad; j++)
		{
			int16 val1 = lptr[i * w + j];
			int16 val2 = rptr[i * w + std::max(j - d, 0)];
			int16 diff1 = val1 - cval1;
			int16 diff2 = val2 - cval2;

			if (diff1 < iv) rank1 = -2;
			else if (diff1 >= iv && diff1 < iu) rank1 = -1;
			else if (diff1 >= iu && diff1 <= u) rank1 = 0;
			else if (diff1 > u && diff1 <= v) rank1 = 1;
			else if (diff1 > v) rank1 = 2;

			if (diff2 < iv) rank2 = -2;
			else if (diff2 >= iv && diff2 < iu) rank2 = -1;
			else if (diff2 >= iu && diff2 <= u) rank2 = 0;
			else if (diff2 > u && diff2 <= v) rank2 = 1;
			else if (diff2 > v) rank2 = 2;

			F += std::abs(rank1 - rank2);
		}
	}
	return F;
}
// ----------------------------------------------------------------------------------------------------------------------------------

// ----------------------------------------------------------------------------------------------------------------------------------
template <typename CostType>
void pseudo_lrc_check(CostType* cost_mat, float32* disp, const int& w, const int& h, const double& lrc_thr, float32 invalid_disp)
{
	if (!cost_mat || lrc_thr <= 0)
		return;

	CostType* disp2cost = (CostType*)malloc(w * sizeof(CostType));
	float32* disp2buf = (float32*)malloc(w * sizeof(float32));
	if (!disp2cost || !disp2buf)
		return;

	CostType max_cost = (CostType)0;
	if (std::is_same<CostType, float32>::value) 
		max_cost = FLT_MAX;
	else if (std::is_same<CostType, int32>::value) 
		max_cost = INT_MAX;
	else if (std::is_same<CostType, int16>::value) 
		max_cost = SHRT_MAX;

	int y = 0;
	for (int x = 0; x < h; x++)
	{
		float32* dptr = disp + x * w;

		for (y = 0; y < w; y++)
		{
			disp2buf[y] = invalid_disp;
			disp2cost[y] = max_cost;
		}

		const CostType* cptr = cost_mat + x * w;
		for (y = 0; y < w; y++)
		{
			float32 d = dptr[y];
			CostType c = cptr[y];
			if (d == invalid_disp)
				continue;

			int y2 = (int)(y - d);
			if (y2 < 0)
				continue;
			if (disp2cost[y2] > c)
			{
				disp2cost[y2] = c;
				disp2buf[y2] = d;
			}
		}

		for (y = 0; y < w; y++)
		{
			float32 d0 = dptr[y];
			if (d0 == invalid_disp)
				continue;
			float32 d1 = d0 + 1.f;
			int y0 = (int)(y - d0);
			int y1 = (int)(y - d1);
			if ((0 <= y0 && y0 < w && disp2buf[y0] > invalid_disp && std::fabs(disp2buf[y0] - d0) > lrc_thr) &&
				(0 <= y1 && y1 < w && disp2buf[y1] > invalid_disp && std::fabs(disp2buf[y1] - d0) > lrc_thr))
				dptr[y] = invalid_disp;

		}
	}

	free(disp2cost);
	free(disp2buf);
}

// ----------------------------------------------------------------------------------------------------------------------------------


// ----------------------------------------------------------------------------------------------------------------------------------

template<typename CostType>
void matchImpl(const uint8* lptr, const uint8* rptr, float32* dptr, const int& w, const int& h, int& ws, int& ndisp, int mode, int ip_mode, int uniq, int lrc_thr)
{
	int num = ws * ws;
	int rad = ws / 2;
	CostType* cost0 = (CostType*)malloc((ndisp + 2) * sizeof(CostType));
	memset(cost0, 0, (ndisp + 2) * sizeof(CostType));
	CostType* cost = cost0 + 1;
	if (std::is_same<CostType, float32>::value)
		uniq = 0;  // ncc代价不是在任何情况下都适合用下面的唯一性检测，故设为0
	float32 invalid_disp = -1.f;

	CostType* cost_mat = nullptr;
	if (lrc_thr > 0)
	{
		cost_mat = (CostType*)malloc(w * h * sizeof(CostType));
	}

	for (int i = rad; i < h - rad; i++)
	{
		for (int j = rad; j < w - rad; j++)
		{
			for (int k = 0; k < ndisp + 2; k++)
			{
				cost0[k] = (CostType)0;
			}

			// 代价计算
			for (int d = 0; d < ndisp; d++)
			{
				if (mode == 1)
					cost[d] = compute_cost_sad(lptr, rptr, i, j, w, rad, d);
				if (mode == 2)
					cost[d] = compute_cost_ncc(lptr, rptr, i, j, w, rad, d);
				if (mode == 3)
					cost[d] = compute_cost_census(lptr, rptr, i, j, w, rad, d);
				if (mode == 4)
					cost[d] = compute_cost_rank(lptr, rptr, i, j, w, rad, d);
			}


			// 寻找最优视差
			CostType mincost = 0;
			int32 bestd = 0;
			if (std::is_same<CostType, float32>::value) mincost = FLT_MAX;
			else if (std::is_same<CostType, int32>::value) mincost = INT_MAX;
			else if (std::is_same<CostType, int16>::value) mincost = SHRT_MAX;

			for (int d = 0; d < ndisp; d++)
			{
				const CostType& currcost = cost[d];
				if (currcost < mincost)
				{
					mincost = currcost;
					bestd = d;
				}
			}

			if (cost_mat)
				cost_mat[i * w + j] = mincost;

			// 唯一性检测
			if (uniq > 0)
			{
				CostType thresh = mincost + mincost * ((CostType)(uniq)) / ((CostType)(100));
				int d = 0;
				for (d = 0; d < ndisp; d++)
				{
					const auto& costp = cost[d];
					if ((d < bestd - 1 || d > bestd + 1) && costp <= thresh)
						break;
				}
				if (d < ndisp)
				{
					dptr[i * w + j] = invalid_disp;
					continue;
				}
			}

			// 亚像素插值
			const CostType cost1 = cost[bestd - 1];
			const CostType cost2 = cost[bestd + 1];
			if (((bestd - 1) == -1) || ((bestd + 1) == ndisp))
			{
				dptr[i * w + j] = (float32)bestd;
				continue;
			}
			if (ip_mode == 1)
				dptr[i * w + j] = parabola_ip<CostType>(cost1, cost2, mincost, bestd);
			else if (ip_mode == 2)
				dptr[i * w + j] = linear_ip<CostType>(cost1, cost2, mincost, bestd);
			else if (ip_mode == 3)
				dptr[i * w + j] = equiangular_equalize_ip<CostType>(cost1, cost2, mincost, bestd);
			else
				dptr[i * w + j] = (float32)bestd;

		}
	}

	// 伪左右一致性检测
	pseudo_lrc_check(cost_mat, dptr, w, h, lrc_thr, invalid_disp);

	if (cost_mat)
		free(cost_mat);
	cost_mat = nullptr;
}


void LocalStereoMatching(const cv::Mat& left, const cv::Mat& right, cv::Mat& disp, int window_size, int max_disparities, 
	int mode, int ip_mode, int uinqueness_ratio, int lrc_threshold)
{
	assert(!left.empty());
	assert(!right.empty());
	assert(left.size() == right.size());

	const int width = left.cols;
	const int height = left.rows;
	int ws = window_size;
	int rad = ws / 2;
	int num = ws * ws;
	int ndisp = max_disparities;
	const uint8* lptr = left.data;
	const uint8* rptr = right.data;

	disp.release();
	disp.create(cv::Size(width, height), CV_32FC1);
	float32* dptr = disp.ptr<float32>();

	if (mode == 1 /*sad*/)
		matchImpl<int32>(lptr, rptr, dptr, width, height, ws, ndisp, mode, ip_mode, uinqueness_ratio, lrc_threshold);
	else if (mode == 2 /*ncc*/)
		matchImpl<float32>(lptr, rptr, dptr, width, height, ws, ndisp, mode, ip_mode, uinqueness_ratio, lrc_threshold);
	else if (mode == 3 /*census*/)
		matchImpl<int16>(lptr, rptr, dptr, width, height, ws, ndisp, mode, ip_mode, uinqueness_ratio, lrc_threshold);
	else if (mode == 4 /*rank*/)
		matchImpl<int16>(lptr, rptr, dptr, width, height, ws, ndisp, mode, ip_mode, uinqueness_ratio, lrc_threshold);
	else
	{
		printf("Error: unsupported alogrithm.\n");
		return;
	}

	cv::medianBlur(disp, disp, 3);
}
// ----------------------------------------------------------------------------------------------------------------------------------

四、结果及运算效率分析

   测试图像采用了MiddleBurry网站的双目图像，各个匹配算法采用了统一的参数设置：window_size = 17, max_disparities = 128~145, uniqueness_ratio = 5, lrc_threshold = 1，结果如下图，从左到右使用的匹配算法依次为SAD、NCC、Census、Rank。从中可以看出NCC的匹配效果是最好的，具有良好的抗噪性能，生成的视差图完整度最高，但同时噪声也最多，SAD次之，然后是rank和census。其中SAD的运算量最低，因此运算速度最快，只需十几秒便可以计算一张图像。

五、如何提升局部立体匹配算法性能？

  匹配代价至关重要，最常用的改善方式是使用自适应权重对匹配代价进行加权，这通常适用于pixel-wise的匹配代价。还有的是使用多个不同的窗口尺寸进行匹配，从中选择置信度最高的结果作为最终结果，但这种方法的计算量会比较高，实际使用起来并不划算。随着深度学习的发展，深度学习主导的特征提取方式大大超越了传统的手工特征设计，因此一些基于卷积神经网络的度量学习开始用来替代传统的匹配代价，比较典型的有MC-CNN、HashMatch等。

  代价聚合能够很好的提升匹配精度与鲁棒性，至今为止已发展出多种代价聚合方式：普通的邻域求和、基于引导滤波的代价聚合、基于双边滤波的代价聚合、基于十字交叉臂的代价聚合以及跨尺度代价聚合等等，另外SGM中的动态规划部分通常也被认为是一种代价聚合方式。

觉得我的博客写的不错的记得点个赞哦！