本教程主要内容为介绍如何使用双目相机计算出稠密的3D点云。主要涉及到elas包的使用，通过KITTI数据及和ZED相机进行测试。

在机器人导航中深度图是产生稠密3D环境地图的重要数据，在室内机器人一般使用基于主动红外测距原理的RGB-D相机（比如kinect）获取深度图像，RGB-D相机由于其测量范围和原理导致它主要适用于室内环境中。在室外环境中由于双目相机没有尺度问题，通常使用双目相机计算深度图。

1、双目立体匹配原理

首先我们先介绍一些在双目立体匹配中的名称和双目图像立体匹配的流程。

图片来源于[1]

这里涉及几个定义：
基线：左右光心的连线
极点：像平面与基线的交点称为极点
极线：像点与极点之间的连线称为极线
极平面：左右极线与基线构成的平面为空间点对应的极平面

利用双目相机计算稠密的深度需要经过3个步骤：(图像矫正，视差计算)

1.1 图像矫正

步骤1：图像矫正，这个部分可以细分为图像去畸变矫正和图像立体匹配

图像去畸变是利用图像的畸变参数 [d1 d2 d3 p1 p2] 对图像畸变进行校正，检测方法就是，原本在图像边缘的直线由于相机的畸变导致在成像的时候变成了曲线，通过畸变校正以后，在图像中变成的直线，如下图所示：

这个也称作相机的内参数校准（焦距 fx fy 相机中心 cx cy），通常我们调用ROS里面的包就可以实现了。

步骤2：立体畸变校正

立体匹配是指把左右图像旋转到同一个平面（这里说同一个平面不太准确，应该是两个平行的平面），使得左右两幅图片的光轴平行，检测方法是判断左右目图像中的同一个像素点是否在同一水平线上面。

立体矫正是校准相机的外参数实现的，这个也可以通过ROS下的相机校准包实现，只需要输入棋盘格的size即可。校准的参数包括（基线 fx 相机的R矩阵 投影矩阵P矩阵）

 
  在获得相机的内参数和外参数以后我们可以调用opencv的函数对双目的图像进行矫正，如下所示：  

cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
        
cv::remap(img_left,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(img_right,imRight,M1r,M2r,cv::INTER_LINEAR);

    还有一种方式是通过ROS自带的包实现，以下是通过launch文件来启动两个节点分别处理左目和右目图像  

<launch>
    <!-- If you already have rectified camera images remove the image_proc nodes -->
    <group ns="stereo/left">
        <node name="left_rect" pkg="image_proc" type="image_proc"/>
    </group>
    
    <group ns="stereo/right">
        <node name="right_rect" pkg="image_proc" type="image_proc"/>
    </group>
 
</launch>

1.2 视差计算

视差定义为 特征点p在左目图像中的像素坐标减去右目图像的像素坐标： disparity=xl-xr ,这里只计算横坐标的差值，因为输入的图像认为是已经立体矫正过的图像了，其左右目同一个像素在一条直线上。深度与视差转换关系： depth = (fx * baseline) / disparity，其中 baseline表示基线的长度。

双目立体匹配实质是指，如果利用双目相机在同一个时刻采集到的左、右目图像，生成一个3D的点云图。

双目计算深度点云的ROS包有两个： stereo_image_proc 和 LIBELAS（: Library for Efficient Large-scale Stereo Matching）两个库，其中LIBELAS提供了一个ROS环境下的包 elas_ros 可以在ROS下面直接使用。

ELAS这个库计算的深度相比于 stereo_image_proc 更加的稠密，推荐使用ELAS这个库。

2、elas_ros 包运行

ELAS [2] 是一种基于概率模型的有效立体匹配算法，该算法具有两个假设：（I）某些立体对应关系可以得到可靠匹配，（II）像素i的视差独立于所有其他像素。 因此，ELAS首先使用梯度图像上的SAD匹配来计算稀疏的支持点集，然后使用Delaunay三角剖分来构造用于密集匹配的2D网格。 最后，使用最大后验（MAP）模型来估计视差。如果对文章感兴趣的话，可以阅读以下大佬在FPGA上对ELAS算法实现了加速 [3],并开源了代码 [4].
1、首先在 elas_ros 的github地址上下载代码包，然后进行编译

cd ~/catkin_ws/src
git clone https://github.com/jeffdelmerico/cyphy-elas-ros.git
cd ..
catkin_make

  ２、参考 elas_ros 中launch文件夹下面　elas.launch　文件，新建一个名为　dataset.launch　的文件夹，并把launch文件第四行的　参数 “stereo” 修改为 “narrow_stereo_textured”　修改后的 dataset.launch 文件内容如下:  

<launch>
    <!-- Launches ELAS node, and rectification nodes for input --> 
    <!-- Arguments: input stereo namespace and output elas namespace -->
    <arg name="stereo" default="narrow_stereo_textured"/>
    <arg name="elas_ns" default="elas"/>
    
    <!-- If you already have rectified camera images remove the image_proc nodes -->
    <group ns="$(arg stereo)/left">
        <node name="left_rect" pkg="image_proc" type="image_proc"/>
    </group>
    
    <group ns="$(arg stereo)/right">
        <node name="right_rect" pkg="image_proc" type="image_proc"/>
    </group>

    <!-- This node actually does the stereo reconstruction -->
    <node name="$(arg elas_ns)" pkg="elas_ros" type="elas_ros" output="screen">
        <remap from="stereo" to="$(arg stereo)"/>
        <remap from="image" to="image_rect"/>

        <param name="approximate_sync" value="true" />
        <param name="disp_min" type="int" value="0"/>
        <param name="disp_max" type="int" value="255"/>
        <param name="support_threshold" type="double" value="0.95"/>
        <param name="support_texture" type="int" value="10"/>
        <param name="candidate_stepsize" type="int" value="5"/>
        <param name="incon_window_size" type="int" value="5"/>
        <param name="incon_threshold" type="int" value="5"/>
        <param name="incon_min_support" type="int" value="5"/>
        <param name="add_corners" type="bool" value="0"/>
        <param name="grid_size" type="int" value="20"/>
        <param name="beta" type="double" value="0.02"/>
        <param name="gamma" type="double" value="3"/>
        <param name="sigma" type="double" value="1"/>
        <param name="sradius" type="double" value="2"/>
        <param name="match_texture" type="int" value="1"/>
        <param name="lr_threshold" type="int" value="2"/>
        <param name="speckle_sim_threshold" type="double" value="1"/>
        <param name="speckle_size" type="int" value="200"/>
        <param name="ipol_gap_width" type="int" value="300"/>
        <param name="filter_median" type="bool" value="0"/>
        <param name="filter_adaptive_mean" type="bool" value="1"/>
        <param name="postprocess_only_left" type="bool" value="1"/>
        <param name="subsampling" type="bool" value="0"/>

        <!-- If your cameras are not synchronised then uncomment the following line -->
        <!-- <param name="approximate_sync" value="true" type="bool"/> -->
    </node>
</launch>

３、下载我们所用的测试数据集　“rotating_detergent_1_6.bag”，（百度网盘链接：　提取码：e9ga）数据集源地址

４、新建三个终端，然后启动节点

• 启动 elas_ros

roslaunch elas_ros dataset.launch

• 播放 rosbag

rosbag play  rotating_detergent_1_6.bag

• 启动 rviz

rosrun rviz rviz

  在RVIZ中添加对应的话题，即可看到视差图和点云图：  
   

3、KITTI数据集运行

3.1 kitti数据集转换为rosbag

在运行双目深度话的过程中我们首先需要把KITTI数据集转换为rosbag，以便使用。kitti数据集转换为rosbag 可以参考博客: KITTI数据集测试 – 1 从KITTI数据到rosbag

python img2bag_kitti_odo.py /your directory/KITTI/dataset/sequences/00/image_0 kitti_00.bag /your directory/KITTI/dataset/sequences/00/times.txt

除此之外还有两个包也可以实现这个功能，第一个是 ethz-asl 的 kitti_to_rosbag包，但是 kitti_to_rosbag 需要依赖很多的包，这我没有使用这个包，感兴趣的小伙伴可以研究一下。还有一个是 kitti2bag 。

这里需要注意添加上双目之间的基线信息（通常是在P矩阵中加入bf=基线*fx）。其次我所使用的kITTI的数据及是对齐以后的数据集，也就是KITTI官网上Odometry那个网页上的，其中的图像是经过立体矫正以后的图像，因此我们在使用kitti数据集时，不需要包含图像立体矫正的包

3.2 运行KITTI数据集

创建一个名为 kitti.launch 的 launch 文件写入以下内容

<launch>
    <!-- Launches ELAS node, and rectification nodes for input --> 
    <!-- Arguments: input stereo namespace and output elas namespace -->
    <arg name="stereo" default="camera"/>
    <arg name="elas_ns" default="elas"/>
    
    <!-- If you already have rectified camera images remove the image_proc nodes -->
    <group ns="$(arg stereo)/left">
        <node name="left_rect" pkg="image_proc" type="image_proc"/>
    </group>
    
    <group ns="$(arg stereo)/right">
        <node name="right_rect" pkg="image_proc" type="image_proc"/>
    </group>

    <!-- This node actually does the stereo reconstruction -->
    <node name="$(arg elas_ns)" pkg="elas_ros" type="elas_ros" output="screen">
        <remap from="stereo" to="$(arg stereo)"/>
        <remap from="image" to="image_rect"/>

        <param name="disp_min" type="int" value="0"/>
        <param name="disp_max" type="int" value="255"/>
        <param name="support_threshold" type="double" value="0.95"/>
        <param name="support_texture" type="int" value="10"/>
        <param name="candidate_stepsize" type="int" value="5"/>
        <param name="incon_window_size" type="int" value="5"/>
        <param name="incon_threshold" type="int" value="5"/>
        <param name="incon_min_support" type="int" value="5"/>
        <param name="add_corners" type="bool" value="0"/>
        <param name="grid_size" type="int" value="20"/>
        <param name="beta" type="double" value="0.02"/>
        <param name="gamma" type="double" value="3"/>
        <param name="sigma" type="double" value="1"/>
        <param name="sradius" type="double" value="2"/>
        <param name="match_texture" type="int" value="1"/>
        <param name="lr_threshold" type="int" value="2"/>
        <param name="speckle_sim_threshold" type="double" value="1"/>
        <param name="speckle_size" type="int" value="200"/>
        <param name="ipol_gap_width" type="int" value="300"/>
        <param name="filter_median" type="bool" value="0"/>
        <param name="filter_adaptive_mean" type="bool" value="1"/>
        <param name="postprocess_only_left" type="bool" value="1"/>
        <param name="subsampling" type="bool" value="0"/>

        <param name="approximate_sync" value="true" />
        <param name="queue_size" type="int" value="5"/>
        <!-- If your cameras are not synchronised then uncomment the following line -->
        <!-- <param name="approximate_sync" value="true" type="bool"/> -->
    </node>
 
    <!--node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find elas_ros)/rviz/KITTI.rviz" / -->
</launch>

注：在上述launch文件中的第7行-14行，这里我偷懒为不去修改topic 我依旧保留了这个图像立体矫正的,实际中输入已经矫正过的图像情况下不需要再使用这段代码了。

接下来播放kitti对应的rosbag包并启动elas_ros既可以：

roslaunch elas_ros kitti.launch
rosbag play kitti_01.bag

  这样就可以看到使用elas_ros包实时构建的双目点云了，这个包大约可以以10HZ左右的频率运行，因此在用于建图时候最好还是使用关键帧。  
 

参考资料

官方教程：https://github.com/MichaelGrupp/evo
[1] https://blog.csdn.net/luoru/article/details/49048287
[2] Geiger A, Roser M, Urtasun R. Efficient large-scale stereo matching[C]//Asian conference on computer vision. Springer, Berlin, Heidelberg, 2010: 25-38.
[3] Rahnama O, Frost D, Miksik O, et al. Real-time dense stereo matching with ELAS on FPGA-accelerated embedded devices[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 2008-2015.
[4] https://github.com/torrvision/ELAS_SoC
[5] Jellal R A, Lange M, Wassermann B, et al. LS-ELAS: Line segment based efficient large scale stereo matching[C]//2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017: 146-152.

如果大家觉得文章对你有所帮助，麻烦大家帮忙点个赞。O(∩_∩)O

欢迎大家在评论区交流讨论（cenruping@vip.qq.com）

需要代码的同学，可在评论区留下邮箱，可将代码和数据测试包发送到邮箱。