双目相机计算稠密深度点云详解教程

点击上方“计算机视觉工坊”，选择“星标”

干货第一时间送达

作者：本文为3D视觉开发者社区特约作者-熊猫飞天授权发布。

引言：本教程主要内容为介绍如何使用双目相机计算出稠密的3D点云。主要涉及到elas包的使用，通过KITTI数据集和ZED相机进行测试。在机器人导航中深度图是产生稠密3D环境地图的重要数据，在室内机器人一般使用基于主动红外测距原理的RGB-D相机（比如kinect）获取深度图像，RGB-D相机由于其测量范围和原理导致它主要适用于室内环境中。在室外环境中由于双目相机没有尺度问题，通常使用双目相机计算深度图。

一、双目立体匹配原理

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

图片来源于[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这个库。

二、elas_ros 包运行

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

cd ~/catkin_ws/srcgit clone https://github.com/jeffdelmerico/cyphy-elas-ros.gitcd ..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”，（下载链接：https://pan.baidu.com/s/1uXsNj77OydZoDxaK4PIjIw　提取码：e9ga）
数据集源地址：http://download.ros.org/data/stereo_image_proc/rotating_detergent_1_6.bag

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

• 启动 elas_ros

  roslaunch elas_ros dataset.launch
  

• 播放 rosbag

  rosbag play  rotating_detergent_1_6.bag
  

• 启动 rviz

  rosrun rviz rviz
  

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

elas双目点云建图

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.launchrosbag play kitti_01.bag

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

(ELAS在KITTI数据集上的运行效果)

elas

参考资料

官方教程：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.

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「计算机视觉工坊」公众号后台回复：深度学习，即可下载深度学习算法、3D深度学习、深度学习框架、目标检测、GAN等相关内容近30本pdf书籍。

下载2

在「计算机视觉工坊」公众号后台回复：计算机视觉，即可下载计算机视觉相关17本pdf书籍，包含计算机视觉算法、Python视觉实战、Opencv3.0学习等。

下载3

在「计算机视觉工坊」公众号后台回复：SLAM，即可下载独家SLAM相关视频课程，包含视觉SLAM、激光SLAM精品课程。

重磅！计算机视觉工坊-学习交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有ORB-SLAM系列源码学习、3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、深度估计、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~