slam学习之——stereoVision.cpp实践

在学习高博的slam时，用的是slambook2-master这个新仓库，在ch5/stereo这个工程运行时，做了一些更改；

 

工程包括如下内容（其中build是自己创建的）

 根据安装依赖库的路径，对CMakeLists.txt做了些更改，更改后的内容如下：

find_package(Pangolin REQUIRED)

find_package(OpenCV 3.0 REQUIRED)      # add by csq，由于我装了好几个版本的，所以这里指定个高版本的
include_directories(${OpenCV_INCLUDE_DIRS})  #add by csq
#添加Eigen头文件
include_directories("/usr/include/eigen3")   #add by csq ，根据你的安装路径来选择
set(CMAKE_CXX_FLAGS "-std=c++11")

add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

stereoVision.cpp的代码分析如下：

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>  
//unistd.h为Linux/Unix系统中内置头文件，包含了许多系统服务的函数原型，例如read函数、write函数和getpid函数等。其作用相当于windows操作系统的"windows.h"，是操作系统为用户提供的统一API接口，方便调用系统提供的一些服务。

using namespace std;
using namespace Eigen;

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

// 在pangolin中画图，已写好，无需调整，定义绘制点云的函数，需要传入4维向量构成的点云集
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线 （就是两个相机光轴间的距离，单位是m）
    double b = 0.573;

    // 读取图像（以灰度图形式）
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(  //SGBM是立体匹配算法中的半全局块匹配，得到的视差图相比于BM算法来说，减少了很多不准确的匹配点，尤其是在深度不连续区域，速度上SGBM要慢于BM算法；
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // 神奇的参数
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm); //由左右视图按照SGBM匹配方式计算得到视差图
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //将16位符号整形的视差Mat转换为32位浮点型Mat

    //生成点云
    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;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前3维为xyz，第4维为归一化后的强度值

            // 根据双目模型计算point的位置，计算的是左视图点的相机位置
            double x = (u - cx) / fx;    //该公式计算的是归一化在相机Zc=1平面的相机坐标
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));  //由视差，双目的基计算像素点对应的实际距离（深度信息）
            point[0] = x * depth;  //由深度信息获取真实相机坐标系下的Xc
            point[1] = y * depth;  //由深度信息获取真实相机坐标系下的Yc
            point[2] = depth;      //相机坐标系下的Zc

            pointcloud.push_back(point);    //获得的是相机坐标系下的点云位置
        }

    cv::imshow("disparity", disparity / 96.0);   //把视差值限定在0-96
    cv::waitKey(0);
    // 画出点云
    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(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);   //点的颜色
            glVertex3d(p[0], p[1], p[2]);  //点的相机坐标
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

其中stereoSGBM的函数和参数说明如下：

static Ptr<StereoSGBM> cv::StereoSGBM::create   (int    minDisparity = 0,
int     numDisparities = 16,
int     blockSize = 3,
int     P1 = 0,
int     P2 = 0,
int     disp12MaxDiff = 0,
int     preFilterCap = 0,
int     uniquenessRatio = 0,
int     speckleWindowSize = 0,
int     speckleRange = 0,
int     mode = StereoSGBM::MODE_SGBM 
)       

• minDisparity：最小的视差值。
• numDisparity：视差范围，即最大视差值和最小视差值之差，必须是16的倍数。
• blockSize：匹配块大小（SADWindowSize），必须是大于等于1的奇数，一般为3~11 。
• P1，P2：惩罚系数，一般：P1=8*通道数*SADWindowSize*SADWindowSize，P2=4*P1
• disp12MaxDiff ：左右视差图的最大容许差异（超过将被清零），默认为 -1，即不执行左右视差检查。
• preFilterCap：预滤波图像像素的截断值。该算法首先计算每个像素的x导数，并通过[-preFilterCap，preFilterCap]间隔剪切其值。结果值被传递给Birchfield-Tomasi像素成本函数。
• uniquenessRatio：视差唯一性百分比， 视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0，通常为5~15.
• speckleRange：视差变化阈值。
• mode：模式

 

实践运行步骤如下，

#终端切到工程目录下
cd /home/chensq/Downloads/slambook2-master/ch5/stereo

#创建build并cd
mkdir build
cd build

#编译
cmake ..
make

在build中生成了个stereoVision的执行文件，将左右图像拷贝进来做测试 

 

原图如下：

运行

./stereoVision 

测试结果如下：

视差图结果

 点云图如下

参考：

半全局块匹配（Semi-Global Block Matching）算法