Close Loop cause Map to Disappear

[chainer]

Ok, im using ROS Hydro. How ever I am having difficulties in getting it to work under a close loop. Whenever it detected a loop it just delete the cloud map.

here is my stereo outdoor demo modified launch file.

launch>

  <!-- Rotate the camera frame. -->
  <arg name="pi/2" value="1.5707963267948966" />
  <arg name="camdist" value ="0.1397" />
  <arg name="optical_rotate" value="0.3 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link stereo_camera 100" /> 


<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>
        <remap from="odom" to="/stereo_camera/odom"/> <!-- addd -->
        <param name="frame_id" type="string" value="base_link"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="approx_sync" type="bool" value="false"/>
        <param name="queue_size" type="int" value="5"/>

        <param name="Odom/InlierDistance" type="string" value="0.1"/>
        <param name="Odom/MinInliers" type="string" value="10"/>
        <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
        <param name="Odom/MaxDepth" type="string" value="10"/>
        <param name="OdomBow/NNDR" type="string" value="0.8"/>   <!--addd -->
        <param name="Odom/FillInfoData" type="string" value="true"/> <!-- - addd -->
        <param name="GFTT/MaxCorners" type="string" value="500"/>
        <param name="GFTT/MinDistance" type="string" value="5"/>
    </node>     
</group>

<group ns="rtabmap">   
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     <param name="frame_id" type="string" value="base_link"/>
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_depth" type="bool" value="true"/>
      <param name="wait_for_transform" type="bool" value="true"/> <!--added-->
     <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"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

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

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

     <!-- RTAB-Map's parameters -->
     <param name="Rtabmap/TimeThr" type="string" value="700"/>
     <param name="Rtabmap/DetectionRate" type="string" value="0"/>

     <param name="Kp/WordsPerImage" type="string" value="200"/>
     <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
     <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
     <param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->

     <param name="SURF/HessianThreshold" type="string" value="1000"/>

     <param name="LccBow/MaxDepth" type="string" value="5"/> <!--5 -->
     <param name="LccBow/MinInliers" type="string" value="10"/>
     <param name="LccBow/InlierDistance" type="string" value="0.02"/> <!--0.02 -->

     <param name="LccReextract/Activated" type="string" value="true"/>
     <param name="LccReextract/MaxWords" type="string" value="500"/>
         
         <!-- Disable graph optimization because we use map_optimizer node below -->
         <param name="RGBD/ToroIterations" type="string" value="0"/>
  
<param name="RGBD/OptimizeStrategy" type="string" value="2"/> <!-- g2o=1, GTSAM=2 -->
   <param name="RGBD/OptimizeRobust" type="string" value="true"/>

 <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>   <!-- Local loop closure detection with locations in STM -->


  </node>

<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/> 
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">  
         <param name="occupancy_grid" type="bool" value="false"/>
         <remap from="mapData" to="mapData_optimized"/>
         <remap from="grid_projection_map" to="/map"/>
      </node>



  <!-- Visualisation RTAB-Map -->
  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_odom_info" type="bool" value="true"/>
     <param name="queue_size" type="int" value="10"/>
     <param name="frame_id" type="string" value="base_link"/>
     <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"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
     <remap from="odom_info" to="/stereo_camera/odom_info"/>
     <remap from="odom" to="/stereo_camera/odom"/>
     <remap from="mapData" to="mapData_optimized"/><!-- addd -->
     
  </node>

 </group>

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


 </launch>

[chainer]

Also why is the rviz version of the map is very poorly shown compared to rtabmap where the points are more refined.?


Close loop matched, but map gone

[matlabbe]

Hi chainer,

The point cloud rendering parameters in RVIZ can be changed under MapCloud display's parameters in RVIZ. For rtabmapviz, the rendering parameters are in Preferences->3D Rendering.

Thx for the report! There was effectively a bug in map_optimizer making it clearing the whole map. This is now fixed.

However, I recommend to use graph optimization inside rtabmap node ("RGBD/OptimizeIterations=100" instead of "RGBD/ToroIterations=0"). "RGBD/LocalLoopDetectionSpace=true" requires also that graph optimization is done in rtabmap. The parameters "RGBD/OptimizeStrategy" and "RGBD/OptimizeRobust" are also ignored when optimization is disabled. I've updated the stereo outdoor mapping tutorial to not use map_optimizer node. If you want to use GTSAM, I would reduce the number of optimization iterations (RGBD/OptimizeIterations=10) as it is slower than TORO but converges faster.

Also, you set "Rtabmap/DetectionRate=0" (which means there are map updates as fast as possible), this will make rtabmap to add a lot of nodes (or key frames) in the graph, increasing significantly loop closure detection time, graph optimization time and cloud reconstruction time. The default is 1 Hz as most general usages don't need to update the map faster. You may increase up to 2 Hz, but I don't recommend to increase it more.

In summary, here are some modifications I would do to your launch file:

<node name="rtabmap" ...
   <param name="Rtabmap/DetectionRate" type="string" value="1"/> 

   <param name="RGBD/OptimizeIterations" type="string" value="10"/>  <!-- 10 for GTSAM, 100 for TORO -->
   <param name="RGBD/OptimizeStrategy" type="string" value="2"/> <!-- TORO=0, g2o=1, GTSAM=2 -->
   <param name="RGBD/OptimizeRobust" type="string" value="true"/>

   <param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- default is now 0.1, less loop closure rejections -->
</node>

<!-- 
Remove these nodes: 
<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/> 
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">  
   <param name="occupancy_grid" type="bool" value="false"/>
   <remap from="mapData" to="mapData_optimized"/>
   <remap from="grid_projection_map" to="/map"/>
</node>
-->

<node name="rtabmapviz" ...
   <remap from="mapData" to="mapData"/> <!-- Use mapData from rtabmap node -->
</node>

cheers,
Mathieu