ELAS_ROS算法在KITTI数据集上生成稠密点云

ELAS是一种基于概率模型的有效立体匹配算法,能够给予双目图像生成深度图,进而转化为点云.该算法的一种改进算法为LS-ELAS,其论文发表在2017年ICRA上,文章题目为"LS-ELAS: Line Segment based Efficient Large Scale Stereo Matching".本片博客主要介绍ELAS算法的一种开源代码ELAS_ROS安装,及其在KITTI数据集上的具体实现.

一.KITTI数据集下载与转换为rosbag

本文使用的KITTI数据集由kitti2bag转换为rosbag格式,以便于用于ELAS_ROS程序测试.由于该模块较为繁琐,固单独整理成为一篇博客.

步骤一:参照以下博客完成KITTI数据集到rosbag的转换
https://blog.csdn.net/qq_42138662/article/details/114529162

二.安装ELAS_ROS

步骤二:按照以下说明安装ELAS_ROS

zhangman@zhangman-G3-3579:~$ mkdir catkin_elas
zhangman@zhangman-G3-3579:~$ cd catkin_elas
zhangman@zhangman-G3-3579:~/catkin_elas$ mkdir src
zhangman@zhangman-G3-3579:~/catkin_elas$ cd src
zhangman@zhangman-G3-3579:~/catkin_elas/src$ git clone https://github.com/jeffdelmerico/cyphy-elas-ros.git
zhangman@zhangman-G3-3579:~/catkin_elas/src$ cd ..
zhangman@zhangman-G3-3579:~/catkin_elas$ catkin_make

三.配置ELAS_ROS以运行kitti

步骤三:在/catkin_elas/src/cyphy-elas-ros/elas_ros/文件夹下新建launch文件kitti.launch:

<launch>
    <!-- Launches ELAS node, and rectification nodes for input --> 
    <!-- Arguments: input stereo namespace and output elas namespace -->
    <arg name="stereo" default="kitti"/>
    <arg name="elas_ns" default="elas"/>
    
    <node name="$(arg elas_ns)" pkg="elas_ros" type="elas_ros" output="screen">
        <remap from="stereo" to="$(arg stereo)"/>
        <remap from="image" to="image_raw"/>

        <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>

步骤四:修改/catkin_elas/src/cyphy-elas-ros/elas_ros/src/文件夹下的elas.cpp源码以在kitti对应的rosbag上运行

将84行位置处的如下代码

    // Topics
    std::string stereo_ns = nh.resolveName("stereo");
    std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
    std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
    std::string left_info_topic = stereo_ns + "/left/camera_info";
    std::string right_info_topic = stereo_ns + "/right/camera_info";

更改为:

    // Topics
    std::string stereo_ns = nh.resolveName("stereo");
    std::string left_topic = ros::names::clean(stereo_ns + "/camera_gray_left/" + nh.resolveName("image"));
    std::string right_topic = ros::names::clean(stereo_ns + "/camera_gray_right/" + nh.resolveName("image"));
    std::string left_info_topic = stereo_ns + "/camera_gray_left/camera_info";
    std::string right_info_topic = stereo_ns + "/camera_gray_right/camera_info";

步骤五:重新编译

zhangman@zhangman-G3-3579:~/catkin_elas$ catkin_make

四.在KITTI上运行ElAZ

步骤六:运行elas和kitti的rosbag,并在rivz可视化

• 运行elas节点

roslaunch elas_ros kitti.launch

• 播放kitti对应的rosbag

rosbag play kitti.bag 

• 启动rviz

rviz

• 在rviz中添加相关topic后进行可视化
  

五.elas程序更改说明

如果想应用该程序到其他rosbag,请按照以下说明进行修改
0.修改思路
ELAS_ROS程序的运行共需要监听左相机灰度图像,右相机灰度图像,左相机的info,右相机的info,因此我们的更改主要围绕这四个topic名字来进行更改.

首先运行rosbag,然后查看rosbag对应的topic,如kitti数据集相应的rostopic如下:
为了便于叙述,下面的topic结构称为:
/rosbag名/相机名/图像话题名或相机info名

/kitti/camera_gray_left/image_raw
/kitti/camera_gray_right/image_raw
/kitti/camera_gray_left/camera_info
/kitti/camera_gray_right/camera_info

1.launch文件更改
launch文件按照以下模板进行更改:

<launch>
    <!-- Launches ELAS node, and rectification nodes for input --> 
    <!-- Arguments: input stereo namespace and output elas namespace -->
    <arg name="stereo" default="camera"/>    ##这里的camera更改为rosbag的名字,如kitti
    <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"/>     ##这里的image_rect更改为图像名字,如image_raw

        ##以下参数是程序相关参数,改动后会影响立体匹配效果
        <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>

2.elas.cpp源代码更改

    // Topics
    std::string stereo_ns = nh.resolveName("stereo");
    //这里的left改成rosbag中对应的左相机的名字,如camera_gray_left
    std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
    //这里的rigth改成rosbag中对应的右相机的名字,如camera_gray_right
    std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
    //这里的left改成rosbag中对应的左相机的名字,如camera_gray_left
    std::string left_info_topic = stereo_ns + "/left/camera_info";
    //这里的rigth改成rosbag中对应的右相机的名字,如camera_gray_right
    std::string right_info_topic = stereo_ns + "/right/camera_info";

3.更改后先编译再运行