李迎松博士双目立体匹配-码上实践-2代价计算_源代码展示
初学碎碎念
我在学习3d视觉时，看到了李迎松博士写的文章，受益颇多，但由于我没有c++实战经历，在运行过程中踩了许多的坑，在网页上直接git下的源码，也达不到我想学习这一章的要求，再加上代码量比较多，我确实找不到在哪，就决定自己复现出来代码。但没想到一整天才复现了第二段代码，实在是不容易。希望后来人进来学习之时能够有一个完好的运行代码，先运行，再从中找代码学习。
代码录入
1.SemiGlobalMatching.h文件
这个文件在第一讲中主要介绍，主要是声明方法，在下文中需要实现它，这里我不进行过多的阐述，将代码完整的给到大家
#pragma once
#include <cstdint>
typedef int8_t sint8; // 有符号8位整数
typedef uint8_t uint8; // 无符号8位整数
typedef int16_t sint16; // 有符号16位整数
typedef uint16_t uint16; // 无符号16位整数
typedef int32_t sint32; // 有符号32位整数
typedef uint32_t uint32; // 无符号32位整数
typedef int64_t sint64; // 有符号64位整数
typedef uint64_t uint64; // 无符号64位整数
typedef float float32; // 单精度浮点
typedef double float64; // 双精度浮点
class SemiGlobalMatching
{
public:
SemiGlobalMatching();
~SemiGlobalMatching();
/** \brief SGM参数结构体 */
struct SGMOption {
uint8 num_paths; // 聚合路径数
sint32 min_disparity; // 最小视差
sint32 max_disparity; // 最大视差
// P1,P2
// P2 = P2_int / (Ip-Iq)
sint32 p1; // 惩罚项参数P1
sint32 p2_int; // 惩罚项参数P2
SGMOption() : num_paths(8), min_disparity(0), max_disparity(64), p1(10), p2_int(150) {
}
};
public:
/**
* \brief 类的初始化，完成一些内存的预分配、参数的预设置等
* \param width 输入，核线像对影像宽
* \param height 输入，核线像对影像高
* \param option 输入，SemiGlobalMatching参数
*/
//bool Initialize(const uint32& width, const uint32& height, const SGMOption& option);
bool Initialize(const sint32& width, const sint32& height, const SGMOption& option);
/**
* \brief 执行匹配
* \param img_left 输入，左影像数据指针
* \param img_right 输入，右影像数据指针
* \param disp_left 输出，左影像深度图指针，预先分配和影像等尺寸的内存空间
*/
bool Match(const uint8* img_left, const uint8* img_right, float32* disp_left);
/**
* \brief 重设
* \param width 输入，核线像对影像宽
* \param height 输入，核线像对影像高
* \param option 输入，SemiGlobalMatching参数
*/
//bool Reset(const uint32& width, const uint32& height, const SGMOption& option);
bool Reset(const sint32& width, const sint32& height, const SGMOption& option);
/**
* \brief census变换
* \param source 输入，影像数据
* \param census 输出，census值数组
* \param width 输入，影像宽
* \param height 输入，影像高
*/
private:
/** \brief Census变换 */
void CensusTransform() const;
/** \brief 代价计算 */
void ComputeCost() const;
/** \brief 代价聚合 */
//void CostAggregation() const;
/** \brief 视差计算 */
void ComputeDisparity() const;
private:
/** \brief SGM参数 */
SGMOption option_;
/** \brief 影像宽 */
sint32 width_;
/** \brief 影像高 */
sint32 height_;
/** \brief 左影像数据 */
const uint8* img_left_;
/** \brief 右影像数据 */
const uint8* img_right_;
/** \brief 左影像census值 */
uint32* census_left_;
/** \brief 右影像census值 */
uint32* census_right_;
/** \brief 初始匹配代价 */
uint8* cost_init_;
/** \brief 聚合匹配代价 */
uint16* cost_aggr_;
/** \brief 左影像视差图 */
float32* disp_left_;
/** \brief 是否初始化标志 */
bool is_initialized_;
};
这里呢跟老师给的还是有区别，具体什么区别呢，我来解释一下
这是我改动的一点小部分
const uint8* img_left_;
const uint8* img_right_;
老师的原文是这样的：
uint8* img_left_;
uint8* img_right_;
在SemiGlobalMatching.cpp文中就会报错，具体错误如下图，因为在实现的声明中是const uint，但是在SemiGlobalMatching.h的文件中却没有这个声明，所以同学们要注意对等。
2. 接下来是SemiGlobalMatching.cpp文件，这个文件中主要实现SemiGlobalMatching.h文件中的方法
这里如果只是照着老师一点一点粘贴就有许多的坑，还是先上代码，再具体说明坑在哪：
1.引用要给全，少引用在代码上就无法跑通
2.老师这一讲明确的说明代价聚合模块是不讲的，竟然是不讲的，那我们就要在.h和.cpp文件中吧这个代码先注释掉，一步一步来。如若不注释，在代码中也是无法运行的。
3.在void SemiGlobalMatching::CensusTransform() const这个方法中，我们要加这一行代码，否则会在文中报这个错误
const sint32 disp_range = max_disparity - min_disparity;
if (disp_range <= 0) {
return;
4.同理在void SemiGlobalMatching::ComputeDisparity() const中也是要声明的
const sint32 disp_range = max_disparity - min_disparity;
if (disp_range <= 0) {
return;
}
const sint32 width = width_;
const sint32 height = height_;
当然我发下面的是我修改过的完整代码，可以直接复制我的我解释是为了能够看老师的文章有代码跑不通有不足之处，再来到我这看看具体问题出在哪里。
#include "stdafx.h"
#include "SemiGlobalMatching.h"
#include "sgm_util.h"
#include <utility>
#include <algorithm>
#include <vector>
#include <cassert>
#include <chrono>
using namespace std::chrono;
SemiGlobalMatching::SemiGlobalMatching() {
}
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_;
}
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;
}
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;
}
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_);
}
void SemiGlobalMatching::ComputeCost() const
{
const sint32& min_disparity = option_.min_disparity;
const sint32& max_disparity = option_.max_disparity;
const sint32 disp_range = max_disparity - min_disparity;
if (disp_range <= 0) {
return;
}
// 计算代价（基于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 / 2;
continue;
}
// 右影像对应像点的census值
const uint32 census_val_r = census_right_[i * width_ + j - d];
// 计算匹配代价
cost = sgm_util::Hamming32(census_val_l, census_val_r);
}
}
}
}
void SemiGlobalMatching::ComputeDisparity() const
{
// 最小最大视差
const sint32& min_disparity = option_.min_disparity;
const sint32& max_disparity = option_.max_disparity;
const sint32 disp_range = max_disparity - min_disparity;
if (disp_range <= 0) {
return;
}
// 未实现聚合步骤，暂用初始代价值来代替
auto cost_ptr = cost_init_;
const sint32 width = width_;
const sint32 height = height_;
// 逐像素计算最优视差
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;
}
}
}
}
3.sgm_util.h函数，这里比较少，我不做过多解释，大家可以直接复制粘贴：
#pragma once
#include <cstdint>
#include <limits>
/** \brief float无效值 */
constexpr auto Invalid_Float = std::numeric_limits<float>::infinity();
/** \brief 基础类型别名 */
typedef int8_t sint8; // 有符号8位整数
typedef uint8_t uint8; // 无符号8位整数
typedef int16_t sint16; // 有符号16位整数
typedef uint16_t uint16; // 无符号16位整数
typedef int32_t sint32; // 有符号32位整数
typedef uint32_t uint32; // 无符号32位整数
typedef int64_t sint64; // 有符号64位整数
typedef uint64_t uint64; // 无符号64位整数
typedef float float32; // 单精度浮点
typedef double float64; // 双精度浮点
4. 接下来是sgm_util.cpp代码，一样直接给上
#include "stdafx.h"
#include "sgm_util.h"
#include <algorithm>
#include <cassert>
#include <vector>
#include <queue>
#include "SemiGlobalMatching.h"
void sgm_util::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;
}
}
}
uint8 sgm_util::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 static_cast<uint8>(dist);
}
5.然后就是main函数 我还是一样直接给出
值得一提的是在这个部分：
我//掉的就是原来的代码，但是会抛出一个异常，然后程序就此终止，这就很麻烦，我看不到怎么实现，于是加了一个try ccatch把异常隐掉继续运行发现不影响实现就可以了
#include "stdafx.h"
#include "SemiGlobalMatching.h"
#include <chrono>
#include <iostream>
using namespace std::chrono;
// opencv library
#include <opencv2/opencv.hpp>
#include "sgm_types.h"
#ifdef _DEBUG
#pragma comment(lib,"opencv_world310d.lib")
#else
#pragma comment(lib,"opencv_world310.lib")
#endif
/**
* \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);
}
}
}
try {
cv::imwrite(argc[3], disp_mat);
}
catch (cv::Exception& e) {
std::cerr << e.what() << std::endl;
}
//cv::imwrite(argc[3], disp_mat);
cv::imshow("视差图", disp_mat);
cv::waitKey(0);
delete[] disparity;
disparity = nullptr;
return 0;
}
其他代码呢确实就没什么了，直接从原作者git来抄就行，我这里把有区别的全部给大家，如果觉得麻烦我也会放上所有代码的打包，在我上传的文件中找即可，当然这里还有最重要的一环就是配置opencv，我下一节展示怎么配置opencv并且把踩过的坑告诉大家。
本文源代码：https://download.csdn.net/download/weixin_43511871/88250514
李迎松博士原文：https://ethanli.blog.csdn.net/article/details/105142484
谢谢大家~！
281
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
