双目立体匹配算法：SGM 实时代码

以下是双目立体匹配算法SGM的实时代码：

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

const int max_image_size = 960;
const int max_disparity = 64;
const int P1 = 5;
const int P2 = 70;
const int penalty_scale = 10;
const int uniqueness_ratio = 15;
const int speckle_window_size = 100;
const int speckle_range = 32;

int main(int argc, char** argv) {
    if(argc != 3) {
        cout << "Usage: ./sgm_stereo left_image right_image" << endl;
        return -1;
    }

    Mat imgL = imread(argv[1], IMREAD_GRAYSCALE);
    Mat imgR = imread(argv[2], IMREAD_GRAYSCALE);

    if(imgL.empty() || imgR.empty()) {
        cout << "Error: Could not open or find the images" << endl;
        return -1;
    }

    int width = imgL.cols;
    int height = imgL.rows;

    if(width > max_image_size || height > max_image_size) {
        cout << "Error: Image size too large" << endl;
        return -1;
    }

    int min_disparity = 0;
    int max_disparity = 64;

    Mat disparity_map = Mat::zeros(height, width, CV_8UC1);

    for(int y = 0; y < height; y++) {
        for(int x = 0; x < width; x++) {
            int min_cost = INT_MAX;
            int best_disparity = min_disparity;

            for(int d = min_disparity; d < max_disparity; d++) {
                int sum = 0;
                int count = 0;

                for(int dy = -1; dy <= 1; dy++) {
                    for(int dx = -1; dx <= 1; dx++) {
                        int xl = x + dx;
                        int xr = x + dx - d;

                        if(xl < 0 || xl >= width || xr < 0 || xr >= width) {
                            continue;
                        }

                        int diff = abs((int)imgL.at<uchar>(y+dy, x+dx) - (int)imgR.at<uchar>(y+dy, xr));

                        sum += diff;
                        count++;
                    }
                }

                int cost = sum / count;

                if(d > min_disparity) {
                    int diff = abs(d - best_disparity);

                    cost += (diff == 1) ? P1 : (P1 + (diff - 1) * P2);
                }

                if(cost < min_cost) {
                    min_cost = cost;
                    best_disparity = d;
                }
            }

            disparity_map.at<uchar>(y, x) = best_disparity;
        }
    }

    Ptr<StereoSGBM> stereo = StereoSGBM::create(min_disparity, max_disparity, penalty_scale, 8 * imgL.channels() * speckle_window_size * speckle_window_size, 32 * speckle_range, uniqueness_ratio, StereoSGBM::MODE_SGBM_3WAY);
    stereo->compute(imgL, imgR, disparity_map);

    namedWindow("Disparity Map", WINDOW_NORMAL);
    imshow("Disparity Map", disparity_map);

    waitKey(0);

    return 0;
}

需要注意的是，此代码只是SGM算法的实现之一，针对不同的场景和需求，实现方式可能会有所不同。