Simple parallelized sum of absolute differences (SAD) stereo matching implementation

dumbsad.cpp

#include <cstdint>
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>        
#include <opencv2/core/hal/interface.h>
#include <opencv2/core/matx.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <optional>

void sadmatch(const cv::Mat& img1, const cv::Mat& img2, cv::Mat& disp, int bsize) {
    int bsize2 = bsize / 2;

    const cv::Size size = img1.size();
    disp.create(size.height - 2 * bsize2, size.width - 2 * bsize2, CV_16SC1);
    disp.setTo(0);
    
    cv::parallel_for_(cv::Range(bsize2, size.height - bsize2), [&](const cv::Range& r){
        for (int row = r.start; row < r.end; ++row) {
            for (int col = bsize2; col < size.width - bsize2; ++col) {

                int best_rcol;
                uint64_t best_sad = UINT64_MAX;
                for (int rcol = bsize2; rcol < size.width - bsize2; ++rcol) {

                    uint64_t sad = 0;
                    for (int by = -bsize2; by < bsize2 + 1; ++by)
                        for (int bx = -bsize2; bx < bsize2 + 1; ++bx)
                            sad += std::abs( signed(img1.at<uchar>(row + by, col + bx)) - signed(img2.at<uchar>(row + by, rcol + bx)) );

                    if (sad < best_sad) {
                        best_sad = sad;
                        best_rcol = rcol;
                    }
                }

                disp.at<int16_t>(row - bsize2, col - bsize2) = std::abs(best_rcol - col);
            }
        }
    });
}

void aggree(const cv::Mat& disp1, const cv::Mat& disp2, cv::Mat& agreement, int maxdiff) {
    auto size = disp1.size();

    agreement.create(size, disp1.type());

    cv::parallel_for_(cv::Range(0, size.height), [&](const cv::Range& r) {
        for (int row = r.start; row < r.end; ++row) {
            for (int col = 0; col < size.width; ++col) {

                int16_t l = disp1.at<int16_t>(row, col),
                        r = disp2.at<int16_t>(row, col);

                
                agreement.at<int16_t>(row, col) = maxdiff < std::abs(l - r) ? -1 : l;
            }
        }
    });
}

int main(int argc, char const* const* argv)
{
    if (argc < 4)
    {
        std::cerr << "Usage: " << argv[0] << " <image file> <image file> <bsize> [maxdiff] [maxdisp]" << std::endl;
        return 1;
    }

    cv::Mat img1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
    cv::Mat img2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
    int blocksize = std::stoi(argv[3]);

    std::optional<int> maxdiff, maxdisp;
    if (4 < argc)
        maxdiff = std::stoi(argv[4]);
    if (5 < argc)
        maxdisp = std::stoi(argv[5]);

    if (blocksize % 2 == 0)
    {
        std::cerr << "Error: block size must be odd" << std::endl;
        return 1;
    }

    if (img1.empty() || img2.empty())
    {
        std::cerr << "Error: cannot read image file" << std::endl;
        return 1;
    }

    if (img1.size() != img2.size())
    {
        std::cerr << "Error: image size is different" << std::endl;
        return 1;
    }

    if (img1.type() != CV_8UC1 || img2.type() != CV_8UC1)
    {
        std::cerr << "Error: both images must be CV_8UC1" << std::endl;
        return 1;
    }

    cv::Mat lrdisp, rldisp, agreement;
    sadmatch(img1, img2, lrdisp, blocksize);
    sadmatch(img2, img1, rldisp, blocksize);

    if (maxdiff.has_value()) {
        aggree(lrdisp, rldisp, agreement, maxdiff.value());

        if (maxdisp.has_value()) {
            agreement.setTo(-1, maxdisp.value() < agreement);
        }
    } else {
        agreement = lrdisp;
    }

    cv::Mat normalized;
    cv::normalize(agreement, normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    cv::imwrite("disparity.png", normalized);

    return 0;
}