Loop Closure Issue + FLIR Question

[aarif]

Hi,

I'd like some help on a few things today. I'm using RTAB-Map using a P3-DX, Kinect and a Hokuyo Rangefinder. Our final goal is to have the P3-DX autonomously navigate a floor and map it in 3D. We're running into issues with incorrect loop closures.

I uploaded a video here. This video shows me manually navigating the P3-DX on a floor at my university. As you can see at time 22:30 of this video, the map completely collapses on itself due to it assuming that it's at the other end of the floor. Also, I noticed that some long hallways seemed to have invalid loop closures on itself (as seen at 1:28) as I moved the P3-DX along that hallway.

What can I do to fix this problem? What do you suggest?

Here is my launch file that I used to make this work (I am using ROS Indigo on Ubuntu 14.04):

<?xml version="1.0"?>

<launch>



	<arg name="HOME"                    default="/home/abdul"/>
	<arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
	
	<!-- If openni.launch is changed, these topic names may also need to be changed -->
	<arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
	<arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
	<arg name="camera_info_topic"       default="/camera/rgb/camera_info" />

	<arg name="subscribe_scan"          default="false"/>
	<arg name="subscribe_scan_cloud"    default="false"/>     
	<arg name="scan_topic"              default="/scan"/>
	<arg name="scan_cloud_topic"        default="/scan_cloud"/>
	
	<arg name="odom_topic"              default="/RosAria/pose"/>
	<arg name="visual_odometry"         default="false"/>

	<!-- argument that defines the port that will be used to interface with the P3-DX from our laptop -->
	<arg name="port_val"                default="/dev/ttyUSB0"/>

	<!-- cmd_vel param to direct the topic name for our movement msgs -->
	<param name="cmd_vel_topic" value="/RosAria/cmd_vel"/>



<!-- This section launches openni.launch, which launches the RGBD camera and makes all of the nodes/topics available -->
	<include file="$(find openni_launch)/launch/openni.launch"> 
		<arg name = "depth_registration" value = "true"/>
	</include>



	<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
		<param name="scan_height"     value="60"/>
		<param name="output_frame_id" value="/camera_depth_frame"/>
		<param name="range_min"       value="0.45"/>
		<param name="range_max"       value="4.0"/>

		<remap from="image"           to="$(arg depth_registered_topic)"/>
		<remap from="scan"            to="$(arg scan_topic)"/>
	</node>



	<node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="0 0 0 0 0 0 /base_link /camera_link 100" /> <!-- To relate the camera's tf tree to the P3-DX's "basic" tf tree -->
  
	<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_depth" type="bool" value="true"/>

      <remap from="odom" to="/RosAria/pose"/>
      <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <param name="queue_size" type="int" value="10"/>

      <!-- RTAB-Map's parameters -->
      <param name="RGBD/AngularUpdate" type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate" type="string" value="0.01"/>
      <param name="Rtabmap/TimeThr" type="string" value="700"/>
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>

    </node>



    <!-- Visualisation RTAB-Map -->

    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" launch-prefix="">

      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="false"/>
      <param name="subscribe_scan_cloud" type="bool" value="false"/>
      <param name="subscribe_odom_info" type="bool" value="false"/>
      <param name="frame_id" type="string" value="camera_link"/>
      <param name="wait_for_transform_duration" type="double" value="0.2"/>

      <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>

      <remap from="odom" to="/RosAria/pose"/>

    </node>
  </group>


	<node pkg="topic_tools" type="relay" name="map_relay" args="/rtabmap/grid_map /map" />


	<!-- Interface with the P3-DX -->
	<node name="RosAria" pkg="rosaria" type="RosAria" required="true">
		<param name="port" value="$(arg port_val)"/>
		<remap from="/RosAria/pose" to="$(arg odom_topic)"/>
	</node>



	<!-- Control the P3DX model with the ARC -->
	<node name="map_mover" type="map_mover.py" pkg="native" output="screen" required="true"/>


</launch>

I have one more question: If I wanted to incorporate FLIR camera information to your RTAB-Map on ROS, how would I approach it?

Thank you for your time

[matlabbe]

Hi,

Repetitive areas are quite challenging for loop closure detection. For the long corridor problem, if you have relatively good odometry (errors between two nodes added would be under 10 cm), you can reduce the "RGBD/OptimizeMaxError" parameter under 0.1 m or less (instead of 1 meter). That parameter will reject a loop closure if the graph has been too much corrected (e.g., if a single odometry link take more than 10 cm of error):

<rosparam name="RGBD/OptimizeMaxError" type="string" value="0.1"/>

Be careful that setting it too low comparatively to odometry errors would result in rejecting all loop closures.

You can also try Vertigo (a robust graph optimization approach) if you have rtabmap compiled with g2o or GTSAM (note that this parameter is not compatible with RGBD/OptimizeMaxError, so the later should be set to 0):

<rosparam name="RGBD/OptimizeMaxError" type="string" value="0"/> <!- disabled as Optimizer/Robust is enabled-->
<rosparam name="Optimizer/Strategy"    type="string" value="2"/> <!- 1=g2o 2=GTSAM -->
<rosparam name="Optimizer/Robust"      type="string" value="true"/> <!- disabled -->

Note that odometry covariance should be correctly set to get good results.

The parameters above can be used to detect/reject wrong loop closures, but you may want to increase tolerance of the loop closure estimation process (to avoid detecting false loop closures at first):

<rosparam name="Rtabmap/LoopThr" type="string" value="0.11"/> <!-- Setting it over 0.5 may reduce the chance of false positive hypotheses -->
<rosparam name="Mem/STMSize"     type="string" value="10"/> <!-- For long corridors, you may want to set it over 20 -->

The idea behind "Mem/STMSize" is to not estimate loop closure hypotheses for the last seen locations.

For FLIR integration, it depends if you just want to save them in the nodes (like adding "data.setUserData(flirImage)" here) or if you want to use them for loop closure hypotheses (would require not so trivial changes to core library).

cheers,
Mathieu

[aarif]

Hi Mathieu,

Thank you so much for your response. In our implementation, we'll be using an EKF to estimate the pose of the robot. For this reason, the first option worked really well when I tried mapping a floor. Once again, thank you for your help!

Cheers,
Abdul