【码上实战】【立体匹配系列】经典AD-Census: （6）多步骤视差优化

同学们好久不见！

下载完整源码，点击进入: https://github.com/ethan-li-coding/AD-Census
欢迎同学们在Github项目里讨论！

在实战的上一篇，我们对AD-Census的扫描线优化步骤的实战代码进行了介绍：

【码上实战】【立体匹配系列】经典AD-Census: （5）扫描线优化

通过扫描线优化，视差图效果基本定型，我们来回顾一下：

代价计算

代价计算

代价聚合

扫描线优化

熟悉立体匹配同学们都应该知道，后续我们一般都会做一个视差优化，以剔除错误视差以及视差填充等。AD-Census也不例外，它总共进行了4个小步骤来完成视差优化的目的，他们分别是：

1. Outlier Detection离群点检测
2. Iterative Region Voting迭代局部投票
3. Proper Interpolation
4. Depth Discontinuity Adjustment视差非连续区调整

理论部分我们就不再赘述了，大家请查看往期博文：

【理论恒叨】【立体匹配系列】经典AD-Census: （4）多步骤视差优化

本篇的内容是紧跟代码，做好代码讲解。大家往下看。

代码实现

类设计

成员函数

我们来用一个多步骤优化器类MultiStepRefiner来实现该功能。类的实现放在文件multistep_refiner.h/multistep_refiner.cpp中。

/**
 * \brief 多步骤优化器
 */
class MultiStepRefiner
{
    
    
public:
	MultiStepRefiner();
	~MultiStepRefiner();
}

为了完成算法运算，我们需要分配一些内存来存储数据或结果，Initialize成员函数可以帮助完成这件事：

/**
* \brief 初始化
 * \param width		影像宽
 * \param height	影像高
 * \return true:初始化成功
 */
bool Initialize(const sint32& width, const sint32& height);

算法需要数据输入以及参数设计，所以我们设计成员函数SetData和SetParam来达到这个目的：

/**
 * \brief 设置多步优化的数据
 * \param img_left			// 左影像数据，三通道
 * \param cost				// 代价数据
 * \param cross_arms		// 十字交叉臂数据
 * \param disp_left			// 左视图视差数据
 * \param disp_right		// 右视图视差数据
 */
void SetData(const uint8* img_left, float32* cost,const CrossArm* cross_arms, float32* disp_left, float32* disp_right);


/**
 * \brief 设置多步优化的参数
 * \param min_disparity					// 最小视差
 * \param max_disparity					// 最大视差
 * \param irv_ts						// Iterative Region Voting参数ts
 * \param irv_th						// Iterative Region Voting参数th
 * \param lrcheck_thres					// 一致性检查阈值
 * \param do_lr_check					// 是否检查左右一致性
 * \param do_region_voting				// 是否做内插填充
 * \param do_interpolating				// 是否局部投票填充
 * \param do_discontinuity_adjustment	// 是否做非连续区调整
 */
void SetParam(const sint32& min_disparity, const sint32& max_disparity, const sint32& irv_ts, const float32& irv_th, const float32& lrcheck_thres,
			  const bool&	do_lr_check, const bool& do_region_voting, const bool& do_interpolating, const bool& do_discontinuity_adjustment);

最后，需要一个主函数来执行优化：

/** \brief 多步视差优化 */
void Refine();

以上便是类的公有函数列表，可以通过调用以上函数完成整个优化操作。

而我们知道Refine其实包含有四个小步骤，每个步骤都是一个独立的算法模块，所以我们来设计四个私有成员函数来分别实现这四个功能。

//------4小步视差优化------//
/** \brief 离群点检测 */
void OutlierDetection();
/** \brief 迭代局部投票 */
void IterativeRegionVoting();
/** \brief 内插填充 */
void ProperInterpolation();
/** \brief 深度非连续区视差调整 */
void DepthDiscontinuityAdjustment();

最后是类的一些成员变量，图像数据、代价数据、视差数据、算法参数等，不必一一解释。

/** \brief 图像尺寸 */
sint32	width_;
sint32	height_;

/** \brief 左影像数据（三通道） */
const uint8* img_left_;

/** \brief 代价数据 */
float32* cost_;
/** \brief 交叉臂数据 */
const CrossArm* cross_arms_;

/** \brief 左视图视差数据 */
float* disp_left_;
/** \brief 右视图视差数据 */
float* disp_right_;

/** \brief 左视图边缘数据 */
vector<uint8> vec_edge_left_;

/** \brief 最小视差值 */
sint32 min_disparity_;
/** \brief 最大视差值 */
sint32 max_disparity_;

/** \brief Iterative Region Voting参数ts */
sint32	irv_ts_;
/** \brief Iterative Region Voting参数th */
float32 irv_th_;


float32 lrcheck_thres_;

/** \brief 是否检查左右一致性 */
bool	do_lr_check_;				
/** \brief 是否局部投票填充 */
bool	do_region_voting_;
/** \brief 是否做内插填充 */
bool	do_interpolating_;
/** \brief 是否做非连续区调整 */
bool	do_discontinuity_adjustment_;

/** \brief 遮挡区像素集	*/
vector<pair<int, int>> occlusions_;
/** \brief 误匹配区像素集	*/
vector<pair<int, int>> mismatches_;

类实现

首先我们来看，三个非算法功能性函数Initialize、SetData、SetParam，它们代码都比较简单，都是为算法实现做准备的。

首先是Initialize，代码如下，只做了一个边缘数据的初始化，vec_edge_left_是和图像尺寸一样的数组，它保存着图像所有像素的边缘值，作用是在视差非连续调整步骤的时候，提供边缘信息。

bool MultiStepRefiner::Initialize(const sint32& width, const sint32& height)
{
    
    
	width_ = width;
	height_ = height;
	if (width_ <= 0 || height_ <= 0) {
    
    
		return false;
	}

	// 初始化边缘数据
	vec_edge_left_.clear();
	vec_edge_left_.resize(width*height);
	
	return true;
}

其次是SetData，传入影像数据、代价数据、十字交叉臂数据、左右视差图。

void MultiStepRefiner::SetData(const uint8* img_left, float32* cost,const CrossArm* cross_arms, float32* disp_left, float32* disp_right)
{
    
    
	img_left_ = img_left;
	cost_ = cost; 
	cross_arms_ = cross_arms;
	disp_left_ = disp_left;
	disp_right_= disp_right;
}

再就是SetParam，对算法的所有参数进行赋值。

void MultiStepRefiner::SetParam(const sint32& min_disparity, const sint32& max_disparity, const sint32& irv_ts, const float32& irv_th, const float32& lrcheck_thres,
								const bool& do_lr_check, const bool& do_region_voting, const bool& do_interpolating, const bool& do_discontinuity_adjustment)
{
    
    
	min_disparity_ = min_disparity;
	max_disparity_ = max_disparity;
	irv_ts_ = irv_ts;
	irv_th_ = irv_th;
	lrcheck_thres_ = lrcheck_thres;
	do_lr_check_ = do_lr_check;
	do_region_voting_ = do_region_voting;
	do_interpolating_ = do_interpolating;
	do_discontinuity_adjustment_ = do_discontinuity_adjustment;
}

当算法数据和参数都赋值完毕，接下来便是各个优化子步骤的实现了。我们一个个来讲解。

1. Outlier Detection离群点检测

离群点检测听上去似乎与众不同，但实际它完完全全就是一个左右一致性检查，关于左右一致性检查前面已经说得很多了，实在不是一个陌生的词儿。

在离群点检测的过程中，我们会对离群点进行遮挡点和非遮挡的的分类。这和SGM的思路是一模一样的，遮挡区和非遮挡区的判别方法为：

（1）$p$的视差值和周围的背景像素视差值比较接近。
（2）$p$因为遮挡而在右影像上不可见，所以它会匹配到右影像上的前景像素，而前景像素的视差值必定比背景像素大，即比$p$的视差大。

这里更详细的解释，请查看博文：

【码上实战】【立体匹配系列】经典SGM：（6）视差填充

我们来看代码：

void MultiStepRefiner::OutlierDetection()
{
    
    
	const sint32 width = width_;
	const sint32 height = height_;

	const float32& threshold = lrcheck_thres_;

	// 遮挡区像素和误匹配区像素
	auto& occlusions = occlusions_;
	auto& mismatches = mismatches_;
	occlusions.clear();
	mismatches.clear();

	// ---左右一致性检查
	for (sint32 y = 0; y < height; y++) {
    
    
		for (sint32 x = 0; x < width; x++) {
    
    
			// 左影像视差值
			auto& disp = disp_left_[y * width + x];
			if (disp == Invalid_Float) {
    
    
				mismatches.emplace_back(x, y);
				continue;
			}

			// 根据视差值找到右影像上对应的同名像素
			const auto col_right = lround(x - disp);
			if (col_right >= 0 && col_right < width) {
    
    
				// 右影像上同名像素的视差值
				const auto& disp_r = disp_right_[y * width + col_right];
				// 判断两个视差值是否一致（差值在阈值内）
				if (abs(disp - disp_r) > threshold) {
    
    
					// 区分遮挡区和误匹配区
					// 通过右影像视差算出在左影像的匹配像素，并获取视差disp_rl
					// if(disp_rl > disp) 
					//		pixel in occlusions
					// else 
					//		pixel in mismatches
					const sint32 col_rl = lround(col_right + disp_r);
					if (col_rl > 0 && col_rl < width) {
    
    
						const auto& disp_l = disp_left_[y * width + col_rl];
						if (disp_l > disp) {
    
    
							occlusions.emplace_back(x, y);
						}
						else {
    
    
							mismatches.emplace_back(x, y);
						}
					}
					else {
    
    
						mismatches.emplace_back(x, y);
					}

					// 让视差值无效
					disp = Invalid_Float;
				}
			}
			else {
    
    
				// 通过视差值在右影像上找不到同名像素（超出影像范围）
				disp = Invalid_Float;
				mismatches.emplace_back(x, y);
			}
		}
	}
}

我们对左视图的所有像素视差执行一致性检查，不满足一致性检查的像素，我们将视差值赋值为无效值，且区分为误匹配区像素和遮挡区像素并单独存储，目的是在后面视差插值的时候使用不同的策略。

2. Iterative Region Voting迭代局部投票

对无效像素 $p$ 的十字交叉域支持区内的所有可靠像素，统计[0, $d_{max}$]范围视差分布的直方图 $H_p$
（直方图的值相当于视差的得票数）。占有最多像素（也就是得票最多）的视差值记为 $d_p^{\ast }$ 。可靠像素数量记为 $S_p$
。如果可靠像素的数量足够多，且得票最多的视差值得票率足够多，则把 $d_p^{\ast }$ 赋给 $p$ 。这里的两个“足够多”，用阈值来控制：

式中，${\tau }_s$和${\tau }_H$为两个预设阈值。

以上是局部投票的理论描述，所以我们对每个像素，要做的就是，把无效像素$p$十字交叉域支持区的所有可靠像素的直方图计算出来，然后挑出出现次数最多的视差值 $d_p^{\ast }$，如果它的数量占总数的比例大于阈值，且整个支持区内可靠像素足够多，就把$d_p^{\ast }$赋给$p$。

以上操作，我们要重复做多次，算法推荐是5次。

我们来看代码：

void MultiStepRefiner::IterativeRegionVoting()
{
    
    
	const sint32 width = width_;

	const auto disp_range = max_disparity_ - min_disparity_;
	if(disp_range <= 0) {
    
    
		return;
	}
	const auto arms = cross_arms_;

	// 直方图
	vector<sint32> histogram(disp_range,0);

	// 迭代5次
	const sint32 num_iters = 5;
	
	for (sint32 it = 0; it < num_iters; it++) {
    
    
		for (sint32 k = 0; k < 2; k++) {
    
    
			auto& trg_pixels = (k == 0) ? mismatches_ : occlusions_;
			for (auto& pix : trg_pixels) {
    
    
				const sint32& x = pix.first;
				const sint32& y = pix.second;
				auto& disp = disp_left_[y * width + x];
				if(disp != Invalid_Float) {
    
    
					continue;
				}

				// init histogram
				memset(&histogram[0], 0, disp_range * sizeof(sint32));

				// 计算支持区的视差直方图
				// 获取arm
				auto& arm = arms[y * width + x];
				// 遍历支持区像素视差，统计直方图
				for (sint32 t = -arm.top; t <= arm.bottom; t++) {
    
    
					const sint32& yt = y + t;
					auto& arm2 = arms[yt * width_ + x];
					for (sint32 s = -arm2.left; s <= arm2.right; s++) {
    
    
						const auto& d = disp_left_[yt * width + x + s];
						if (d != Invalid_Float) {
    
    
							const auto di = lround(d);
							histogram[di - min_disparity_]++;
						}
					}
				}
				// 计算直方图峰值对应的视差
				sint32 best_disp = 0, count = 0;
				sint32 max_ht = 0;
				for (sint32 d = 0; d < disp_range; d++) {
    
    
					const auto& h = histogram[d];
					if (max_ht < h) {
    
    
						max_ht = h;
						best_disp = d;
					}
					count += h;
				}

				if (max_ht > 0) {
    
    
					if (count > irv_ts_ && max_ht * 1.0f / count > irv_th_) {
    
    
						disp = best_disp + min_disparity_;
					}
				}
			}
			// 删除已填充像素
			for (auto it = trg_pixels.begin(); it != trg_pixels.end();) {
    
    
				const sint32 x = it->first;
				const sint32 y = it->second;
				if(disp_left_[y * width + x]!=Invalid_Float) {
    
    
					it = trg_pixels.erase(it);
				}
				else {
    
     ++it; }
			}
		}
	}
}

Proper Interpolation

此步骤其实就是视差填充。在一致性检查中无效视差被区分为遮挡区和误匹配区。首先对无效像素 $p$ ，沿其邻域16个方向搜索可靠像素视差值，对于遮挡区像素，则选择所有可靠像素视差值中的最小值，因为遮挡区大概率来自于背景，背景视差往往是较小值；对于误匹配区像素，则选择和 $p$ 颜色最近的像素的视差，因为颜色相近的像素往往具有相近的视差值（这里应该是要限制下搜索步长的，太远了假设大概率都失效了）。

void MultiStepRefiner::ProperInterpolation()
{
    
    
	const sint32 width = width_;
	const sint32 height = height_;

	const float32 pi = 3.1415926f;
	// 最大搜索行程，没有必要搜索过远的像素
	const sint32 max_search_length = std::max(abs(max_disparity_), abs(min_disparity_));

	std::vector<pair<sint32, float32>> disp_collects;
	for (sint32 k = 0; k < 2; k++) {
    
    
		auto& trg_pixels = (k == 0) ? mismatches_ : occlusions_;
		if (trg_pixels.empty()) {
    
    
			continue;
		}
		std::vector<float32> fill_disps(trg_pixels.size());

		// 遍历待处理像素
		for (auto n = 0u; n < trg_pixels.size(); n++) {
    
    
			auto& pix = trg_pixels[n];
			const sint32 x = pix.first;
			const sint32 y = pix.second;

			// 收集16个方向上遇到的首个有效视差值
			disp_collects.clear();
			double ang = 0.0;
			for (sint32 s = 0; s < 16; s++) {
    
    
				const auto sina = sin(ang);
				const auto cosa = cos(ang);
				for (sint32 m = 1; m < max_search_length; m++) {
    
    
					const sint32 yy = lround(y + m * sina);
					const sint32 xx = lround(x + m * cosa);
					if (yy < 0 || yy >= height || xx < 0 || xx >= width) {
    
     break;}
					const auto& d = disp_left_[yy * width + xx];
					if (d != Invalid_Float) {
    
    
						disp_collects.emplace_back(yy * width * 3 + 3 * xx, d);
						break;
					}
				}
				ang += pi / 16;
			}
			if (disp_collects.empty()) {
    
    
				continue;
			}

			// 如果是误匹配区，则选择颜色最相近的像素视差值
			// 如果是遮挡区，则选择最小视差值
			if (k == 0) {
    
    
				sint32 min_dist = 9999;
				float32 d = 0.0f;
				const auto color = ADColor(img_left_[y*width * 3 + 3 * x], img_left_[y*width * 3 + 3 * x + 1], img_left_[y*width * 3 + 3 * x + 2]);
				for (auto& dc : disp_collects) {
    
    
					const auto color2 = ADColor(img_left_[dc.first], img_left_[dc.first + 1], img_left_[dc.first + 2]);
					const auto dist = abs(color.r - color2.r) + abs(color.g - color2.g) + abs(color.b - color2.b);
					if (min_dist > dist) {
    
    
						min_dist = dist;
						d = dc.second;
					}
				}
				fill_disps[n] = d;
			}
			else {
    
    
				float32 min_disp = Large_Float;
				for (auto& dc : disp_collects) {
    
    
					min_disp = std::min(min_disp, dc.second);
				}
				fill_disps[n] = min_disp;
			}
		}
		for (auto n = 0u; n < trg_pixels.size(); n++) {
    
    
			auto& pix = trg_pixels[n];
			const sint32 x = pix.first;
			const sint32 y = pix.second;
			disp_left_[y * width + x] = fill_disps[n];
		}
	}
}

Depth Discontinuity Adjustment视差非连续区调整

此步的目的是进一步优化视差非连续区域的视差值。

首先，会对视差图做一个边缘检测，对于边缘上的像素 $p$ ，记其视差值为 $D(p)$ ，记录其左右两边像素 $p_1$、$p_2$的视差值 $D_L(p_1)$、$D_L(p_2)$。如果$D_L(p_1)$、$D_L(p_2)$中存在一个视差值赋给像素 $p$ 后的匹配代价比 $p$ 原先的匹配代价 $C_2(p,D(p))$ 更小，则把 $D(p)$替换成该视差值。

其实就是对边缘上的像素值进行微调，选择左右两边使其代价更小的那个视差值。

所以我们的代码实现也比较简单，做个边缘检测，再左右搜索有没有视差值会带来更小的匹配代价，若有则替换。边缘检测我们使用Sobel算子，两个简单的二维卷积运算（也可以使用其他的边缘检测算子替代）。

void MultiStepRefiner::EdgeDetect(uint8* edge_mask, const float32* disp_ptr, const sint32& width, const sint32& height, const float32 threshold)
{
    
    
	memset(edge_mask, 0, width*height * sizeof(uint8));
	// sobel算子
	for (int y = 1; y < height - 1; y++) {
    
    
		for (int x = 1; x < width - 1; x++) {
    
    
			const auto grad_x = (-disp_ptr[(y - 1) * width + x - 1] + disp_ptr[(y - 1) * width + x + 1]) +
				(-2 * disp_ptr[y * width + x - 1] + 2 * disp_ptr[y * width + x + 1]) +
				(-disp_ptr[(y + 1) * width + x - 1] + disp_ptr[(y + 1) * width + x + 1]);
			const auto grad_y = (-disp_ptr[(y - 1) * width + x - 1] - 2 * disp_ptr[(y - 1) * width + x] - disp_ptr[(y - 1) * width + x + 1]) +
				(disp_ptr[(y + 1) * width + x - 1] + 2 * disp_ptr[(y + 1) * width + x] + disp_ptr[(y + 1) * width + x + 1]);
			const auto grad = abs(grad_x) + abs(grad_y);
			if (grad > threshold) {
    
    
				edge_mask[y*width + x] = 1;
			}
		}
	}
}

void MultiStepRefiner::DepthDiscontinuityAdjustment()
{
    
    
	const sint32 width = width_;
	const sint32 height = height_;
	const auto disp_range = max_disparity_ - min_disparity_;
	if (disp_range <= 0) {
    
    
		return;
	}
	
	// 对视差图做边缘检测
	// 边缘检测的方法是灵活的，这里选择sobel算子
	const float32 edge_thres = 5.0f;
	EdgeDetect(&vec_edge_left_[0], disp_left_, width, height, edge_thres);

	// 调整边缘像素的视差
	for (sint32 y = 0; y < height; y++) {
    
    
		for (sint32 x = 1; x < width - 1; x++) {
    
    
			const auto& e_label = vec_edge_left_[y*width + x];
			if (e_label == 1) {
    
    
				const auto disp_ptr = disp_left_ + y*width;
				float32& d = disp_ptr[x];
				if (d != Invalid_Float) {
    
    
					const sint32& di = lround(d);
					const auto cost_ptr = cost_ + y*width*disp_range + x*disp_range;
					float32 c0 = cost_ptr[di];

					// 记录左右两边像素的视差值和代价值
					// 选择代价最小的像素视差值
					for (int k = 0; k<2; k++) {
    
    
						const sint32 x2 = (k == 0) ? x - 1 : x + 1;
						const float32& d2 = disp_ptr[x2];
						const sint32& d2i = lround(d2);
						if (d2 != Invalid_Float) {
    
    
							const auto& c = (k == 0) ? cost_ptr[-disp_range + d2i] : cost_ptr[disp_range + d2i];
							if (c < c0) {
    
    
								d = d2;
								c0 = c;
							}
						}
					}
				}
			}
		}
	}
}

以上便是所有子步骤的实现，注意到论文中还有一个子像素优化，有同学想问我的代码中怎么没有看到，因为我把子像素优化放在了视差计算里，在主类ADCensusStereo类的成员函数ComputeDisparity中实现。

最后，我们在Refine成员函数中，按顺序执行四个子步骤，完成整个多步骤视差优化。代码如下：

void MultiStepRefiner::Refine()
{
    
    
	if (width_ <= 0 || height_ <= 0 ||
		disp_left_ == nullptr || disp_right_ == nullptr ||
		cost_ == nullptr || cross_arms_ == nullptr) {
    
    
		return;
	}

	// step1: outlier detection
	if (do_lr_check_) {
    
    
		OutlierDetection();
	}
	// step2: iterative region voting
	if (do_region_voting_) {
    
    
		IterativeRegionVoting();
	}
	// step3: proper interpolation
	if (do_interpolating_) {
    
    
		ProperInterpolation();
	}
	// step4: discontinuities adjustment
	if (do_discontinuity_adjustment_) {
    
    
		DepthDiscontinuityAdjustment();
	}

	// median filter
	adcensus_util::MedianFilter(disp_left_, disp_left_, width_, height_, 3);
}

后面我们跟了一个中值滤波，也算常规操作，虽然论文里没有，但是加上没什么坏处。

实验

对Cone数据，我们对应四个子步骤做四组实验，分别是

1. Outlier Detection（1）
2. Outlier Detection + Iterative Region Voting（1+2）
3. Outlier Detection + Iterative Region Voting + Proper Interpolation（1+2+3）
4. Outlier Detection + Iterative Region Voting + Proper Interpolation + Depth Discontinuity Adjustment（1+2+3+4）

我们先贴一下上篇的结果，也就是到扫描线优化步骤的效果图：

代价计算

代价计算

代价聚合

扫描线优化

多步骤视差优化的实验结果如下：

1

1+2

1+2+3

1+2+3+4

可以看到，对视差图效果影响最大的就是第1个和第3步，一个是一致性检查去噪，一个是视差填充，另外两步对视差图改变很小，算是锦上添花或者视差微调，如果是做高效率实现，这两步便可以适当舍弃了。

好了，我们ADCensus的码上实战系列到此就结束了，希望对大家有所帮助。

下载AD-Census完整源码，点击进入: https://github.com/ethan-li-coding/AD-Census
欢迎同学们在Github项目里讨论，如果觉得博主代码质量不错，右上角给颗星！感谢！

往期博文如下：

码上实战系列

【码上实战】【立体匹配系列】经典AD-Census: （1）框架
【码上实战】【立体匹配系列】经典AD-Census: （2）主类
【码上实战】【立体匹配系列】经典AD-Census: （3）代价计算
【码上实战】【立体匹配系列】经典AD-Census: （4）十字交叉域代价聚合
【码上实战】【立体匹配系列】经典AD-Census: （5）扫描线优化

博主简介：
Ethan Li 李迎松（知乎：李迎松）
武汉大学 摄影测量与遥感专业博士
主方向立体匹配、三维重建
2019年获测绘科技进步一等奖（省部级）

爱三维，爱分享，爱开源
GitHub： https://github.com/ethan-li-coding （欢迎follow和star）

个人微信：

欢迎交流！

关注博主不迷路，感谢！
博客主页：https://ethanli.blog.csdn.net