【码上实战】【立体匹配系列】经典AD-Census: （4）十字交叉域代价聚合

抱歉让大家久等了！

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

上篇代价计算中，带大家深入了解了ADCensusStereo代价计算步骤的实现代码，并做了实验展示了算法的结果。这里先贴一下代价计算的实验结果图：

AD

Census

AD-Census

正如上篇结尾所述，AD-Census的代价计算方法结合了AD和Census匹配策略的优点，是一个优秀的策略。

而无论如何，代价计算的结果都无法作为最终的视差结果，后续的优化步骤对ADCensus来说都是不可少的步骤。本篇即介绍第一个优化步骤：基于十字交叉域的代价聚合 的代码实现。

算法

算法原理请看博文：

经典AD-Census: （2）十字交叉域代价聚合（Cross-based Cost Aggregation）

不再赘述，以免喧宾夺主。

代码实现

类设计

成员函数

我将十字交叉域代价聚合写成一个类CrossAggregator，放在文件cross_aggregator.h/cross_aggregator.cpp中。

/*** \brief 十字交叉域代价聚合器*/
class CrossAggregator {
    
public:CrossAggregator();~CrossAggregator();
}

我们需要为CrossAggregator类设计一些公有接口，从而可以调用它们来达到代价聚合的目的。

我们沿用代价计算器设计的思路。

第一个接口是 初始化函数Initialize ，为代价聚合做一些准备工作，例如很重要的一点我需要开辟一块内存用来存放每个像素的十字交叉臂数据，以便后面进行聚合。

第二类接口是必不可少的 设置数据SetData 以及 设置参数SetParam ，脱离数据和参数，算法啥也不是。

第三个就是关键接口 Aggregate 了，最终还是要靠调用它来完成代价聚合。

最后两个额外的接口是 get_arms_ptr 和 get_cost_ptr，前者获取十字交叉臂数组，这纯粹是因为后处理步骤需要；后者是获取聚合后的代价数据指针，它很重要，因为后面的扫描线优化步骤需要它。

我们来看看最终的类接口设计：

/*** \brief 初始化代价聚合器* \param width 影像宽* \param height 影像高* \return true:初始化成功*/
bool Initialize(const sint32& width, const sint32& height, const sint32& min_disparity, const sint32& max_disparity);/*** \brief 设置代价聚合器的数据* \param img_left // 左影像数据，三通道* \param img_right // 右影像数据，三通道* \param cost_init // 初始代价数组*/
void SetData(const uint8* img_left, const uint8* img_right, const float32* cost_init);/*** \brief 设置代价聚合器的参数* \param cross_L1 // L1* \param cross_L2 // L2* \param cross_t1 // t1* \param cross_t2 // t2*/
void SetParams(const sint32& cross_L1, const sint32& cross_L2, const sint32& cross_t1, const sint32& cross_t2);/** \brief 聚合 */
void Aggregate(const sint32& num_iters);/** \brief 获取所有像素的十字交叉臂数据指针 */
CrossArm* get_arms_ptr();/** \brief 获取聚合代价数组指针 */
float32* get_cost_ptr();

如上接口为类的全部公有函数，调用者可按逻辑次序调用。具体接口的参数含义，注释写的很清楚，大家对着看就行了。

需要提到的一点是，get_arms_ptr函数范围的类型CrossArm，它是我自定义的十字交叉臂结构体，保存了像素的上下左右臂的长度。如下所示：

/** * \brief 交叉十字臂结构 * 为了限制内存占用，臂长类型设置为uint8，这意味着臂长最长不能超过255 */
struct CrossArm {
    uint8 left, right, top, bottom;CrossArm() : left(0), right(0), top(0), bottom(0) {
     }
};

为了完成代价聚合，我们需要一些子步骤，它们包括：

1. 构建十字交叉臂
2. 计算像素的支持区像素数量
3. 聚合所有像素在某固定视差下的代价

它们被设计为类的私有成员函数：

/** \brief 构建十字交叉臂 */
void BuildArms();
/** \brief 搜索水平臂 */
void FindHorizontalArm(const sint32& x, const sint32& y, uint8& left, uint8& right) const;
/** \brief 搜索竖直臂 */
void FindVerticalArm(const sint32& x, const sint32& y, uint8& top, uint8& bottom) const;
/** \brief 计算像素的支持区像素数量 */
void ComputeSupPixelCount();
/** \brief 聚合某个视差 */
void AggregateInArms(const sint32& disparity, const bool& horizontal_first);

在聚合函数 Aggregate 中，这些私有函数被依次调用从而完成代价聚合。

成员变量

最后是成员函数类不可或缺的成员变量，比如影像尺寸、交叉臂数据（左影像）、影像数据、代价数据、算法参数等，正是它们在类的作用域内始终保持着算法计算所需要的值，才能达到最后的计算目的。得用私有类型把它们保护起来，仅限类的内部使用，我们来看看吧！

/** \brief 图像尺寸 */
sint32	width_;
sint32	height_;/** \brief 交叉臂 */
vector<CrossArm> vec_cross_arms_;/** \brief 影像数据 */
const uint8* img_left_;
const uint8* img_right_;/** \brief 初始代价数组指针 */
const float32* cost_init_;
/** \brief 聚合代价数组 */
vector<float32> cost_aggr_;/** \brief 临时代价数据 */
vector<float32> vec_cost_tmp_[2];
/** \brief 支持区像素数量数组 0：水平臂优先 1：竖直臂优先 */
vector<uint16> vec_sup_count_[2];
vector<uint16> vec_sup_count_tmp_;sint32	cross_L1_;			// 十字交叉窗口的空间域参数：L1
sint32  cross_L2_;			// 十字交叉窗口的空间域参数：L2
sint32	cross_t1_;			// 十字交叉窗口的颜色域参数：t1
sint32  cross_t2_;			// 十字交叉窗口的颜色域参数：t2
sint32  min_disparity_;			// 最小视差
sint32	max_disparity_;			// 最大视差/** \brief 是否成功初始化标志 */
bool is_initialized_;

说明一下，在成员变量中，我们的初始代价数据cost_init_是指针型，因为是直接使用的代价计算器返回的数据，而聚合代价数据cost_aggr_是vector容器，它将在本类的初始化函数中开辟内存，并存在于代价聚合器的整个生命周期，将通过接口get_cost_ptr输出其地址，给后续步骤使用。

类实现

我们先看看代价聚合器类CrossAggregator的初始化函数实现：

bool CrossAggregator::Initialize(const sint32& width, const sint32& height, const sint32& min_disparity, const sint32& max_disparity)
{
    width_ = width;height_ = height;min_disparity_ = min_disparity;max_disparity_ = max_disparity;const sint32 img_size = width_ * height_;const sint32 disp_range = max_disparity_ - min_disparity_;if (img_size <= 0 || disp_range <= 0) {
    is_initialized_ = false;return is_initialized_;}// 为交叉十字臂数组分配内存vec_cross_arms_.clear();vec_cross_arms_.resize(img_size);// 为临时代价数组分配内存vec_cost_tmp_[0].clear();vec_cost_tmp_[0].resize(img_size);vec_cost_tmp_[1].clear();vec_cost_tmp_[1].resize(img_size);// 为存储每个像素支持区像素数量的数组分配内存vec_sup_count_[0].clear();vec_sup_count_[0].resize(img_size);vec_sup_count_[1].clear();vec_sup_count_[1].resize(img_size);vec_sup_count_tmp_.clear();vec_sup_count_tmp_.resize(img_size);// 为聚合代价数组分配内存cost_aggr_.resize(img_size * disp_range);is_initialized_ = !vec_cross_arms_.empty() && !vec_cost_tmp_[0].empty() && !vec_cost_tmp_[1].empty() && !vec_sup_count_[0].empty() && !vec_sup_count_[1].empty() && !vec_sup_count_tmp_.empty() && !cost_aggr_.empty();return is_initialized_;
}

前面变量赋值不必多说，初始化主要的任务是为交叉十字臂数组分配内存，为临时代价数组分配内存，以及为存储每个像素支持区像素数量的数组分配内存。（如果读懂了算法理论，那么这些操作应该不难理解，都是为了最后计算聚合后的代价）。此外，聚合代价数组的内存也在此被开辟，并由本类终生维护。（自己计算出的结果自己来维护，合情合理！）

初始化之后，SetData和SetParam函数都比较好理解，我也就不占用篇幅了。

当以上准备工作做好后，我们就开始真正实现代价聚合的功能。

从原理可知，要完成代价聚合，我们必须经过两个步骤：

1. 构建聚合区域，也就是构建十字交叉臂
2. 执行聚合

我们知道对于每个像素，都有两个方向的臂，一个水平方向臂，一个竖直方向臂，而构建臂的约束是一模一样的，分为距离约束和颜色约束，简单来说是距离不能超过设定阈值，颜色差也不能超过设定阈值。这么做是为了让像素聚合区域内的像素都和其视差相近。AD-Census虽然不是简单的在臂延伸的过程中计算像素与中心像素的距离和色差，但是也不复杂，只是多计算一个延伸方向上与上一个像素的距离和色差，让构造更加稳健一些。具体实现原理，请看博文：

经典AD-Census: （2）十字交叉域代价聚合

我写了两个函数分别构造水平臂和竖直臂，其实除了方向不一样，构造方法及参数都是一模一样的。这里只贴出像素$(x,y)$水平臂的构造代码：

void CrossAggregator::FindHorizontalArm(const sint32& x, const sint32& y, uint8& left, uint8& right) const
{
    // 像素数据地址const auto img0 = img_left_ + y * width_ * 3 + 3 * x;// 像素颜色值const ADColor color0(img0[0], img0[1], img0[2]);left = right = 0;//计算左右臂,先左臂后右臂sint32 dir = -1;for (sint32 k = 0; k < 2; k++) {
    // 延伸臂直到条件不满足// 臂长不得超过cross_L1auto img = img0 + dir * 3;auto color_last = color0;sint32 xn = x + dir;for (sint32 n = 0; n < std::min(cross_L1_, MAX_ARM_LENGTH); n++) {
    // 边界处理if (k == 0) {
    if (xn < 0) {
    break;}}else {
    if (xn == width_) {
    break;}}// 获取颜色值const ADColor color(img[0], img[1], img[2]);// 颜色距离1（臂上像素和计算像素的颜色距离）const sint32 color_dist1 = ColorDist(color, color0);if (color_dist1 >= cross_t1_) {
    break;}// 颜色距离2（臂上像素和前一个像素的颜色距离）if (n > 0) {
    const sint32 color_dist2 = ColorDist(color, color_last);if (color_dist2 >= cross_t1_) {
    break;}}// 臂长大于L2后，颜色距离阈值减小为t2if (n + 1 > cross_L2_) {
    if (color_dist1 >= cross_t2_) {
    break;}}if (k == 0) {
    left++;}else {
    right++;}color_last = color;xn += dir;img += dir * 3;}dir = -dir;}
}

搞清楚是哪两个像素参与距离计算就差不多弄懂了。其一是在臂延伸过程中新的像素与中心像素的距离；其二是新的像素与其前一个像素的距离。

竖直臂的构造方法和水平臂一样，只是方向不同而已。

接下来，我们需要计算出每个像素的支持区（十字交叉臂覆盖的区域）像素的数量。为什么要有这一步呢？原因是我们在原理部分了解到，代价聚合是将支持区的代价值累加，再除以支持区的像素数量，也就是计算支持区代价的均值，赋给中心像素的代价值。

同时，我们必须注意到的是，不同的聚合方向，支持区并不相同，即先水平再竖直的聚合方向和先竖直再水平的聚合方向，两者的支持区是不同的。所以需要分别用两种方向的数组来保存支持区的像素数。我们来看代码：

void CrossAggregator::ComputeSupPixelCount()
{
    // 计算每个像素的支持区像素数量// 注意：两种不同的聚合方向，像素的支持区像素是不同的，需要分开计算bool horizontal_first = true;for (sint32 n = 0; n < 2; n++) {
    // n=0 : horizontal_first; n=1 : vertical_firstconst sint32 id = horizontal_first ? 0 : 1;for (sint32 k = 0; k < 2; k++) {
    // k=0 : pass1; k=1 : pass2for (sint32 y = 0; y < height_; y++) {
    for (sint32 x = 0; x < width_; x++) {
    // 获取arm数值auto& arm = vec_cross_arms_[y*width_ + x];sint32 count = 0;if (horizontal_first) {
    if (k == 0) {
    // horizontalfor (sint32 t = -arm.left; t <= arm.right; t++) {
    count++;}}else {
    // verticalfor (sint32 t = -arm.top; t <= arm.bottom; t++) {
    count += vec_sup_count_tmp_[(y + t)*width_ + x];}}}else {
    if (k == 0) {
    // verticalfor (sint32 t = -arm.top; t <= arm.bottom; t++) {
    count++;}}else {
    // horizontalfor (sint32 t = -arm.left; t <= arm.right; t++) {
    count += vec_sup_count_tmp_[y*width_ + x + t];}}}if (k == 0) {
    vec_sup_count_tmp_[y*width_ + x] = count;}else {
    vec_sup_count_[id][y*width_ + x] = count;}}}}horizontal_first = !horizontal_first;}
}

在计算的过程中，我们通过模拟构造支持区的过程来计算支持区的像素数，即像原文中所述，先统计像素在一个方向的支持区像素数量，存储于临时数组里，再沿另一个方向的臂累加临时数组里的存储数值。由于两个构造方向的支持区不同，所以vec_sup_count_是一个容量为2的数组，分别保存着两个构造方向每个像素支持区数量。

最后，我们可以进行迭代聚合了，迭代次数是作为参数传入的，我们用一个for循环来完成迭代，每执行一次迭代，我们会转换下支持区构造顺序（偶数次迭代是先水平再竖直构造，奇数次是先竖直再水平构造）。之后算法将对候选视差下的每一个视差 $d$ 进行独立的全图聚合（视差 $d_1$ 与视差 $d_2$ 之间的代价聚合是完全独立的，是可以高度并行的）。

我们先来看单视差 $d$ 下的聚合代码实现：

void CrossAggregator::AggregateInArms(const sint32& disparity, const bool& horizontal_first)
{
    // 此函数聚合所有像素当视差为disparity时的代价if (disparity < min_disparity_ || disparity >= max_disparity_) {
    return;}const auto disp = disparity - min_disparity_;const sint32 disp_range = max_disparity_ - min_disparity_;if (disp_range <= 0) {
    return;}// 将disp层的代价存入临时数组vec_cost_tmp_[0]// 这样可以避免过多的访问更大的cost_aggr_,提高访问效率for (sint32 y = 0; y < height_; y++) {
    for (sint32 x = 0; x < width_; x++) {
    vec_cost_tmp_[0][y * width_ + x] = cost_aggr_[y * width_ * disp_range + x * disp_range + disp];}}// 逐像素聚合const sint32 ct_id = horizontal_first ? 0 : 1;for (sint32 k = 0; k < 2; k++) {
    // k==0: pass1// k==1: pass2for (sint32 y = 0; y < height_; y++) {
    for (sint32 x = 0; x < width_; x++) {
    // 获取arm数值auto& arm = vec_cross_arms_[y*width_ + x];// 聚合float32 cost = 0.0f;if (horizontal_first) {
    if (k == 0) {
    // horizontalfor (sint32 t = -arm.left; t <= arm.right; t++) {
    cost += vec_cost_tmp_[0][y * width_ + x + t];}} else {
    // verticalfor (sint32 t = -arm.top; t <= arm.bottom; t++) {
    cost += vec_cost_tmp_[1][(y + t)*width_ + x];}}}else {
    if (k == 0) {
    // verticalfor (sint32 t = -arm.top; t <= arm.bottom; t++) {
    cost += vec_cost_tmp_[0][(y + t) * width_ + x];}} else {
    // horizontalfor (sint32 t = -arm.left; t <= arm.right; t++) {
    cost += vec_cost_tmp_[1][y*width_ + x + t];}}}if (k == 0) {
    vec_cost_tmp_[1][y*width_ + x] = cost;}else {
    cost_aggr_[y*width_*disp_range + x*disp_range + disp] = cost / vec_sup_count_[ct_id][y*width_ + x];}}}}
}

代码按照原文的思路，先对每个像素，将某一方向的代价值累加到临时存储数组；再对每个像素沿另一方向的臂展对临时存储值累加，最后累加值除以支持区数量，得到最终的代价值。

为了加快速度，在聚合之前，我们将视差为 $d$ 时的初始代价，转存至一个临时的二维数组，这样聚合过程中无需频繁访问三维的代价数组，减少访问耗时。

以上我们就将聚合的子步骤都实现完毕。剩下的工作就简单了，在类的公有聚合接口 Aggregate 中，我们只需要一次调用以上子步骤完成聚合即可。代码如下：

void CrossAggregator::Aggregate(const sint32& num_iters)
{
    if (!is_initialized_) {
    return;}const sint32 disp_range = max_disparity_ - min_disparity_;// 构建像素的十字交叉臂BuildArms();// 代价聚合// horizontal_first 代表先水平方向聚合bool horizontal_first = true;// 计算两种聚合方向的各像素支持区像素数量ComputeSupPixelCount();// 先将聚合代价初始化为初始代价memcpy(&cost_aggr_[0], cost_init_, width_*height_*disp_range*sizeof(float32));// 多迭代聚合for (sint32 k = 0; k < num_iters; k++) {
    for (sint32 d = min_disparity_; d < max_disparity_; d++) {
    AggregateInArms(d, horizontal_first);}// 下一次迭代，调换顺序horizontal_first = !horizontal_first;}
}

函数的参数是聚合的迭代次数，原文推荐为4。

最后在主类ADCensusStereo的代价聚合函数CostAggregation中，我们调用代价聚合器CrossAggregator的公有成员函数完成代价聚合：

void ADCensusStereo::CostAggregation()
{
    // 设置聚合器数据aggregator_.SetData(img_left_, img_right_, cost_computer_.get_cost_ptr());// 设置聚合器参数aggregator_.SetParams(option_.cross_L1, option_.cross_L2, option_.cross_t1, option_.cross_t2);// 代价聚合aggregator_.Aggregate(4);
}

从繁至简，也是C++的特性之一。

实验

按照惯例，我们会提供一些实验结果。

和上篇代价计算一样，我们依然采用Cone数据。

左视图

右视图

代价计算的结果，我在开头已经贴出，代价聚合的实验结果如下：

代价计算

代价聚合

一个字，妙！通过代价聚合，视差图质量得到了显著的提升！

好了今天我们就到此结束，下一篇为大家带来的是 扫描线优化。拜拜！

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

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

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

个人微信：

欢迎交流！

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