【码上实战】【立体匹配系列】经典SGM：（4）代价聚合2_sgm 文本 实战

昔人已乘黄鹤去，此地空余黄鹤楼。

2020对武汉、对中国、对世界来说是异常艰难的一年。武汉壮士扼腕，封一城而救一国，引得八方救援，举国抗疫。中国人在灾难面前总是空前团结，勇往直前！中华民族几千年来从未向任何恶势力低头，必定会彻底战胜病毒，重振辉煌！今天，武汉全面接触离汉通道，标志着我国抗疫的阶段性胜利，真让人热泪盈眶，衷心祝愿各位身体健康，远离疾病！风雨过后，一切付出终会有回报！

黄鹤归来碧空净，乘风万里入青云！
2020年4月8日 中国武汉 晴

码上教学系列

【码上实战】【立体匹配系列】经典SGM：（1）框架与类设计
【码上实战】【立体匹配系列】经典SGM：（2）代价计算
【码上实战】【立体匹配系列】经典SGM：（3）代价聚合
【码上实战】【立体匹配系列】经典SGM：（4）代价聚合2
【码上实战】【立体匹配系列】经典SGM：（5）视差优化
【码上实战】【立体匹配系列】经典SGM：（6）视差填充
【码上实战】【立体匹配系列】经典SGM：（7）弱纹理优化

完整代码已发布于Github开源项目：Github/SemiGlobalMatching，欢迎免费下载

思绪拉回来，上回咱们说到4路径聚合，想必大家对实验效果有深刻的印象，不同路径下的效果有显著的区别，结论显示多路径比单路径有明显的效果改善，4个邻居还是比1个邻居靠谱啊。那有人就要说了，8个邻居岂不是更靠谱，聪明！不过我先偷偷告诉你们，4邻居其实性价比很高，8邻居会有人会偷懒不干事。

好了，我们先回顾下4路径的效果，免得你们再回去看一遍博客（我是不是傻！）。

其实效果已经不错了，剔除错误的匹配，也能达到不错的效果。咦？我怎么老是在说4路径的好！好吧，我坦白我用4路径用的还不少，主要因为我要应对不少对高效率的任务需求。

回正题！上 8路径！

对角线路径

8路径说白了，就是比4路径多4个对角线路径，如图所示：

聚合的整体逻辑和4路径是一致的，可以按照4路径的代码思路来写，大家只要清楚怎么计算下一个像素的代价、灰度的内存地址，实现起来就不难。实际上8个路径聚合的逻辑都是一样，最本质不同的地方就是下个像素相对于当前像素是怎么偏移的，比如左到右，位置偏移就是列号加1；上到下，位置偏移就是行号加1；对角线路径中的左上到右下，位置偏移就是行号列号各加1。每个路径前后像素的代价和灰度的位置偏移量都是固定的，比如左到右偏移量是1个像素(disp_range个代价)；上到下偏移量是 w 个像素(w * disp_range 个代价)；左上到右下偏移量是 (w + 1) 个像素((w+1) * disp_range个代价)。

对角线路径还有另一点不同，就是它们会撞南墙（对，就是撞南墙才回头那种撞）！画个图感受下：

“撞南墙就回头啊！”
“不，聚合任务没完成怎么能回头呢？”
“好，那就行号继续保持前进，列号打道回府，重新来过。”

也就是行号继续按方向走一步，列号重置为起始列号！再画个图感受下：

右上到左下的对角线撞墙图我就不画了，累！

就这两点，其他和4-路径没啥区别了，不信？来给你们看代码：

void sgm_util::CostAggregateDagonal_1(const uint8* img_data, const sint32& width, const sint32& height,
	const sint32& min_disparity, const sint32& max_disparity, const sint32& p1, const sint32& p2_init,
	const uint8* cost_init, uint8* cost_aggr, bool is_forward)
{
	assert(width > 1 && height > 1 && max_disparity > min_disparity);

	// 视差范围
	const sint32 disp_range = max_disparity - min_disparity;

	// P1,P2
	const auto& P1 = p1;
	const auto& P2_Init = p2_init;

	// 正向(左上->右下) ：is_forward = true ; direction = 1
	// 反向(右下->左上) ：is_forward = false; direction = -1;
	const sint32 direction = is_forward ? 1 : -1;

	// 聚合

	// 存储当前的行列号，判断是否到达影像边界
	sint32 current_row = 0;
	sint32 current_col = 0;

	for (sint32 j = 0; j < width; j++) {
		// 路径头为每一列的首(尾,dir=-1)行像素
		auto cost_init_col = (is_forward) ? (cost_init + j * disp_range) : (cost_init + (height - 1) * width * disp_range + j * disp_range);
		auto cost_aggr_col = (is_forward) ? (cost_aggr + j * disp_range) : (cost_aggr + (height - 1) * width * disp_range + j * disp_range);
		auto img_col = (is_forward) ? (img_data + j) : (img_data + (height - 1) * width + j);

		// 路径上上个像素的代价数组，多两个元素是为了避免边界溢出（首尾各多一个）
		std::vector<uint8> cost_last_path(disp_range + 2, UINT8_MAX);

		// 初始化：第一个像素的聚合代价值等于初始代价值
		memcpy(cost_aggr_col, cost_init_col, disp_range * sizeof(uint8));
		memcpy(&cost_last_path[1], cost_aggr_col, disp_range * sizeof(uint8));

		// 路径上当前灰度值和上一个灰度值
		uint8 gray = *img_col;
		uint8 gray_last = *img_col;

		// 对角线路径上的下一个像素，中间间隔width+1个像素
		// 这里要多一个边界处理
		// 沿对角线前进的时候会碰到影像列边界，策略是行号继续按原方向前进，列号到跳到另一边界
		current_row = is_forward ? 0 : height - 1;
		current_col = j;
		if (is_forward && current_col == width - 1 && current_row < height - 1) {
			// 左上->右下，碰右边界
			cost_init_col = cost_init + (current_row + direction) * width * disp_range;
			cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range;
			img_col = img_data + (current_row + direction) * width;
		}
		else if (!is_forward && current_col == 0 && current_row > 0) {
			// 右下->左上，碰左边界
			cost_init_col = cost_init + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
			cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
			img_col = img_data + (current_row + direction) * width + (width - 1);
		}
		else {
			cost_init_col += direction * (width + 1) * disp_range;
			cost_aggr_col += direction * (width + 1) * disp_range;
			img_col += direction * (width + 1);
		}

		// 路径上上个像素的最小代价值
		uint8 mincost_last_path = UINT8_MAX;
		for (auto cost : cost_last_path) {
			mincost_last_path = std::min(mincost_last_path, cost);
		}

		// 自方向上第2个像素开始按顺序聚合
		for (sint32 i = 0; i < height - 1; i ++) {
			gray = *img_col;
			uint8 min_cost = UINT8_MAX;
			for (sint32 d = 0; d < disp_range; d++) {
				// Lr(p,d) = C(p,d) + min( Lr(p-r,d), Lr(p-r,d-1) + P1, Lr(p-r,d+1) + P1, min(Lr(p-r))+P2 ) - min(Lr(p-r))
				const uint8  cost = cost_init_col[d];
				const uint16 l1 = cost_last_path[d + 1];
				const uint16 l2 = cost_last_path[d] + P1;
				const uint16 l3 = cost_last_path[d + 2] + P1;
				const uint16 l4 = mincost_last_path + std::max(P1, P2_Init / (abs(gray - gray_last) + 1));

				const uint8 cost_s = cost + static_cast<uint8>(std::min(std::min(l1, l2), std::min(l3, l4)) - mincost_last_path);

				cost_aggr_col[d] = cost_s;
				min_cost = std::min(min_cost, cost_s);
			}

			// 重置上个像素的最小代价值和代价数组
			mincost_last_path = min_cost;
			memcpy(&cost_last_path[1], cost_aggr_col, disp_range * sizeof(uint8));

			// 当前像素的行列号
			current_row += direction;
			current_col += direction;
			
			// 下一个像素,这里要多一个边界处理
			// 这里要多一个边界处理
			// 沿对角线前进的时候会碰到影像列边界，策略是行号继续按原方向前进，列号到跳到另一边界
			if (is_forward && current_col == width - 1 && current_row < height - 1) {
				// 左上->右下，碰右边界
				cost_init_col = cost_init + (current_row + direction) * width * disp_range;
				cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range;
				img_col = img_data + (current_row + direction) * width;
			}
			else if (!is_forward && current_col == 0 && current_row > 0) {
				// 右下->左上，碰左边界
				cost_init_col = cost_init + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
				cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
				img_col = img_data + (current_row + direction) * width + (width - 1);
			}
			else {
				cost_init_col += direction * (width + 1) * disp_range;
				cost_aggr_col += direction * (width + 1) * disp_range;
				img_col += direction * (width + 1);
			}

			// 像素值重新赋值
			gray_last = gray;
		}
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

你品，你细品，是不是就下个路径的计算方法不一样，并增加了触碰边界处理（咦？边界是啥？兄弟！别问！自己感受！我总不能在代码里写南墙！）

自我吐槽一下，函数名字取的不行，什么叫对角线1，这样的阿拉伯数字怎么能当名字，好吧，自我反省，各位有才之人自己换吧。郑重的解释一下：对角线1表示：“左上>>右下” 路径！

是的兄弟，我还有个对角线2，表示：“右下>>左上” 路径：

void sgm_util::CostAggregateDagonal_2(const uint8* img_data, const sint32& width, const sint32& height,
	const sint32& min_disparity, const sint32& max_disparity, const sint32& p1, const sint32& p2_init,
	const uint8* cost_init, uint8* cost_aggr, bool is_forward)
{
	assert(width > 1 && height > 1 && max_disparity > min_disparity);

	// 视差范围
	const sint32 disp_range = max_disparity - min_disparity;

	// P1,P2
	const auto& P1 = p1;
	const auto& P2_Init = p2_init;

	// 正向(右上->左下) ：is_forward = true ; direction = 1
	// 反向(左下->右上) ：is_forward = false; direction = -1;
	const sint32 direction = is_forward ? 1 : -1;

	// 聚合

	// 存储当前的行列号，判断是否到达影像边界
	sint32 current_row = 0;
	sint32 current_col = 0;

	for (sint32 j = 0; j < width; j++) {
		// 路径头为每一列的首(尾,dir=-1)行像素
		auto cost_init_col = (is_forward) ? (cost_init + j * disp_range) : (cost_init + (height - 1) * width * disp_range + j * disp_range);
		auto cost_aggr_col = (is_forward) ? (cost_aggr + j * disp_range) : (cost_aggr + (height - 1) * width * disp_range + j * disp_range);
		auto img_col = (is_forward) ? (img_data + j) : (img_data + (height - 1) * width + j);

		// 路径上上个像素的代价数组，多两个元素是为了避免边界溢出（首尾各多一个）
		std::vector<uint8> cost_last_path(disp_range + 2, UINT8_MAX);

		// 初始化：第一个像素的聚合代价值等于初始代价值
		memcpy(cost_aggr_col, cost_init_col, disp_range * sizeof(uint8));
		memcpy(&cost_last_path[1], cost_aggr_col, disp_range * sizeof(uint8));

		// 路径上当前灰度值和上一个灰度值
		uint8 gray = *img_col;
		uint8 gray_last = *img_col;

		// 对角线路径上的下一个像素，中间间隔width-1个像素
		// 这里要多一个边界处理
		// 沿对角线前进的时候会碰到影像列边界，策略是行号继续按原方向前进，列号到跳到另一边界
		current_row = is_forward ? 0 : height - 1;
		current_col = j;
		if (is_forward && current_col == 0 && current_row < height - 1) {
			// 右上->左下，碰左边界
			cost_init_col = cost_init + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
			cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
			img_col = img_data + (current_row + direction) * width + (width - 1);
		}
		else if (!is_forward && current_col == width - 1 && current_row > 0) {
			// 左下->右上，碰右边界
			cost_init_col = cost_init + (current_row + direction) * width * disp_range ;
			cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range;
			img_col = img_data + (current_row + direction) * width;
		}
		else {
			cost_init_col += direction * (width - 1) * disp_range;
			cost_aggr_col += direction * (width - 1) * disp_range;
			img_col += direction * (width - 1);
		}

		// 路径上上个像素的最小代价值
		uint8 mincost_last_path = UINT8_MAX;
		for (auto cost : cost_last_path) {
			mincost_last_path = std::min(mincost_last_path, cost);
		}

		// 自路径上第2个像素开始按顺序聚合
		gray = *img_col;
		for (sint32 i = 0; i < height - 1; i++) {
			uint8 min_cost = UINT8_MAX;
			for (sint32 d = 0; d < disp_range; d++) {
				// Lr(p,d) = C(p,d) + min( Lr(p-r,d), Lr(p-r,d-1) + P1, Lr(p-r,d+1) + P1, min(Lr(p-r))+P2 ) - min(Lr(p-r))
				const uint8  cost = cost_init_col[d];
				const uint16 l1 = cost_last_path[d + 1];
				const uint16 l2 = cost_last_path[d] + P1;
				const uint16 l3 = cost_last_path[d + 2] + P1;
				const uint16 l4 = mincost_last_path + P2_Init / (abs(gray - gray_last) + 1);

				const uint8 cost_s = cost + static_cast<uint8>(std::min(std::min(l1, l2), std::min(l3, l4)) - mincost_last_path);

				cost_aggr_col[d] = cost_s;
				min_cost = std::min(min_cost, cost_s);
			}

			// 重置上个像素的最小代价值和代价数组
			mincost_last_path = min_cost;
			memcpy(&cost_last_path[1], cost_aggr_col, disp_range * sizeof(uint8));

			// 当前像素的行列号
			current_row += direction;
			current_col -= direction;

			// 下一个像素,这里要多一个边界处理
			// 这里要多一个边界处理
			// 沿对角线前进的时候会碰到影像列边界，策略是行号继续按原方向前进，列号到跳到另一边界
			if (is_forward && current_col == 0 && current_row < height - 1) {
				// 右上->左下，碰左边界
				cost_init_col = cost_init + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
				cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range + (width - 1) * disp_range;
				img_col = img_data + (current_row + direction) * width + (width - 1);
			}
			else if (!is_forward && current_col == width - 1 && current_row > 0) {
				// 左下->右上，碰右边界
				cost_init_col = cost_init + (current_row + direction) * width * disp_range;
				cost_aggr_col = cost_aggr + (current_row + direction) * width * disp_range;
				img_col = img_data + (current_row + direction) * width;
			}
			else {
				cost_init_col += direction * (width - 1) * disp_range;
				cost_aggr_col += direction * (width - 1) * disp_range;
				img_col += direction * (width - 1);
			}

			// 像素值重新赋值
			gray_last = gray;
			gray = *img_col;
		}
	}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123

对角线2的代码我就不多说了，大家应该能融会贯通了吧！（大哥，你对角线1的代码也没怎么说啊！）（…）

代价聚合

相比之前的4-路径，再增加4条对角线路径：

void SemiGlobalMatching::CostAggregation() const
{
    // 路径聚合
    // 1、左->右/右->左
    // 2、上->下/下->上
    // 3、左上->右下/右下->左上
    // 4、右上->左上/左下->右上
    //
    // ↘ ↓ ↙   5  3  7
    // →    ←	 1    2
    // ↗ ↑ ↖   8  4  6
    //
    const auto& min_disparity = option_.min_disparity;
    const auto& max_disparity = option_.max_disparity;
    assert(max_disparity > min_disparity);

    const sint32 size = width_ * height_ * (max_disparity - min_disparity);
    if(size <= 0) {
        return;
    }

    const auto& P1 = option_.p1;
    const auto& P2_Int = option_.p2_init;

    // 左右聚合
    sgm_util::CostAggregateLeftRight(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_1_, true);
	sgm_util::CostAggregateLeftRight(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_2_, false);
    // 上下聚合
	sgm_util::CostAggregateUpDown(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_3_, true);
	sgm_util::CostAggregateUpDown(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_4_, false);
	// 对角线1聚合
    sgm_util::CostAggregateDagonal_1(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_5_, true);
    sgm_util::CostAggregateDagonal_1(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_6_, false);
    // 对角线2聚合
    sgm_util::CostAggregateDagonal_2(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_7_, true);
    sgm_util::CostAggregateDagonal_2(img_left_, width_, height_, min_disparity, max_disparity, P1, P2_Int, cost_init_, cost_aggr_8_, false);


    // 把4/8个方向加起来
    for(sint32 i =0;i<size;i++) {
    	cost_aggr_[i] = cost_aggr_1_[i] + cost_aggr_2_[i] + cost_aggr_3_[i] + cost_aggr_4_[i];
    	if (option_.num_paths == 8) {
            cost_aggr_[i] += cost_aggr_5_[i] + cost_aggr_6_[i] + cost_aggr_7_[i] + cost_aggr_8_[i];
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

嗯，这节省力了！（其实这是应该的，前面架构都搭好了，后面就越写越轻松。）

实验

春暖花开，又到了喜闻悦见的实验环节。

国际惯例，首先贴核线像对：

实验(1)：只做从左上到右下聚合：

实验(2)：只做从右下到左上聚合：

实验(3)：只做从右上到左下聚合：

实验(4)：只做从左下到右上聚合：

实验(5)：从左上到右下+从右下到左上聚合：

实验(6)：从右上到左下+从左下到右上聚合：

实验(7)：4-路径聚合：（感觉似曾相识？）

实验(8)：8-路径聚合：

发现了什么？4路径效果图是不是哪儿见过？8路径感觉也没好多少？

嗯，感觉是对的，对角线4路径和上篇4路径效果很接近，8路径并不会有很大的提升，我说过8路径有的会偷懒不干事（当然提升确实也是有的，我承认，Hirschmuller老爷子不要打我，去统计错误匹配率就能发现了）。我们再放一起贴一下两个4路径以及8路径的效果：

话说回来，8-路径提升有限主要还是因为4-路径已经达到了较好的效果，提升幅度有限。4-邻域和8-邻域在图像处理领域本来也是二可选其一的选项。

劝大家不要去打2-路径的主意，真不行！

代价聚合到此篇就宣告完结，后面将给大家介绍的是视差优化。

最后大家对完整代码感兴趣，请移步开源项目Github/GemiGlobalMatching下载全代码，点下项目右上角的star，有更新会实时通知到你的个人中心！。

敬礼！

下篇：恒叨立码|码上教学|立体匹配系列|经典SGM：5 视差优化

理论恒叨系列

【理论恒叨】【立体匹配系列】经典SGM：（1）匹配代价计算之互信息（MI）
【理论恒叨】【立体匹配系列】经典SGM：（2）匹配代价计算之Census变换
【理论恒叨】【立体匹配系列】经典SGM：（3）代价聚合（Cost Aggregation）
【理论恒叨】【立体匹配系列】经典SGM：（4）视差计算、视差优化

博主简介：
Ethan Li 李迎松
武汉大学 摄影测量与遥感专业博士

主方向立体匹配、三维重建

2019年获测绘科技进步一等奖（省部级）

爱三维，爱分享，爱开源
GitHub： https://github.com/ethan-li-coding
邮箱：ethan.li.whu@gmail.com

个人微信：

欢迎交流！

喜欢博主的文章不妨关注一下博主的博客，感谢！
博客主页：https://ethanli.blog.csdn.net/