RTAB-Map with SFU Mountain Dataset

[hugormluis]

Hi, Mathieu,

I am trying to use RTAB-Map on SFU Mountain Dataset with the stereo configuration, I am using the stereo images for both the odometry and loop closure (however, for this dataset the loop closure is not that important because the robot does not revisit previous areas), so the stereo images are my only source of sensorial information. The stereo images are provided by two different cameras that were deployed in a stereo configuration. I have used the demo_stereo_outdoor.launch file as template and I changed some parameters to make it compatible with the SFU Mountain Dataset.

However, I am getting a bad performance. The first thing I see is that the 3D map is too "empty", it is composed of few points when compared with the map that is built for the stereo_outdoorA.bag that you provided with the stereo outdoor mapping tutorial. I have messed around with some parameters in the launch file but it lead me to nowhere (probably because I cannot fully understand the majority of them and which ones are related with the mapping part). And the second thing is when I compare the ground truth with the trajectory generated by the RTAB-Map, they are completely different.

Here is the ground truth vs the RTAB-Map trajectory:

And here is a video showing the RVIZ during runtime.

I am also posting my launch file:

<launch>
  <!-- Choose visualization -->
  <arg name="rtabmapviz"   default="false" /> 
  <arg name="rviz"         default="true" />
  <arg name="local_bundle" default="true" />
  <arg name="stereo_sync"  default="true" />
  <arg name="approx_sync"  default="true"/>

  <param name="use_sim_time" type="bool" value="True"/>
  <!-- Just to uncompress images for stereo_image_rect -->
  <node name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
  <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />

  <!-- Run the ROS package stereo_image_proc for image rectification -->
  <group ns="/stereo_camera" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
        <remap from="left/image_raw"    to="left/image_raw_throttle_relay"/>
        <remap from="left/camera_info"  to="left/camera_info_throttle"/>
        <remap from="right/image_raw"   to="right/image_raw_throttle_relay"/>
        <remap from="right/camera_info" to="right/camera_info_throttle"/>
        <param name="disparity_range" value="128"/>
    </node>
    
    <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
      <remap from="left/image_rect"   to="left/image_rect_color"/>
      <remap from="right/image_rect"   to="right/image_rect_color"/>
      <remap from="left/camera_info"   to="left/camera_info_throttle"/>
      <remap from="right/camera_info"   to="right/camera_info_throttle"/>
    </node>
  </group>
                
  
  <group ns="rtabmap">   
  
    <!-- Stereo Odometry -->   
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="/stereo_camera/left/image_rect_color"/>
        <remap from="right/image_rect"      to="/stereo_camera/right/image_rect_color"/>
        <remap from="left/camera_info"      to="/stereo_camera/left/camera_info_throttle"/>
        <remap from="right/camera_info"     to="/stereo_camera/right/camera_info_throttle"/>
        <remap from="rgbd_image"            to="/stereo_camera/rgbd_image"/>
        <remap from="odom"                  to="/stereo_odometry"/>

        <param name="subscribe_rgbd"  type="bool" value="$(arg stereo_sync)"/>
        <param name="frame_id"        type="string" value="base_footprint"/>
        <param name="odom_frame_id"   type="string" value="rtabmap_odom"/>

        <param name="Odom/Strategy"         type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame  DEFAULT 0-->
        <param name="Vis/EstimationType"    type="string" value="1"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
        <param name="Vis/MaxDepth"          type="string" value="0"/>
        <param name="Odom/GuessMotion"      type="string" value="true"/>
        <param name="Vis/MinInliers"        type="string" value="10"/> <!-- DEFAULT 10-->
        <param unless="$(arg local_bundle)" name="OdomF2M/BundleAdjustment" type="string" value="0"/>
        <param name="OdomF2M/MaxSize"       type="string" value="2000"/> 
        <param name="GFTT/MinDistance"      type="string" value="0.00001"/> <!-- DEFAULT 10-->
        <param name="GFTT/QualityLevel"     type="string" value="0.00001"/> <!-- DEFAULT 0.00001-->
        <param name="Stereo/OpticalFlow"    type="bool"   value="true"/>
        <param name="Vis/FeatureType"       type='string' value="8"/>
        <param name="Stereo/MaxDisparity"   type='string' value="512"/>
    </node>
  
    <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
        <param name="frame_id"        type="string" value="base_footprint"/>
        
        <param unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
        <param name="subscribe_depth"  type="bool" value="false"/>
        <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>

        <remap from="left/image_rect"   to="/stereo_camera/left/image_rect_color"/>
        <remap from="right/image_rect"  to="/stereo_camera/right/image_rect_color"/>
        <remap from="left/camera_info"  to="/stereo_camera/left/camera_info_throttle"/>
        <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
        <remap from="rgbd_image"        to="/stereo_camera/rgbd_image"/>

        <remap from="odom" to="/stereo_odometry"/>

        <param name="queue_size" type="int" value="30"/>
        <param name="map_negative_poses_ignored" type="bool" value="true"/>

        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/TimeThr"                   type="string" value="700"/>
        <param name="Grid/DepthDecimation"              type="string" value="4"/>
        <param name="Grid/FlatObstacleDetected"         type="string" value="true"/>
        <param name="Kp/MaxDepth"                       type="string" value="0"/>
        <param name="Kp/DetectorStrategy"               type="string" value="6"/>  
        <param name="Vis/EstimationType"                type="string" value="1"/>   <!-- 0=3D->3D, 1=3D->2D (PnP) -->
        <param name="Vis/MaxDepth"                      type="string" value="0"/>
        <param name="RGBD/CreateOccupancyGrid"          type="string" value="true"/>       
    </node>
    
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
        <param unless="$(arg stereo_sync)" name="subscribe_stereo"    type="bool" value="true"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>
        <param name="queue_size"          type="int" value="10"/>
        <param name="frame_id"        type="string" value="base_footprint"/>
        <remap from="left/image_rect"  to="/stereo_camera/left/image_rect_color"/>
        <remap from="right/image_rect"  to="/stereo_camera/right/image_rect"/>
        <remap from="left/camera_info"  to="/stereo_camera/left/camera_info_throttle"/>
        <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
        <remap from="rgbd_image"          to="/stereo_camera/rgbd_image"/>
        <remap from="odom_info"         to="odom_info"/>
        <remap from="odom"              to="/stereo_odometry"/>
        <remap from="mapData"           to="mapData"/>
    </node>
        
  </group>

  <!-- Visualisation RVIZ --> 
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>

</launch>

Do you have any idea on where I can improve the results?

[matlabbe]

Hi,

Not sure how your launch file worked. I created a new one with minimal stuff to do stereo SLAM:

<launch>
  <!-- Choose visualization -->
  <arg name="rtabmapviz"   default="false" /> 
  <arg name="rviz"         default="true" />

  <param name="use_sim_time" type="bool" value="True"/>
  <!-- Just to uncompress images for stereo_image_rect -->
  <node name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=/camera/stereo/left/image_color raw out:=/camera/stereo/left/image_color" />
  <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/camera/stereo/right/image_color raw out:=/camera/stereo/right/image_color" />

  <!-- Run the ROS package stereo_image_proc for image rectification -->
  <group ns="/camera/stereo" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
        <remap from="left/image_raw"    to="left/image_color"/>
        <remap from="left/camera_info"  to="left/camera_info"/>
        <remap from="right/image_raw"   to="right/image_color"/>
        <remap from="right/camera_info" to="right/camera_info"/>
        <param name="disparity_range" value="128"/>
    </node>
    
    <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
      <remap from="left/image_rect"   to="left/image_rect_color"/>
      <remap from="right/image_rect"   to="right/image_rect_color"/>
      <remap from="left/camera_info"   to="left/camera_info"/>
      <remap from="right/camera_info"   to="right/camera_info"/>
    </node>
  </group>
                
  
  <group ns="rtabmap">   
  
    <!-- Stereo Odometry -->   
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="rgbd_image"            to="/camera/stereo/rgbd_image"/>

        <param name="subscribe_rgbd"  type="bool" value="true"/>
        <param name="frame_id"        type="string" value="base_footprint"/>
        <param name="odom_frame_id"   type="string" value="stereo_odom"/>
        <param name="guess_frame_id"   type="string" value="odom"/>
    </node>
  
    <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
        <param name="frame_id"        type="string" value="base_footprint"/>
        <param name="subscribe_rgbd"      type="bool" value="true"/>

        <remap from="rgbd_image"        to="/camera/stereo/rgbd_image"/> 
    </node>
    
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_rgbd"      type="bool" value="true"/>
        <param name="queue_size"          type="int" value="10"/>
        <param name="frame_id"        type="string" value="base_footprint"/>
        <remap from="rgbd_image"          to="/camera/stereo/rgbd_image"/>
    </node>
        
  </group>

  <!-- Visualisation RVIZ --> 
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>

</launch>

Then with this bag:

rosbag play --clock sfumd-dry-b.bag

ISSUES
1) An optical rotation is missing for the cameras, so rtabmap thinks they are looking up.

This issue could be fixed by adding a new static transform publisher with the optical rotation after the stereo frames in Tf tree (similar to optical transform from here).

  <arg name="pi/2" value="1.5707963267948966" />
  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) camera_stereo_left camera_stereo_left_optical 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) camera_stereo_right camera_stereo_right_optical 100" /> 

Then republish the left and right images with header/frame_id set to camera_stereo_left_optical and camera_stereo_right_optical respectively.

2) Stereo calibration looks wrong, so rtabmap will perform poorly and 3D dense disparity images will be poorly estimated. Here an example of left and right images superposed:

As you can see they have a disparity error in vertical. Normally correctly rectified stereo would have no disparity error along vertical axis, only horizontal. Example of good stereo rectification can be found here (notice how there is no disparity error along the vertical):


Conclusion, while issue 1 can be fixed, issue 2 is more difficult if we cannot recalibrate the cameras, otherwise the stereo data is useless with rtabmap.