【opencv】示例-stereo_match.cpp 立体匹配：通过对左右视图图像进行处理来生成视差图和点云数据...

十年一梦实验室

于 2024-04-13 13:14:06 发布

阅读量554

点赞数 10

文章标签： opencv 人工智能计算机视觉

本文链接：https://blog.csdn.net/cxyhjl/article/details/137733129

版权

/*
 *  stereo_match.cpp
 *  calibration
 *
 *  创建者 Victor Eruhimov，日期为 2010年1月18日。
 *  版权所有 2010 Argus Corp.
 *
 */


#include "opencv2/calib3d/calib3d.hpp" // 导入OpenCV相机标定和三维重建相关的头文件
#include "opencv2/imgproc.hpp" // 导入OpenCV图像处理相关的头文件
#include "opencv2/imgcodecs.hpp" // 导入OpenCV图像编解码相关的头文件
#include "opencv2/highgui.hpp" // 导入OpenCV高层GUI(图形界面)相关的头文件
#include "opencv2/core/utility.hpp" // 导入OpenCV核心工具(utility)模块的头文件


#include <stdio.h> // 导入C标准输入输出头文件
#include <sstream> // 导入C++字符串流处理头文件


using namespace cv; // 使用OpenCV的命名空间


// 声明了print_help函数，用于显示帮助信息
static void print_help(char** argv)
{
    // 打印使用demo时的帮助信息
    printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n");
    // 打印程序使用方式的说明
    printf("\nUsage: %s <left_image> <right_image> [--algorithm=bm|sgbm|hh|hh4|sgbm3way] [--blocksize=<block_size>]\n"
           "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
           "[--no-display] [--color] [-o=<disparity_image>] [-p=<point_cloud_file>]\n", argv[0]);
}


// 声明了saveXYZ函数，用于将三维点云数据保存到文件
static void saveXYZ(const char* filename, const Mat& mat)
{
    // 设置三维点深度的最大值
    const double max_z = 1.0e4;
    // 以文本写入方式打开文件
    FILE* fp = fopen(filename, "wt");
    // 遍历图像的每个像素点
    for(int y = 0; y < mat.rows; y++)
    {
        for(int x = 0; x < mat.cols; x++)
        {
            // 读取每个像素点的三维坐标
            Vec3f point = mat.at<Vec3f>(y, x);
            // 如果该点的Z坐标无效，则忽略此点
            if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            // 将有效的三维坐标写入到文件中
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
        }
    }
    // 关闭文件
    fclose(fp);
}


int main(int argc, char** argv)
{
    // 定义了一系列的字符串变量用来存储命令行参数
    std::string img1_filename = "";
    std::string img2_filename = "";
    std::string intrinsic_filename = "";
    std::string extrinsic_filename = "";
    std::string disparity_filename = "";
    std::string point_cloud_filename = "";


    // 定义了枚举类型，列出了所有双目算法
    enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3, STEREO_3WAY=4, STEREO_HH4=5 };
    int alg = STEREO_SGBM; // 默认使用STEREO_SGBM算法
    int SADWindowSize, numberOfDisparities; // 定义了SAD窗口大小和视差的数量
    bool no_display; // 定义了是否显示结果的标志位
    bool color_display; // 定义了是否彩色显示视差图的标志位
    float scale; // 定义了缩放因子


    // 创建两种双目匹配算法实例对象：StereoBM和StereoSGBM
    Ptr<StereoBM> bm = StereoBM::create(16,9);
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);


    // 使用命令行解析器解析参数
    cv::CommandLineParser parser(argc, argv,
        "{@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{no-display||}{color||}{scale|1|}{i||}{e||}{o||}{p||}");
    // 如果用户请求帮助，则调用print_help函数并退出
    if(parser.has("help"))
    {
        print_help(argv);
        return 0;
    }
    // 获取命令行中指定的左右图像文件名
    img1_filename = samples::findFile(parser.get<std::string>(0));
    img2_filename = samples::findFile(parser.get<std::string>(1));


    // 解析算法参数
    if (parser.has("algorithm"))
    {
        std::string _alg = parser.get<std::string>("algorithm");
        alg = _alg == "bm" ? STEREO_BM :
            _alg == "sgbm" ? STEREO_SGBM :
            _alg == "hh" ? STEREO_HH :
            _alg == "var" ? STEREO_VAR :
            _alg == "hh4" ? STEREO_HH4 :
            _alg == "sgbm3way" ? STEREO_3WAY : -1;
    }
    // 解析其他所有需要的命令行参数
    numberOfDisparities = parser.get<int>("max-disparity");
    SADWindowSize = parser.get<int>("blocksize");
    scale = parser.get<float>("scale");
    no_display = parser.has("no-display");
    color_display = parser.has("color");
    if( parser.has("i") )
        intrinsic_filename = parser.get<std::string>("i");
    if( parser.has("e") )
        extrinsic_filename = parser.get<std::string>("e");
    if( parser.has("o") )
        disparity_filename = parser.get<std::string>("o");
    if( parser.has("p") )
        point_cloud_filename = parser.get<std::string>("p");


    // 检查解析后的参数是否正确
    if (!parser.check())
    {
        parser.printErrors();
        return 1;
    }
    // 检查算法参数是否指定正确
    if( alg < 0 )
    {
        printf("Command-line parameter error: Unknown stereo algorithm\n\n");
        print_help(argv);
        return -1;
    }
    // 检查视差数量设置是否正确
    if ( numberOfDisparities < 1 || numberOfDisparities % 16 != 0 )
    {
        printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
        print_help(argv);
        return -1;
    }
    // 检查缩放因子是否正确
    if (scale < 0)
    {
        printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
        return -1;
    }
    // 检查SAD窗口大小设置是否正确
    if (SADWindowSize < 1 || SADWindowSize % 2 != 1)
    {
        printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
        return -1;
    }
    // 检查是否指定了左右图像文件
    if( img1_filename.empty() || img2_filename.empty() )
    {
        printf("Command-line parameter error: both left and right images must be specified\n");
        return -1;
    }
    // 检查内参和外参是否同时指定
    if( (!intrinsic_filename.empty()) ^ (!extrinsic_filename.empty()) )
    {
        printf("Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified)\n");
        return -1;
    }
    // 检查生成点云时是否指定了内参和外参
    if( extrinsic_filename.empty() && !point_cloud_filename.empty() )
    {
        printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
        return -1;
    }


    // 根据算法的不同，设置图像的加载模式
    int color_mode = alg == STEREO_BM ? 0 : -1;
    // 加载左右视图图像
    Mat img1 = imread(img1_filename, color_mode);
    Mat img2 = imread(img2_filename, color_mode);


    // 如果左视图图像加载失败，打印错误并退出
    if (img1.empty())
    {
        printf("Command-line parameter error: could not load the first input image file\n");
        return -1;
    }
    // 如果右视图图像加载失败，打印错误并退出
    if (img2.empty())
    {
        printf("Command-line parameter error: could not load the second input image file\n");
        return -1;
    }
    // 如果缩放系数不等于1，则对图像进行缩放
    if (scale != 1.f)
    {
        // 定义两个临时矩阵用于存储缩放后的图像
        Mat temp1, temp2;
        // 根据scale的大小选择合适的插值方法
        int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
        // 对左右图像进行缩放
        resize(img1, temp1, Size(), scale, scale, method);
        img1 = temp1;
        resize(img2, temp2, Size(), scale, scale, method);
        img2 = temp2;
    }
    
    // 获取缩放后的图像大小
    Size img_size = img1.size();
    
    // 定义两个ROI区域以及Q矩阵（用于三维重建）
    Rect roi1, roi2;
    Mat Q;
    
    // 如果内参文件名不为空，则读取内（camera）参和外（stereo）参数
    if( !intrinsic_filename.empty() )
    {
        // 读取内参文件
        FileStorage fs(intrinsic_filename, FileStorage::READ);
        if(!fs.isOpened())
        {
            printf("Failed to open file %s\n", intrinsic_filename.c_str());
            return -1;
        }
    
        // 读取内参矩阵和畸变系数
        Mat M1, D1, M2, D2;
        fs["M1"] >> M1;
        fs["D1"] >> D1;
        fs["M2"] >> M2;
        fs["D2"] >> D2;
    
        // 根据缩放因子调整内参矩阵
        M1 *= scale;
        M2 *= scale;
    
        // 读取外参文件
        fs.open(extrinsic_filename, FileStorage::READ);
        if(!fs.isOpened())
        {
            printf("Failed to open file %s\n", extrinsic_filename.c_str());
            return -1;
        }
    
        // 读取旋转矩阵和平移矩阵
        Mat R, T, R1, P1, R2, P2;
        fs["R"] >> R;
        fs["T"] >> T;
    
        // 对立体图像进行校正
        stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );
    
        // 初始化校正映射矩阵
        Mat map11, map12, map21, map22;
        initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
        initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
    
        // 应用映射矩阵进行畸变校正和立体校正
        Mat img1r, img2r;
        remap(img1, img1r, map11, map12, INTER_LINEAR);
        remap(img2, img2r, map21, map22, INTER_LINEAR);
    
        // 更新校正后的图像
        img1 = img1r;
        img2 = img2r;
    }
    
    // 计算或更新视差的数量
    numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16;
    
    // 配置块匹配（Block Matching）算法的参数
    // 设置块匹配算法的区域兴趣（ROI），由roi1和roi2的矩形区域定义。
    bm->setROI1(roi1);
    bm->setROI2(roi2);
    // 设置前置滤波器的上限值用于块匹配算法。
    bm->setPreFilterCap(31);
    // 设置SAD窗口的大小，比较块大小。如果SADWindowSize变量大于0，就使用该变量的值，否则使用默认值9。
    bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9);
    // 设置最小视差，默认为0。
    bm->setMinDisparity(0);
    // 设置视差的数量，这里使用numberOfDisparities变量的值。
    bm->setNumDisparities(numberOfDisparities);
    // 设置纹理阈值，用于过滤掉纹理不够的区域。
    bm->setTextureThreshold(10);
    // 设置唯一性比例，用于判断最佳视差的唯一性。
    bm->setUniquenessRatio(15);
    // 设置视差连通区域变化的窗口大小，这里为100。
    bm->setSpeckleWindowSize(100);
    // 设置视差连通区域的最大视差变化范围，这里为32。
    bm->setSpeckleRange(32);
    // 设置左右视差图的最大差异，超过这个差异的视差值会被剔除。
    bm->setDisp12MaxDiff(1);
   
    
    // 配置半全局块匹配（Semi-Global Block Matching）算法的参数
    // 为半全局块匹配算法设置前置滤波器的上限值。
    sgbm->setPreFilterCap(63);
    // 计算SGBM算法的SAD窗口大小，如果SADWindowSize变量大于0，使用该变量的值，否则使用默认值3。
    int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
    // 设置SGBM算法的块大小。
    sgbm->setBlockSize(sgbmWinSize);
    
    // 根据输入图像的通道数计算常数值。
    int cn = img1.channels();
    
    // 设置SGBM算法的P1和P2参数，这些参数控制视差变化的平滑程度。
    sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
    // 设置最小视差，默认为0。
    sgbm->setMinDisparity(0);
    // 设置视差数量，这里使用numberOfDisparities变量的值。
    sgbm->setNumDisparities(numberOfDisparities);
    // 设置唯一性比例，用于判断最佳视差的唯一性。
    sgbm->setUniquenessRatio(10);
    // 设置视差连通区域变化的窗口大小，这里为100。
    sgbm->setSpeckleWindowSize(100);
    // 设置视差连通区域的最大视差变化范围，这里为32。
    sgbm->setSpeckleRange(32);
    // 设置左右视差图的最大差异，超过这个差异的视差值会被剔除。
    sgbm->setDisp12MaxDiff(1);
    // 根据选择的算法设置SGBM模式。
    if(alg==STEREO_HH)
        sgbm->setMode(StereoSGBM::MODE_HH);
    else if(alg==STEREO_SGBM)
        sgbm->setMode(StereoSGBM::MODE_SGBM);
    else if(alg==STEREO_HH4)
        sgbm->setMode(StereoSGBM::MODE_HH4);
    else if(alg==STEREO_3WAY)
        sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);
    
    Mat disp, disp8; // 定义存储视差图和转换后的8位视差图的矩阵。
    
    // 获取当前的时间刻度，用于计算视差计算的耗时。
    int64 t = getTickCount();
    float disparity_multiplier = 1.0f; // 定义视差倍数的默认值。
    
    // 根据选定的算法执行视差计算，并根据返回的数据类型设置合适的倍数。
    if( alg == STEREO_BM )
    {
        bm->compute(img1, img2, disp); // 使用BM算法计算视差。
        if (disp.type() == CV_16S)
            disparity_multiplier = 16.0f; // 如果视差图是16位有符号整型，则将倍数设为16。
    }
    else if( alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_HH4 || alg == STEREO_3WAY )
    {
        sgbm->compute(img1, img2, disp); // 使用SGBM算法族计算视差。
        if (disp.type() == CV_16S)
            disparity_multiplier = 16.0f; // 如果视差图是16位有符号整型，则将倍数设为16。
    }
    t = getTickCount() - t; // 计算视差计算耗时。
    printf("Time elapsed: %fms\n", t*1000/getTickFrequency()); // 打印耗时信息。
    
    // 将计算出的视差图转换为8位图像以供显示或存储。
    if( alg != STEREO_VAR )
        disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
    else
        disp.convertTo(disp8, CV_8U);
    
    Mat disp8_3c; // 定义用于显示彩色视差图的矩阵。
    // 如果选项设置为显示彩色图，则对8位图像应用色彩映射。
    if (color_display)
        cv::applyColorMap(disp8, disp8_3c, COLORMAP_TURBO);
    
    // 如果指定了视差图文件名，则将视差图存储到文件中。
    if(!disparity_filename.empty())
        imwrite(disparity_filename, color_display ? disp8_3c : disp8);
    
    // 如果指定了点云文件名，则保存点云数据到文件中。
    if(!point_cloud_filename.empty())
    {
        printf("storing the point cloud..."); // 打印存储点云数据的信息。
        fflush(stdout);
        Mat xyz; // 定义用于存储3D坐标的矩阵。
        Mat floatDisp; // 定义用于存储转换后的视差图的矩阵。
        // 将视差图转换为浮点型，并应用视差倍数。
        disp.convertTo(floatDisp, CV_32F, 1.0f / disparity_multiplier);
        // 使用Q矩阵将视差图重投影到3D坐标。
        reprojectImageTo3D(floatDisp, xyz, Q, true);
        // 使用saveXYZ函数将3D坐标保存到指定的点云文件中。
        saveXYZ(point_cloud_filename.c_str(), xyz);
        printf("\n"); // 用于换行。
    }
    
    // 如果未设置不显示结果，则创建显示左、右图像和视差图的窗口。
    if( !no_display )
    {
        std::ostringstream oss; // 使用字符串流构建显示的标题。
        oss << "disparity  " << (alg==STEREO_BM ? "bm" :
                                 alg==STEREO_SGBM ? "sgbm" :
                                 alg==STEREO_HH ? "hh" :
                                 alg==STEREO_VAR ? "var" :
                                 alg==STEREO_HH4 ? "hh4" :
                                 alg==STEREO_3WAY ? "sgbm3way" : "");
        oss << "  blocksize:" << (alg==STEREO_BM ? SADWindowSize : sgbmWinSize);
        oss << "  max-disparity:" << numberOfDisparities;
        std::string disp_name = oss.str(); // 从流中获取构建好的标题字符串。
    
        // 创建并显示图像窗口。
        namedWindow("left", cv::WINDOW_NORMAL);
        imshow("left", img1);
        namedWindow("right", cv::WINDOW_NORMAL);
        imshow("right", img2);
        namedWindow(disp_name, cv::WINDOW_AUTOSIZE);
        imshow(disp_name, color_display ? disp8_3c : disp8);
    
        printf("press ESC key or CTRL+C to close..."); // 提示用户如何关闭窗口。
        fflush(stdout);
        printf("\n"); // 用于换行。
        while(1) // 循环等待用户按键操作。
        {
            if(waitKey() == 27) // 如果用户按下ESC键，则退出循环。
                break;
        }
    }
    
    return 0; // 正常结束程序。
}

enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3, STEREO_3WAY=4, STEREO_HH4=5 };

什么是视差图

在计算视差图时，遮挡或缺乏纹理的区域会有什么表现？

SGBM算法是如何处理遮挡和纹理缺失问题的？

stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );

这个函数的主要目的是通过计算两个矫正映射，让来自两个摄像头的图像在水平方向上对准，简化立体匹配的复杂性，提高计算效率和质量。正确定位了坐标对齐后，图像中对应的点就可以直接在同一行上进行匹配，使得视差计算更加准确和便捷。

reprojectImageTo3D(floatDisp, xyz, Q, true);

这个函数中的投影变换矩阵Q是如何计算得到的？

The End