手把手教你編寫SGM雙目立體匹配代碼（基於C++，Github同步更新）（四）（代價聚合2）

昔人已乘黃鶴去，此地空餘黃鶴樓。

2020對武漢、對中國、對世界來說是異常艱難的一年。武漢壯士扼腕，封一城而救一國，引得八方救援，舉國抗疫。中國人在災難面前總是空前團結，勇往直前！中華民族幾千年來從未向任何惡勢力低頭，必定會徹底戰勝病毒，重振輝煌！今天，武漢全面接觸離漢通道，標誌着我國抗疫的階段性勝利，真讓人熱淚盈眶，衷心祝願各位身體健康，遠離疾病！風雨過後，一切付出終會有回報！

黃鶴歸來碧空淨，乘風萬里入青雲！
2020年4月8日 中國武漢 晴

思緒拉回來，上回咱們說到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個像素開始按順序聚合
		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 == 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;
			gray = *img_col;
		}
	}
}

你品，你細品，是不是就下個路徑的計算方法不一樣，並增加了觸碰邊界處理（咦？邊界是啥？兄弟！別問！自己感受！我總不能在代碼裏寫南牆！）

自我吐槽一下，函數名字取的不行，什麼叫對角線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;
		}
	}
}

對角線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)：4-路徑聚合：（感覺似曾相識？） 實驗(8)：8-路徑聚合：

發現了什麼？4路徑效果圖是不是哪兒見過？8路徑感覺也沒好多少？

嗯，感覺是對的，對角線4路徑和上篇4路徑效果很接近，8路徑並不會有很大的提升，我說過8路徑有的會偷懶不幹事（當然提升確實也是有的，我承認，Hirschmuller老爺子不要打我，去統計錯誤匹配率就能發現了）。我們再放一起貼一下兩個4路徑以及8路徑的效果：

4路徑-1

4路徑-2

8-路徑

話說回來，8-路徑提升有限主要還是因爲4-路徑已經達到了較好的效果，提升幅度有限。4-鄰域和8-鄰域在圖像處理領域本來也是二可選其一的選項。

我勸大家不要去打2-路徑的主意，真不行！

代價聚合到此篇就宣告完結，後面將給大家介紹的是子像素擬合和一致性檢查。再往後，就不知道還有沒有了…（真的！教完了，沒貨了！）

最後大家對全代碼感興趣，請移步Github/GemiGlobalMatching下載全代碼，點下右上角的star，有更新會實時通知到你的個人中心！。

敬禮！