码上教学系列
【恒叨立码】【码上教学】【立体匹配系列】经典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）视差计算、视差优化