碼上教學系列
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（1）框架與類設計
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（2）代價計算
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（3）代價聚合
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（4）代價聚合2
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（5）視差優化
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（6）視差填充
【恆叨立碼】【碼上教學】【立體匹配系列】經典SGM：（7）弱紋理優化
代碼已同步於Github開源項目：Github/GemiGlobalMatching
上一篇博客框架與類設計中，我們已經介紹了SemiGlobalMatching類的設計，這篇博客的內容自然是類的成員函數的實現，爲一副空皮囊加入血肉之軀。
公有成員函數實現
上篇博客中提到三個共有函數：Initialize、Match、Reset，我們一個個來實現。
首先是Initialize，它的接口定義如下：
/**
* \brief 類的初始化，完成一些內存的預分配、參數的預設置等
* \param width 輸入，核線像對影像寬
* \param height 輸入，核線像對影像高
* \param option 輸入，SemiGlobalMatching參數
*/
bool Initialize(const sint32& width, const sint32& height, const SGMOption& option);
初始化的工作有兩項：
（1）給成員變量及SemiGlobalMatching參數賦值
（2）給成員變量中的數組開闢內存空間並初始化
因此，我們的函數體實現也是執行這兩部分的工作，如下：
bool SemiGlobalMatching::Initialize(const sint32& width, const sint32& height, const SGMOption& option)
{
// ··· 賦值
// 影像尺寸
width_ = width;
height_ = height;
// SGM參數
option_ = option;
if(width == 0 || height == 0) {
return false;
}
//··· 開闢內存空間
// census值（左右影像）
census_left_ = new uint32[width * height]();
census_right_ = new uint32[width * height]();
// 匹配代價（初始/聚合）
const sint32 disp_range = option.max_disparity - option.min_disparity;
if (disp_range <= 0) {
return false;
}
cost_init_ = new uint8[width * height * disp_range]();
cost_aggr_ = new uint16[width * height * disp_range]();
// 視差圖
disp_left_ = new float32[width * height]();
is_initialized_ = census_left_ && census_right_ && cost_init_ && cost_aggr_ && disp_left_;
return is_initialized_;
}
其次是Match,它的接口定義如下：
/**
* \brief 執行匹配
* \param img_left 輸入，左影像數據指針
* \param img_right 輸入，右影像數據指針
* \param disp_left 輸出，左影像視差圖指針，預先分配和影像等尺寸的內存空間
*/
bool Match(const uint8* img_left, const uint8* img_right, float32* disp_left);
上篇博客中，我們已經闡述了SGM匹配的算法步驟，所以匹配函數體裏就按順序執行四個子步驟即可，邏輯上很清晰。四個子步驟我們都放到了私有成員函數裏，後面我們會重點實現它們。
Match的函數體實現如下：
bool SemiGlobalMatching::Match(const uint8* img_left, const uint8* img_right, float32* disp_left)
{
if(!is_initialized_) {
return false;
}
if (img_left == nullptr || img_right == nullptr) {
return false;
}
img_left_ = img_left;
img_right_ = img_right;
// census變換
CensusTransform();
// 代價計算
ComputeCost();
// 代價聚合
CostAggregation();
// 視差計算
ComputeDisparity();
// 輸出視差圖
memcpy(disp_left, disp_left_, width_ * height_ * sizeof(float32));
return true;
}
最後是Reset。Reset實際上等同於在影像尺寸和參數改變後重新做一遍初始化，但是在做之前我們要把之前分配的內存空間都清理掉，不然會出現內存泄漏。
所以Reset做的就是 “清理內存+初始化” 兩步，實現如下：
bool SemiGlobalMatching::Reset(const sint32& width, const sint32& height, const SGMOption& option)
{
// 釋放內存
if (census_left_ != nullptr) {
delete[] census_left_;
census_left_ = nullptr;
}
if (census_right_ != nullptr) {
delete[] census_right_;
census_right_ = nullptr;
}
if (cost_init_ != nullptr) {
delete[] cost_init_;
cost_init_ = nullptr;
}
if (cost_aggr_ != nullptr) {
delete[] cost_aggr_;
cost_aggr_ = nullptr;
}
if (disp_left_ != nullptr) {
delete[] disp_left_;
disp_left_ = nullptr;
}
// 重置初始化標記
is_initialized_ = false;
// 初始化
return Initialize(width, height, option);
}
看到釋放內存，我們應該會想到析構函數的實現，因爲通常析構函數中一般會有內存釋放的步驟，所以我們把內存釋放操作也在析構函數中做一遍。如下：
SemiGlobalMatching::~SemiGlobalMatching()
{
if (census_left_ != nullptr) {
delete[] census_left_;
census_left_ = nullptr;
}
if (census_right_ != nullptr) {
delete[] census_right_;
census_right_ = nullptr;
}
if (cost_init_ != nullptr) {
delete[] cost_init_;
cost_init_ = nullptr;
}
if (cost_aggr_ != nullptr) {
delete[] cost_aggr_;
cost_aggr_ = nullptr;
}
if(disp_left_ != nullptr) {
delete[] disp_left_;
disp_left_ = nullptr;
}
is_initialized_ = false;
}
在Github線上代碼中，我把重複的釋放內存代碼放到一個Release私有成員函數中，這樣不用重複寫代碼了。
私有成員函數實現
上篇博客介紹到，SGM匹配的四個子步驟：Census變換、代價計算、代價聚合、視差計算，都放到了私有成員函數中，所以這四個函數實際上是匹配類的核心。基於篇幅，本篇博客中，我們會介紹其中三個步驟的實現：Census變換、代價計算、視差計算。而最爲核心的一步代價聚合，我們放到下篇博客中爲大家介紹。在這三步實現後，我們就可以做實驗看到視差圖結果了，但是效果肯定一般，因爲沒有代價聚合的SGM是沒有靈魂的。
話不多說，首先來看Census變換。
Census變換是根據窗口內鄰域像素和中心像素的大小比較而生成一個0/1位串（1011011000這樣的位串），原理參見我前面的博客雙目立體匹配經典算法之Semi-Global Matching（SGM）概述：匹配代價計算之Census變換（Census Transform，CT）（附計算C代碼），原理上其實比較簡單：逐像素選擇特定尺寸的窗口逐一和中心像素比較大小，比較結果組成位串（大於就是1，小於就是0）。
因爲census是和類數據無關的，輸入任意的圖像數據，就可以得到census值數組，所以我寫了一個獨立的census變換函數：census_transform_5x5，接口定義如下：
/**
* \brief census變換
* \param source 輸入，影像數據
* \param census 輸出，census值數組
* \param width 輸入，影像寬
* \param height 輸入，影像高
*/
void census_transform_5x5(const uint8* source, uint32* census, const sint32& width, const sint32& height);
輸入，影像數據和寬高，就可以得到census變換值。
它的實現如下：
void census_transform_5x5(const uint8* source, uint32* census, const sint32& width,
const sint32& height)
{
if (source == nullptr || census == nullptr || width <= 5u || height <= 5u) {
return;
}
// 逐像素計算census值
for (sint32 i = 2; i < height - 2; i++) {
for (sint32 j = 2; j < width - 2; j++) {
// 中心像素值
const uint8 gray_center = source[i * width + j];
// 遍歷大小爲5x5的窗口內鄰域像素，逐一比較像素值與中心像素值的的大小，計算census值
uint32 census_val = 0u;
for (sint32 r = -2; r <= 2; r++) {
for (sint32 c = -2; c <= 2; c++) {
census_val <<= 1;
const uint8 gray = source[(i + r) * width + j + c];
if (gray < gray_center) {
census_val += 1;
}
}
}
// 中心像素的census值
census[i * width + j] = census_val;
}
}
}
我爲什麼要加5x5呢，大家可以思考下哦！如果不是5x5比如是9x7該如何實現呢？交給大家吧！
有了這個獨立的census計算函數，我們就可以完成Census變換的私有成員函數實現了，如下所示：
void SemiGlobalMatching::CensusTransform() const
{
// 左右影像census變換
sgm_util::census_transform_5x5(img_left_, census_left_, width_, height_);
sgm_util::census_transform_5x5(img_right_, census_right_, width_, height_);
}
sgm_util是一個命名空間，我把所有獨立的方法都放到此命名空間裏，作爲一個獨立方法集來管理。在Github線上代碼中，我們把這類函數的定義和實現都放到sgm_util.h和sgm_util.cpp文件裏。
然後我們來看代價計算。博客雙目立體匹配經典算法之Semi-Global Matching（SGM）概述：匹配代價計算之Census變換（Census Transform，CT）（附計算C代碼）中也介紹了基於Census變換怎麼計算代價值，非常的簡單，就是計算兩個census值的漢明（hamming）距離，也就是兩個位串中不同的位的個數，計算方式如下：
uint16 Hamming32(const uint32& x, const uint32& y)
{
uint32 dist = 0, val = x ^ y;
// Count the number of set bits
while (val) {
++dist;
val &= val - 1;
}
return dist;
}
這是一個效率比較高的計算方式，當然還有更高效的算法，比如使用查找表技術，同學們可以下去自己探索。
有了漢明距離的計算算法，我們就可以比較輕鬆的實現代價計算了，簡單原理就是對左影像每個像素，在視差範圍內給定一個視差值，可以定位到右影像中的一個像素，最後計算這兩個像素的census值的漢明距離。
有一個很重要的點必須說明：代價數組的主序爲視差主序。
代價數組有三個維度：行、列、視差，視差主序的意思是同一個像素點各視差下的代價值緊密排列，也就是代價數組元素的排列順序爲：
(0,0)像素的所有視差對應的代價值；
(0,1)像素的所有視差對應的代價值；
…
…
(0,w-1)像素的所有視差對應的代價值；
(1,0)像素的所有視差對應的代價值；
(1,1)像素的所有視差對應的代價值；
…
…
第(h-1,w-1)個像素的所有視差對應的代價值；
這樣排列的好處是：單個像素的代價值存取可以達到很高的效率。這對於大尺寸影像來說可帶來明顯的效率優勢，抑或對於像CUDA這類存儲效率是關鍵因子的平臺來說也有明顯優勢。
代價計算的代碼如下：
void SemiGlobalMatching::ComputeCost() const
{
const sint32& min_disparity = option_.min_disparity;
const sint32& max_disparity = option_.max_disparity;
// 計算代價（基於Hamming距離）
for (sint32 i = 0; i < height_; i++) {
for (sint32 j = 0; j < width_; j++) {
// 左影像census值
const uint32 census_val_l = census_left_[i * width_ + j];
// 逐視差計算代價值
for (sint32 d = min_disparity; d < max_disparity; d++) {
auto& cost = cost_init_[i * width_ * disp_range + j * disp_range + (d - min_disparity)];
if (j - d < 0 || j - d >= width_) {
cost = UINT8_MAX;
continue;
}
// 右影像對應像點的census值
const uint32 census_val_r = census_right_[i * width_ + j - d];
// 計算匹配代價
cost = sgm_util::Hamming32(census_val_l, census_val_r);
}
}
}
}
最後是實現視差計算。
視差計算是採用WTA（Winner Takes All）贏家通喫算法，其實就是在視差範圍內選擇一個代價值最小的視差作爲像素的最終視差。遍歷一遍代價數組就可以了。參考雙目立體匹配經典算法之Semi-Global Matching（SGM）概述：視差計算、視差優化。
原理上，SGM的視差計算，需要遍歷聚合代價數組，但是因爲目前還未實現聚合步驟，所以暫時用初始代價數組來代替，正好也可以實驗下不做聚合是個什麼樣的結果。實現如下：
void SemiGlobalMatching::ComputeDisparity() const
{
// 最小最大視差
const sint32& min_disparity = option_.min_disparity;
const sint32& max_disparity = option_.max_disparity;
// 未實現聚合步驟，暫用初始代價值來代替
auto cost_ptr = cost_init_;
// 逐像素計算最優視差
for (sint32 i = 0; i < height_; i++) {
for (sint32 j = 0; j < width_; j++) {
uint16 min_cost = UINT16_MAX;
uint16 max_cost = 0;
sint32 best_disparity = 0;
// 遍歷視差範圍內的所有代價值，輸出最小代價值及對應的視差值
for (sint32 d = min_disparity; d < max_disparity; d++) {
const sint32 d_idx = d - min_disparity;
const auto& cost = cost_ptr[i * width * disp_range + j * disp_range + d_idx];
if(min_cost > cost) {
min_cost = cost;
best_disparity = d;
}
max_cost = std::max(max_cost, static_cast<uint16>(cost));
}
// 最小代價值對應的視差值即爲像素的最優視差
if (max_cost != min_cost) {
disp_left_[i * width_ + j] = static_cast<float>(best_disparity);
}
else {
// 如果所有視差下的代價值都一樣，則該像素無效
disp_left_[i * width_ + j] = Invalid_Float;
}
}
}
}
至此，四個子步驟中的三個我們已經實現了，有了這三個步驟，就可以做實驗來驗證初始代價下的視差圖了，Let’s do it！
實驗
有了初始代價，我們就可以算出一個視差圖，無論效果如何（當然不好啦，嘿嘿，說了代價聚合纔是SGM靈魂），我們至少可以測試下其他函數的正確性，以及體會代價聚合步驟的重要性！
首先第一個關鍵的步驟是讀核線影像爲SGM類傳入數據。我選擇的影像是stereo經典網站middlebury上的cone數據。如下圖所示：
然後選擇一個圖像庫來讀取圖像，這裏我選擇大家耳熟能詳的視覺算法開源庫OpenCV，版本是310。OpenCV庫比較大，我這裏就不傳到Github上去了，我的百度網盤裏可以下載：
鏈接：https://pan.baidu.com/s/1_WD-KdPyDBazEIim7NU3jA
提取碼：aab4
再一個關鍵的步驟是設計SGM參數，如下：
SemiGlobalMatching::SGMOption sgm_option;
sgm_option.num_paths = 8;
sgm_option.min_disparity = 0;
sgm_option.max_disparity = 64;
sgm_option.p1 = 10;
sgm_option.p2_int = 150;
第一個聚合路徑數，此時實際上沒意義，等聚合步驟實現後纔有效。
第二個第三個是視差範圍，這是根據核線像對的同名點的實際視差範圍來確定的，我們可以根據核線圖像來估計一個比較合理的值，保證所有實際視差值都在範圍內。
第四個第五個是論文裏所說的兩個懲罰項，這兩個是經驗值，首先要保證P2_Int>>P1，具體的值根據經驗來實驗確定，算法對這兩個值不是特別敏感，要求P1和P2差別較大，大家可以自己針對特定數據做一些微調。（P1、P2對視差非連續區像素的結果有一定影響，但整體還是比較魯棒的）
經過上面兩個關鍵步驟後，就可以實現測試代碼了，測試步驟很簡單：
（1）讀核線像對
（2）執行SGM匹配
（3）顯示視差圖
如下：
/**
* \brief
* \param argv 3
* \param argc argc[1]:左影像路徑 argc[2]: 右影像路徑 argc[3]: 視差圖路徑
* \return
*/
int main(int argv,char** argc)
{
if(argv < 3) {
return 0;
}
// ··· 讀取影像
std::string path_left = argc[1];
std::string path_right = argc[2];
cv::Mat img_left = cv::imread(path_left, cv::IMREAD_GRAYSCALE);
cv::Mat img_right = cv::imread(path_right, cv::IMREAD_GRAYSCALE);
if (img_left.data == nullptr || img_right.data == nullptr) {
std::cout << "讀取影像失敗！" << std::endl;
return -1;
}
if (img_left.rows != img_right.rows || img_left.cols != img_right.cols) {
std::cout << "左右影像尺寸不一致！" << std::endl;
return -1;
}
// ··· SGM匹配
const uint32 width = static_cast<uint32>(img_left.cols);
const uint32 height = static_cast<uint32>(img_right.rows);
SemiGlobalMatching::SGMOption sgm_option;
sgm_option.num_paths = 8;
sgm_option.min_disparity = 0;
sgm_option.max_disparity = 64;
sgm_option.p1 = 10;
sgm_option.p2_int = 150;
SemiGlobalMatching sgm;
// 初始化
if(!sgm.Initialize(width, height, sgm_option)) {
std::cout << "SGM初始化失敗！" << std::endl;
return -2;
}
// 匹配
auto disparity = new float32[width * height]();
if(!sgm.Match(img_left.data,img_right.data,disparity)) {
std::cout << "SGM匹配失敗！" << std::endl;
return -2;
}
// 顯示視差圖
cv::Mat disp_mat = cv::Mat(height, width, CV_8UC1);
for (uint32 i=0;i<height;i++) {
for(uint32 j=0;j<width;j++) {
const float32 disp = disparity[i * width + j];
if (disp == Invalid_Float) {
disp_mat.data[i * width + j] = 0;
}
else {
disp_mat.data[i * width + j] = 2 * static_cast<uchar>(disp);
}
}
}
cv::imwrite(argc[3], disp_mat);
cv::imshow("視差圖", disp_mat);
cv::waitKey(0);
delete[] disparity;
disparity = nullptr;
return 0;
}
最後實驗結果如下：
哈哈，這效果，是不是略有點辣眼睛呢！還好前面打了預防針了！
別擔心，下篇博客我們來讓它大變身！
下篇：恆叨立碼|碼上教學|立體匹配系列|經典SGM：3 代價聚合
理論恆叨系列
【恆叨立碼】【理論恆叨】【立體匹配系列】經典SGM：（1）匹配代價計算之互信息（MI））
【恆叨立碼】【理論恆叨】【立體匹配系列】經典SGM：（2）匹配代價計算之Census變換
【恆叨立碼】【理論恆叨】【立體匹配系列】經典SGM：（3）代價聚合（Cost Aggregation）
【恆叨立碼】【理論恆叨】【立體匹配系列】經典SGM：（4）視差計算、視差優化