【死磕slam14】slambook2第五讲 详解stereoVision.cpp

#include <opencv2/opencv.hpp>
//<opencv2/opencv.hpp>头文件包含了各个模块的头文件，原则上无论创建哪个模块的应用程序，仅写一句#include <opencv2/opencv.hpp>即可，以达到精简代码的效果。
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include<unistd.h>
//include<unistd.h>是C++中提供对操作系统访问功能的头文件，如fork/pipe/各种I/O（read/write/close等等）。

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "../left.png";
string right_file = "../right.png";  

// 在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);//提前声明函数 showPointCloud //里面存储Vector4d   ，管理方式    传入引用的pointcloud

int main(int argc, char **argv) {

    // 相机内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157; 
    // 基线
    double b = 0.573;//双目的基线

    // 读取图像 
    cv::Mat left = cv::imread(left_file, 0);  //0 返回一张灰度图
    //cv::imread()函数用于读取图像。参数1为图像路径；参数2为设置为0时，表示返回灰度图像，默认值为1，代表返回彩色图像。
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // 神奇的参数 //关于sgbm算法的经典参数配置
        //最小视差值
        //最大视差减去最小视差值（该参数必须被16整除）
         //匹配块大小（3-11的奇数）
         //第一个控制视差平滑度的参数 P1
        //第二个控制视差平滑度的参数 P2，越大视差越平滑。P1是相邻像素间视差变化±1的惩罚值。P2是相邻像素之间视差变化大于1的惩罚值。算法要求P2 > P1。
        //------查看stereo match.cpp示例，其中显示了一些合理的P1和P2值(如8*图像通道数量*blockSize*blockSize和32*图像通道数量*blockSize*blockSize)
        //------此处通道数量为1
        //左右视差检查中允许的最大差异(整数像素单位)。将其设置为非正值以禁用检查。
        //预过滤图像像素的截断值。该算法首先计算每个像素处的x导数，并通过[-preFilterCap, preFilterCap] interval截取其值。结果值被传递给Birchfield-Tomasi像素代价函数。
        //计算出的最佳(最小)成本函数值应该“赢得”第二最佳值以认为找到的匹配是正确的百分比差额。通常，5-15范围内的值就足够了。
        //光滑视差区域的最大尺寸考虑他们的噪声斑点和无效。将其设置为0以禁用散斑滤波。否则，将其设置在50-200范围内。
        //每个连接组件内的最大视差变化。如果进行散斑滤波，将该参数设置为正值，它将隐式乘以16。通常情况下，1或2就足够了（32）
        //默认
        //将其设置为StereoSGBM::MODE HH，以运行全尺寸双通道动态规划算法。它将消耗O(W*H*numDisparities)字节，这对于640x480立体声和hd大小的图片来说是很大的。默认情况下，它被设置为false
    cv::Mat disparity_sgbm, disparity;//定义了disparity_sgbm  disparity两张图
    sgbm->compute(left, right, disparity_sgbm); 
    //将视差的计算结果放入disparity_sgbm矩阵中
    //disparity_sgbm产出视差地图。它与输入图像的大小相同。一些算法，如StereoBM或StereoSGBM计算16位定点视差图(其中每个视差值有4个小数位)，而其他算法输出32位浮点视差图。
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); 
    //将矩阵disparity_sgbm转换为括号中的格式(32位空间的单精度浮点型矩阵)
    //disparity输出最终视差图   数据类型   optional scale factor可选的比例因子 

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;    //声明一个4维的双精度浮点型可变长动态数组

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    //遍历像素
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
             //Mat.at<存储类型名称>(行，列)[通道]，用以遍历像素。省略通道部分时，可以看做二维数组简单遍历，例如M.at<uchar>(512-1,512*3-1)；
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色 第四维数值归一化。

            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx; //像素坐标转换为归一化坐标
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u)); //计算各像素点深度
            //计算带深度信息的各点坐标
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point); //将各点信息压入点云数组
        }

    cv::imshow("disparity", disparity / 96.0);//输出显示disparity，显示窗口命名为引号中的内容
    cv::waitKey(10000); //等待关闭显示窗口，括号内参数为零则表示等待输入一个按键才会关闭，为数值则表示等待X毫秒后关闭
    
    
    // 画出点云(函数实现在下面)
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {      //定义画出点云的函数,函数参数为四维动态数组的引用

    if (pointcloud.empty()) {//检测是否为空
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam); //激活要渲染到视图
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glPointSize(2);
        //glBegin()要和glEnd()组合使用。其参数表示创建图元的类型，GL_POINTS表示把每个顶点作为一个点进行处理
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]); //在OpenGl中设置颜色
            glVertex3d(p[0], p[1], p[2]);//设置顶点坐标
        }
        glEnd();
        pangolin::FinishFrame();    //结束
        usleep(5000);   // sleep 5 ms
    }
    return;
}

cmake

cmake_minimum_required(VERSION 2.8)
project(stereo)
find_package(Pangolin REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(${Opencv_INCLUDE_DIRS})
add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})