Rtabmap shifting position - fast relocalization?

[martinerk0]

Hi @matlabbe , I'm using rtabmap on a real robot - Kobuki with Astra depth camera, and I'm observing small shifts when robots is not moving.
I'm doing fake laserscan from pointcloud, odometry from Kobuki.

Here is the video: https://youtu.be/Xmj-cdf_Qik

Launchfile:

<launch>

  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgbd_odometry"     default="false"/>
  <arg name="rtabmapviz"        default="false"/>
  <arg name="localization"      default="false"/>
  <arg name="simulation"        default="false"/>
  <arg name="sw_registered"     default="false"/>
  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>

  <arg     if="$(arg simulation)" name="rgb_topic"   default="/camera/rgb/image_raw"/>
  <!-- <arg unless="$(arg simulation)" name="rgb_topic"   default="/camera/rgb/image_rect_color"/> -->
  <arg unless="$(arg simulation)" name="rgb_topic"   default="/camera/rgb/image_rect_color"/>
  <arg     if="$(arg simulation)" name="depth_topic" default="/camera/depth/image_raw"/>
  <arg unless="$(arg simulation)" name="depth_topic" default="/camera/depth_registered/hw_registered/image_rect"/>
  <arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
  
  <arg name="wait_for_transform"  default="0.2"/> 

  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
  
  <!-- Mapping -->
  <group ns="rtabmap">

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <!-- <param name="frame_id"            type="string" value="base_footprint"/> -->
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="subscribe_depth"     type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="map_negative_poses_ignored" type="bool" value="true"/>

          <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
      <param name="use_action_for_goal" type="bool" value="true"/>
          <remap from="move_base"            to="/move_base"/>
    
      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

          <!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
          <param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
      
      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- <param name="subscribe_scan"       type="bool"   value="false"/> -->
    
      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
      <param name="Reg/Strategy"                 type="string" value="0"/>      <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
      <param name="Vis/MinInliers"               type="string" value="15"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
      <param name="Rtabmap/TimeThr"              type="string" value="0"/>
      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
      <param name="Reg/Force3DoF"                type="string" value="true"/>
      <param name="GridGlobal/MinSize"           type="string" value="20"/>

      
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
    </node>
   
    
  </group>
</launch>

What is the reason for the shifting? Are the nodes used for loops closures close together and since camera data fluctutates a bit it switches between the nodes? Thanks!

[matlabbe]

Depending on the node matched for loop closure and the noise of the depth image, localization can jump like this. Decreasing odom_tf_linear_variancecould also help to reduce the jumps. Not sure which rtabmap version you are using, but since January last year, localizations are filtered to smooth the final output (it is why when decresing odom covariance, it kinda says "from odometry I am pretty confident I didn't move, so the new localization jumps will be smoothed").