RTABMAP unable to detect loop closure for corridors

[alexr]

Hi,

I am using ROS indigo. The problem is that when the robot is turning back to the mapped corridor, it cannot find correspondence to the features and cannot detect loop closure.

I recently switched to ROS indigo to test the bag, but had no such problem on Hydro.  

My launch file with parameters is below:

<launch>

  <!-- ROBOT MAPPING VERSION: use this with ROS bag demo_mapping.bag -->
  <!-- WARNING : Database is automatically deleted on each startup -->
  <!--           See "delete_db_on_start" option below... -->

  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" /> 
  
  <param name="use_sim_time" type="bool" value="True"/>

  <group ns="rtabmap">
    <!-- SLAM (robot side) -->
    <!-- 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="wait_for_transform" type="bool" value="true"/>
	
	  <param name="subscribe_depth" type="bool" value="true"/>
	  <param name="subscribe_scan" type="bool" value="true"/>
	
	  <remap from="odom" to="/odom"/>
	  <remap from="scan" to="/scan"/>

	  <remap from="rgb/image" to="/data_throttled_image"/>
  	  <remap from="depth/image" to="/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
  	
      <param name="rgb/image_transport" type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
	
	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
	  <param name="RGBD/ProximityBySpace" type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
	  <param name="RGBD/ProximityByTime" type="string" value="false"/>   <!-- Local loop closure detection with locations in STM -->
	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> 
	  <param name="Reg/Strategy" type="string" value="2"/>                      <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
	  <param name="Vis/MaxDepth" type="string" value="4.0"/>                <!-- 3D visual words maximum depth 0=infinity -->
	  <param name="Vis/MinInliers" type="string" value="5"/>
	  <param name="Vis/InlierDistance" type="string" value="0.05"/>          <!-- 3D visual words correspondence distance -->
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>    <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
	  <param name="Reg/Force3DoF"             type="string" value="true"/>
	  <param name="Mem/IncrementalMemory" type="string" value="true"/>          <!-- true = Mapping/SLAM mode-->
      <param name="DbSqlite3/InMemory" type="string" value="false"/>
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>     <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
      <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
      <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
      <param name="Kp/MaxFeatures" type="string" value="400"/>
      <param name="SURF/HessianThreshold" type="string" value="1000"/>
      <param name="Rtabmap/StatisticLogged" type="string" value="true"/>
      <param name="Rtabmap/StatisticLogsBufferedInRAM" type="string" value="true"/>
	  <param name="Icp/Iterations" type="string" value="100"/>
	  <param name="Icp/VoxelSize" type="string" value="0"/>
	  <param name="RGBD/NewMapOdomChangeDistance" type="string" value="4"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>    <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
      <param name="Grid/FromDepth" type="string" value="false"/>     <!-- Set to true if using laserscan_from_depth (rgbd sensor) -->
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
	  <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
          
    </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_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="wait_for_transform" type="bool" value="true"/>
    
      <remap from="rgb/image" to="/data_throttled_image"/>
      <remap from="depth/image" to="/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
      <remap from="scan" to="/scan"/>
      <remap from="odom" to="/odom"/>
      
      <param name="rgb/image_transport" type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>
  </group>
  
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/data_throttled_image"/>
    <remap from="depth/image"     to="/data_throttled_image_depth"/>
    <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
    
    <param name="rgb/image_transport" type="string" value="compressed"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>
    
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>

</launch>

Attached is the picture of the scene (Indigo). Another problem is that the long corridor map does not remain straight for most part of the mapping process.The robot has relatively good odometry and I am using a hokuyo lidar as well for scan matching.  Is there any paraeters I can set to correct this effect.  As I said in Hydro I did not faced such problem. NOt sure what is wrong.

Next picture is from same bag file when run on ROS Hydro.

Many Thanks

Alex

[matlabbe]

Hi,

The problem seems that laser scans are not used to refine odometry or to find proximity links (yellow links in the second screenshot). Not sure why, the parameters look ok. Can you share the bag or the resulting database of the first screenshot?

cheers

[alexr]

Hi Mathieu,

Can I have your email address. I will send the bag file there.

Thanks

Alex

[matlabbe]

Hi,

I've found the problem. Set "Reg/Strategy" to 1 instead of 2. If you look at the latest demo_robot_mapping.launch:

<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->

When having external odometry and a LIDAR, I recommend to use Reg/Strategy to 1.

You can also try to calibrate better the odometry of the robot, as it drifts quite fast on rotation. Here is an example where the robot rotates 180 degrees and traverses the same path:

Map with ICP refning


Map without ICP refining (raw odometry)


cheers,
Mathieu

[matlabbe]

Just for information, I did run all the ros bag and here the results:

Raw odometry:



RGBD/NeighborLinkRefining=true, without loop closures and proximity links (RGBD/ProximityBySpace=false):



RGBD/NeighborLinkRefining=true, with loop closures and proximity links (RGBD/ProximityBySpace=true):

[alexr]

Dear Mathieu,
Thank you very much for your help

How can check the logged statistics from RTABMAP

 <param name="Rtabmap/StatisticLogged" type="string" value="true"/>
 <param name="Rtabmap/StatisticLogsBufferedInRAM" type="string" value="true"/>

Also, how can I open the RTAB Map database viewer as you showed here. Can you point to right tutorials.

Thanks
Alex

[matlabbe]

Hi,

Logged statistics are saved in the working directory. Under ROS, the working directory is "~/.ros". The files are named LogF.txt and LogI.txt. For example, using the showlogs.m script, you can do under MATLAB or Octave (similar to this tutorial):

showlogs('~/.ros/.');

Note that from version 0.11.11, some statistics are also saved directly in the database, and you can browse them in DatabaseViewer under Statistics view (values can be copied to clipboard with right-click on the legend, I also just updated the code to show all values in the plot instead of the last 50):


The only documation on DatabaseViewer is here: https://github.com/introlab/rtabmap/wiki/Tools#database-viewer. Do File->Open, select a database (*.db), click on View menu and show up all panels to see everything.

cheers,
Mathieu

[alexr]

Hi.

Many thanks for the detailed answer.

One thing, I did not understand what you meant by the odometry is drifting when turning the robot at place. How do I fix it?

Thanks
Alex

[matlabbe]

Hi,

It means that if the robot turns 90 deg in real world, the robot thinks it turned 110 deg for example. Odometry calibration may help to reduce this error.

cheers,
Mathieu