点云PCL库学习-双目图像转化为点云PCD并显示

文章目录

• 双目视觉模型
• 代码实现
• 总结
• 参考文章

---

一、双目视觉模型

和RGBD相机主动发射光线实现测距不同，双目测距原理通过配置立体摄像头（通常由左眼相机和右眼相机两个水平放置的相机组成）来获取一对同一场景中的二维图像，再基于同一空间点在两个坐标系下的视差来计算深度。双目测距的示意图和数学表达式如下所示：

双目相机的成像示意图

上述示意图描述了一个空间点在左右两目相机中位置之间的成像关系，根据相似三角形原理可得：

其中，f和b分别为相机焦距和双目相机基线，d为空间点在两个坐标系下的视差。由于视差的最小值为1个像素，因此双目相机的测距范围有一个理论上的最大值，由fb决定。在生成深度图像时，算法会对两张图像进行逐点匹配并计算视差，从而得到匹配像素点的深度值（一般会以左目相机为基准），因此双目相机的计算量非常大，通常需要采用GPU或FPGA才能实现实时计算。

---

二、代码实现

本文的代码实现环境为Ubuntu 18.04 + OpenCV 3.4.13 + PCL 1.12.1 + CMake 3.10.2，双目图像可从《视觉SLAM14讲》的第五讲源码中获取。本章代码中的核心部分和书中的源码基本一致，但在点云可视化时采用PCL库来实现（书本源码中采用了Pangolin库）。

#include 
#include 
#include  
#include 

using namespace std;
    
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("../data/left.png");
    cv::Mat right = cv::imread("../data/right.png");
    cv::Ptr sgbm = cv::StereoSGBM::create(
        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);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 定义点云使用的格式：这里用的是XYZRGB
    pcl::PointCloud::Ptr road_cloud(new pcl::PointCloud);
    
    // 根据视差和相机模型计算每一个点的三维坐标, 并添加到PCL点云中
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at(v, u) <= 0.0 || disparity.at(v, u) >= 96.0) 
                continue;

            double depth = fx * b / (disparity.at(v, u));
            pcl::PointXYZRGB p;
            p.x = depth * (u - cx) / fx;
            p.y = depth * (v - cy) / fy;
            p.z = depth;
            p.b = left.at(v, u)[0];
            p.g = left.at(v, u)[1];
            p.r = left.at(v, u)[2];
            
            road_cloud->points.push_back(p);
        }
    
    //可视化视差图
    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    
    //可视化三维点云
    road_cloud->height = 1;
    road_cloud->width = road_cloud->points.size();
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
	viewer.showCloud(road_cloud);
	while(!viewer.wasStopped()){}
    
    return 0;
}

stereoVision.cpp

# cmake版本及工程名
cmake_minimum_required( VERSION 2.8 )
project( stereo2pcd )

# 设置opencv库
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# 设置pcl库 
find_package( PCL REQUIRED)
include_directories( ${PCL_INCLUDE_DIRS} )

# 编译与链接
add_executable( stereoVision stereoVision.cpp )
target_link_libraries( stereoVision ${OpenCV_LIBS} ${PCL_LIBRARIES} )

CMakeLists.txt

算法步骤：

1.读取左右两目图像与相机内参

2.基于OpenCV库对左右两目图像进行匹配，并生成视差矩阵

3.基于视差矩阵计算每一个像素点的深度，再通过相机模型计算每一个像素点对应的三维坐标

4.将每一个像素所对应的三维坐标添加到PCL点云中

5.通过OpenCV和PCL库中的可视化模块分别显示视差图和三维点云

左目图像 

右目图像

 视差图

双目点云

注：本代码以左视图为基准生成视差图和三维点云，因此视差图中的左侧空洞部分为右目相机的盲区。而在双目点云可视化的过程中，需要调整窗口的观察角度才能呈现以上效果。

---

三、总结

本文主要基于一组双目图像生成了PCL点云，和上一章的内容相衔接。而RGBD图像和双目图像基本能覆盖计算机视觉领域的绝大部分需求，除此之外也有事件相机和单目重建等特殊情况，但在目前的工业界中应用较少，因此不多加阐述。事实上，无论是哪一种传感器，最终生成的三维点云都将作为机器人定位和建图模块的输入，如SLAM模块的帧间匹配和三维重建模块的点云融合，因此这两种传感器的成像原理与点云生成过程需要重点掌握。

---

四、参考文章

[1]:视觉SLAM十四讲5章

[2]:https://github.com/gaoxiang12/slambook2