双目相机计算稠密深度点云（一）

熊猫飞天

发布时间 2020.12.01阅读数 9961 评论数 6

双目相机计算稠密深度点云

- 1、双目立体匹配原理
- - 1.1 图像矫正
  - 1.2 视差计算
- 2、elas_ros 包运行
- 3、KITTI数据集运行
- 3.1 kitti数据集转换为rosbag
- 3.2 运行KITTI数据集
参考资料

本教程主要内容为介绍如何使用双目相机计算出稠密的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）

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

ROS rviz 图像处理双目相机

原创文章作者：熊猫飞天。如若转载，请注明出处：古月居 https://www.guyuehome.com/18908

打赏 1

上一篇：SLAM 轨迹评估工具EVO

下一篇：ROS节点开机自启动

双目相机计算稠密深度点云（一）

熊猫飞天

双目相机计算稠密深度点云

1、双目立体匹配原理

1.1 图像矫正

1.2 视差计算

2、elas_ros 包运行

3、KITTI数据集运行

3.1 kitti数据集转换为rosbag

3.2 运行KITTI数据集

参考资料

为你推荐

【OpenCV】 Octave | 角点检测 | SIFT/SURF算法

图像分类保姆级教程-深度学习入门教程（附全部代码）

角蜂鸟AI视觉套件：（三）ROS案例

使用百度API和opencv在x3派中实现人脸识别

在ROS系统下使用cv_bridge实现“人脸识别”——基于课程《ROS机器视觉开发入门》

机器人视角下的局部路径规划与避障碍

评论（6）

关于作者

熊猫飞天

24

0

1

2

Webots 机器人仿真平台(十) 添加camera相机

ROS 与 MATLAB 通讯

ROS节点开机自启动

相关推荐

【机器视觉学习笔记】最近邻插值实现图片任意角度旋转（C++）

open cv学习 （四）图像的几何变换

三维空间刚体运动

点云/网格模型的体积计算

三维重建_体素重建_空间雕刻法/体素着色法

OpenCV学习笔记(十七)——K均值聚类

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

Webots 机器人仿真平台

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

open cv学习（四）图像的几何变换

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板