Dr. Li Yingsong’s Binocular Stereo Matching-Code Practice-2 Cost Calculation_Source Code Display

Dr. Li Yingsong’s Binocular Stereo Matching-Code Practice-2 Cost Calculation_Source Code Display

• Beginner’s thoughts
• • Code entry
  • • 1.SemiGlobalMatching.h file
    • 2. Next is the SemiGlobalMatching.cpp file, which mainly implements the methods in the SemiGlobalMatching.h file
    • 3. There are relatively few sgm_util.h functions here. I won’t explain too much. You can copy and paste them directly:
    • 4. Next is the sgm_util.cpp code, which is also given directly
    • 5. Then there is the main function, I still give it directly

Beginners’ thoughts

When I was studying 3D vision, I saw the article written by Dr. Li Yingsong, and I benefited a lot. However, since I have no practical experience in C++, I stepped on many pitfalls during the operation process. The source code directly downloaded from the git on the webpage also reached I didn’t meet the requirement of learning this chapter, and the amount of code was relatively large. I really couldn’t find it, so I decided to reproduce the code by myself. But I didn’t expect to reproduce the second piece of code in a whole day, which is really not easy. I hope that when people come in to learn later, they can have a complete running code, run it first, and then find the code to learn from it.

Code entry

1.SemiGlobalMatching.h file

This file is mainly introduced in the first lecture, mainly the declaration method, which needs to be implemented in the following. I will not elaborate too much here, and give you the complete code

#pragma once
#include <cstdint>

typedef int8_t sint8; // signed 8-bit integer
typedef uint8_t uint8; // unsigned 8-bit integer
typedef int16_t sint16; // signed 16-bit integer
typedef uint16_t uint16; // unsigned 16-bit integer
typedef int32_t sint32; // signed 32-bit integer
typedef uint32_t uint32; // unsigned 32-bit integer
typedef int64_t sint64; // signed 64-bit integer
typedef uint64_t uint64; // unsigned 64-bit integer
typedef float float32; // single precision floating point
typedef double float64; // double precision floating point

class SemiGlobalMatching
{<!-- -->
public:
SemiGlobalMatching();
~SemiGlobalMatching();

\t
/** \brief SGM parameter structure */
struct SGMOption {<!-- -->
uint8 num_paths; // Number of aggregate paths
sint32 min_disparity; // minimum disparity
sint32 max_disparity; // Maximum disparity

//P1,P2
// P2 = P2_int / (Ip-Iq)
sint32 p1; // Penalty term parameter P1
sint32 p2_int; // Penalty parameter P2

SGMOption() : num_paths(8), min_disparity(0), max_disparity(64), p1(10), p2_int(150) {<!-- -->
}

};
public:
/**
* Initialization of the \brief class, completing some memory pre-allocation, parameter pre-setting, etc.
* \param width input, epipolar line image width
* \param height input, epipolar line image height
* \param option input, SemiGlobalMatching parameter
*/
//bool Initialize(const uint32 & amp; width, const uint32 & amp; height, const SGMOption & amp; option);
bool Initialize(const sint32 & amp; width, const sint32 & amp; height, const SGMOption & amp; option);
/**
* \brief perform matching
* \param img_left input, left image data pointer
* \param img_right input, right image data pointer
* \param disp_left output, left image depth map pointer, pre-allocated memory space with the same size as the image
*/
bool Match(const uint8* img_left, const uint8* img_right, float32* disp_left);

/**
* \brief reset
* \param width input, epipolar line image width
* \param height input, epipolar line image height
* \param option input, SemiGlobalMatching parameter
*/
//bool Reset(const uint32 & amp; width, const uint32 & amp; height, const SGMOption & amp; option);
bool Reset(const sint32 & amp; width, const sint32 & amp; height, const SGMOption & amp; option);
/**
 * \brief census transformation
 * \param source input, image data
 * \param census output, census value array
 * \param width input, image width
 * \param height input, image height
 */

private:

/** \brief Census transformation */
void CensusTransform() const;

/** \brief cost calculation */
void ComputeCost() const;

/** \brief cost aggregation */
//void CostAggregation() const;

/** \brief Parallax calculation */
void ComputeDisparity() const;


private:
/** \brief SGM parameters */
SGMOption option_;

/** \brief image width */
sint32 width_;

/** \brief image height */
sint32 height_;

/** \brief left image data */
const uint8* img_left_;

/** \brief right image data */
const uint8* img_right_;

/** \brief left image census value */
uint32* census_left_;

/** \brief Right image census value */
uint32* census_right_;

/** \brief Initial matching cost */
uint8* cost_init_;

/** \brief Aggregation matching cost */
uint16* cost_aggr_;

/** \brief Left image disparity map */
float32* disp_left_;

/** \brief Whether to initialize the flag */
bool is_initialized_;

};

There is still a difference between what the teacher gave you. What is the specific difference? Let me explain.
This is a small part of what I changed

const uint8* img_left_;
const uint8* img_right_;

The teacher’s original text is as follows:

uint8* img_left_;
uint8* img_right_;

An error will be reported in the SemiGlobalMatching.cpp text. The specific error is as shown in the figure below, because it is const uint in the implementation declaration, but there is no such declaration in the SemiGlobalMatching.h file, so students should pay attention to equality.

2. Next is the SemiGlobalMatching.cpp file, which mainly implements the methods in the SemiGlobalMatching.h file

If you just follow the teacher and paste it bit by bit, there will be many pitfalls. It is better to write the code first and then explain in detail where the pitfalls are:
1. All references must be given. If there are too few references, the code will not be able to run through.
2. The teacher clearly explained that the Cost Aggregation module is not covered, but it is not covered, so we need to comment out this code in the .h and .cpp files first, one step one step at a time. If it is not commented, it will not work in the code.
3. In the void SemiGlobalMatching::CensusTransform() const method, we need to add this line of code, otherwise this error will be reported in the text

const sint32 disp_range = max_disparity - min_disparity;
    if (disp_range <= 0) {<!-- -->
        return;

4. Similarly, it must be declared in 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_;

Of course, what I posted below is the complete code I have modified. You can directly copy mine. My explanation is to be able to read the teacher’s article, where the code does not work and there are deficiencies, and then come to me to see where the specific problem is.

#include "stdafx.h"
#include "SemiGlobalMatching.h"
#include "sgm_util.h"
#include 
#include 
#include 
#include 
#include 
using namespace std::chrono;

SemiGlobalMatching::SemiGlobalMatching() {
}
bool SemiGlobalMatching::Initialize(const sint32 & amp; width, const sint32 & amp; height, const SGMOption & amp; option)
{
    // ··· assignment

    // image size
    width_ = width;
    height_ = height;
    //SGM parameters
    option_ = option;

    if (width == 0 || height == 0) {
        return false;
    }

    //··· Open up memory space

    // census value (left and right images)
    census_left_ = new uint32[width * height]();
    census_right_ = new uint32[width * height]();

    // Matching cost (initial/aggregation)
    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]();

    // disparity map
    disp_left_ = new float32[width * height]();

    is_initialized_ = census_left_ & amp; & amp; census_right_ & amp; & amp; cost_init_ & amp; & amp; cost_aggr_ & amp; & amp; 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 transformation
    CensusTransform();

    // cost calculation
    ComputeCost();

    // cost aggregation
    //CostAggregation();

    // disparity calculation
    ComputeDisparity();

    // output disparity map
    memcpy(disp_left, disp_left_, width_ * height_ * sizeof(float32));

    return true;
}

bool SemiGlobalMatching::Reset(const sint32 & amp; width, const sint32 & amp; height, const SGMOption & amp; option)
{
    // release memory
    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;
    }

    // reset initialization flag
    is_initialized_ = false;

    // initialization
    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
{
    // Left and right image census transformation
    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;
    }
    // Calculate cost (based on Hamming distance)
    for (sint32 i = 0; i < height_; i ++ ) {
        for (sint32 j = 0; j < width_; j ++ ) {

            // Left image census value
            const uint32 census_val_l = census_left_[i * width_ + j];

            // Calculate the cost value per disparity
            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;
                }
                // The census value of the image point corresponding to the right image
                const uint32 census_val_r = census_right_[i * width_ + j - d];

                // Calculate matching cost
                cost = sgm_util::Hamming32(census_val_l, census_val_r);
            }
        }
    }
}

void SemiGlobalMatching::ComputeDisparity() const
{
    // minimum and maximum parallax
    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;
    }
    // The aggregation step is not implemented, temporarily use the initial cost value instead
    auto cost_ptr = cost_init_;
    const sint32 width = width_;
    const sint32 height = height_;
    // Calculate the optimal disparity pixel by pixel
    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;

            // Traversing all cost values within the disparity range, output the minimum cost value and the corresponding disparity value
            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(cost));
            }

            // The disparity value corresponding to the minimum cost value is the optimal disparity of the pixel
            if (max_cost != min_cost) {
                disp_left_[i * width_ + j] = static_cast(best_disparity);
            }
            else {
                // If the cost value is the same under all disparities, the pixel is invalid
                disp_left_[i * width_ + j] = Invalid_Float;
            }
        }
    }
}

3.sgm_util.h function, here are relatively few, I will not explain too much, you can directly copy and paste:

#pragma once
#include <cstdint>
#include <limits>

/** \brief float invalid value */
constexpr auto Invalid_Float = std::numeric_limits<float>::infinity();

/** \brief basic type alias */
typedef int8_t sint8; // signed 8-bit integer
typedef uint8_t uint8; // unsigned 8-bit integer
typedef int16_t sint16; // signed 16-bit integer
typedef uint16_t uint16; // unsigned 16-bit integer
typedef int32_t sint32; // signed 32-bit integer
typedef uint32_t uint32; // unsigned 32-bit integer
typedef int64_t sint64; // signed 64-bit integer
typedef uint64_t uint64; // unsigned 64-bit integer
typedef float float32; // single precision floating point
typedef double float64; // double precision floating point

4. Next is the sgm_util.cpp code, which is also given directly

#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 & amp; width, const sint32 & amp; height)
{<!-- -->
if (source == nullptr || census == nullptr || width <= 5u || height <= 5u) {<!-- -->
return;
}

// Calculate the census value pixel by pixel
for (sint32 i = 2; i < height - 2; i ++ ) {<!-- -->
for (sint32 j = 2; j < width - 2; j ++ ) {<!-- -->

// center pixel value
const uint8 gray_center = source[i * width + j];

// Traverse the neighborhood pixels in the window with a size of 5x5, compare the pixel value with the center pixel value one by one, and calculate the census value
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;
}
}
}

// The census value of the center pixel
census[i * width + j] = census_val;
}
}
}

uint8 sgm_util::Hamming32(const uint32 & amp; x, const uint32 & amp; y)
{<!-- -->
uint32 dist = 0, val = x ^ y;

// Count the number of set bits
while (val) {<!-- -->
+ + dist;
val & amp;= val - 1;
}

return static_cast<uint8>(dist);
}

5. Then there is the main function. I still give it directly

It is worth mentioning in this section:

I //dropped the original code, but it will throw an exception, and then the program will terminate, which is very troublesome, I can’t see how to implement it, so I added a try ccatch to hide the exception and continue running, and found that it does not affect the implementation that’s it

#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]: left image path argc[2]: right image path argc[3]: disparity map path
 * \return
 */
int main(int argv, char** argc)
{<!-- -->
    if (argv < 3) {<!-- -->
        return 0;
    }

    // ··· read image
    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 << "Failed to read image!" << std::endl;
        return -1;
    }
    if (img_left.rows != img_right.rows || img_left.cols != img_right.cols) {<!-- -->
        std::cout << "The left and right image sizes are inconsistent!" << std::endl;
        return -1;
    }

    // ··· SGM matching
    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;

    // initialization
    if (!sgm.Initialize(width, height, sgm_option)) {<!-- -->
        std::cout << "SGM initialization failed!" << std::endl;
        return -2;
    }

    // match
    auto disparity = new float32[width * height]();
    if (!sgm.Match(img_left.data, img_right.data, disparity)) {<!-- -->
        std::cout << "SGM match failed!" << std::endl;
        return -2;
    }

    //Display disparity map
    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("Disparity map", disp_mat);
    cv::waitKey(0);

    delete[] disparity;
    disparity = nullptr;

    return 0;
}

The other codes are really nothing, just copy them directly from the original author git, I will share all the differences here, if you find it troublesome, I will also put all the codes in the package, just find it in the file I uploaded, Of course, the most important part here is to configure opencv. I will show you how to configure opencv in the next section and tell you about the pitfalls I have encountered.
Source code of this article: https://download.csdn.net/download/weixin_43511871/88250514
Original text by Dr. Li Yingsong: https://ethanli.blog.csdn.net/article/details/105142484
Thank you everyone~!