rtabmap_ros on p3dx with wheel odometry and kinect data

[udit]

Sir, I am trying to implement rtabamap_ros on p3dx bot with odometry data from wheel encoders with kinect. I am using rtabmap.launch.

Since odometry data is published in /pose topic, so I did:
1. $ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" visual_odometry:=true odom_topic:=/pose rviz:=true rtabmapviz:=true

Then initially I got warning like these:
[ WARN] [1533745834.846713671]: Could not get transform from odom to camera_link after 0.200000 seconds (for stamp=1533745834.300975)! Error="Lookup would require extrapolation into the future.  Requested time 1533745834.300974537 but the latest data is at time 1533745786.048471536, when looking up transform from frame [camera_link] to frame [odom]. canTransform returned after 0.201137 timeout was 0.2.".

Then while at loop closure I got this:
[ERROR] (2018-08-08 22:00:34.872) OptimizerGTSAM.cpp:347::optimize() GTSAM exception caught:
Indeterminant linear system detected while working near variable
22 (Symbol: 22).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
[ERROR] (2018-08-08 22:00:34.872) Rtabmap.cpp:3276::optimizeCurrentMap() Failed to optimize the graph! returning empty optimized poses...


And the loop hypothesis got rejected for the same starting place with lot of features, as shown in this screenshot:



2. I also tried modifying the demo_turtlebot_mapping.launch https://github.com/introlab/rtabmap_ros/blob/master/launch/demo/demo_turtlebot_mapping.launch

by replacing :

frame_id -> base_link
and replacing <remap from="scan"            to="/scan"/> with
 <remap from="odom"            to="/pose"/>
also setting subscribe_depth and subscribe_scan to false, like this:

<launch>
  <!-- 
       Bringup Turtlebot:
       $ roslaunch turtlebot_bringup minimal.launch
       
       Mapping:
       $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch
       
       Visualization:
       $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
       
       This launch file is a one to one replacement of the gmapping_demo.launch in the 
       "SLAM Map Building with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
       
       For localization-only after a mapping session, add argument "localization:=true" to
       demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in 
       the previous map, then the 2D map should re-appear again. You can then follow the same steps 
       from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
 
       For turtlebot in simulation (Gazebo):
         $ roslaunch turtlebot_gazebo turtlebot_world.launch
         $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
         $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
  -->
  
  <!-- frame_id -> base_link -->
  
  <arg name="database_path"     default="~/.ros/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     if="$(arg simulation)" name="depth_topic" default="/camera/depth/image_raw"/>
  <arg unless="$(arg simulation)" name="depth_topic" default="/camera/depth_registered/image_raw"/>
  <arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
  
  <arg name="wait_for_transform"  default="0.2"/> 
  <!-- 
      robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml" 
      can be increase from 5 to 10 Hz to avoid some TF warnings.
  -->
  
  <!-- Navigation stuff (move_base) 
  <include unless="$(arg simulation)" file="$(find turtlebot_bringup)/launch/3dsensor.launch">
     <arg     if="$(arg sw_registered)" name="depth_registration" value="false"/>
     <arg unless="$(arg sw_registered)" name="depth_registration" value="true"/>
  </include>
  <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_link"/>
	  <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
	  <param name="subscribe_depth"     type="bool"   value="false"/>
	  <param name="subscribe_scan"      type="bool"   value="false"/>
	  <param name="map_negative_poses_ignored" type="bool" value="true"/>
	
	  <!-- inputs -->
	  <remap from="odom"            to="/pose"/>
	  <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)"/>
	  <remap unless="$(arg rgbd_odometry)" from="odom" to="/odom"/>
  	  
  	  <!-- output -->
  	  <remap from="grid_map" to="/map"/>
	
	  <!-- 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="700"/>
	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
	  <param name="Optimizer/Slam2D"             type="string" value="true"/>
	  <param name="Reg/Force3DoF"                type="string" value="true"/>
	  <param name="GridGlobal/MinSize"           type="string" value="20"/>
          <param name="RGBD/OptimizeMaxError"        type="string" value="0.1"/>

	  
	  <!-- 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>
   
    <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
    <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="frame_id"                    type="string" value="base_link"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="Reg/Force3DoF"               type="string" value="true"/>
      <param name="Vis/InlierDistance"          type="string" value="0.05"/>
      
      <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)"/>
    </node>
    
    <!-- visualization with rtabmapviz -->
    <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="false"/>
      <param name="subscribe_scan"              type="bool" value="false"/>
      <param name="frame_id"                    type="string" value="base_link"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
    
      <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)"/>
      <remap from="odom"            to="/pose"/>
    </node>
    
  </group>
</launch>

and got output like:

/rtabmap/rtabmap subscribed to:
   /camera/rgb/image_rect_color
[ INFO] [1533753195.269828623]: rtabmap 0.17.4 started...
[ERROR] (2018-08-09 00:03:18.538) Rtabmap.cpp:999::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 5194 is ignored!
[ WARN] [1533753198.538483464]: RTAB-Map could not process the data received! (ROS id = 5194)


My doubt is I am not sure which approach out of two above should I follow since both are giving errors. And basically I just want to provide my wheel odometry (Topic: /pose) to rtabmap as mentioned here http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_Odometry
And also want to visualize the loop closure part in rtabmapviz or rviz . Later I would be fusing odometries as mentioned in the answer: https://answers.ros.org/question/251369/how-does-rtabmap-fuse-wheel-odometry-with-visual-odometry/ 
So could you provide an example of launch file that I should use. Finally I want to use the navigation stack.

Thanks

[matlabbe]

Did you try the example you referred?

<launch>
  <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="/base_controller/odom"/>

          <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>
  </group>
</launch>

by changing frame_id and remap input topics if needed. You may also want to set Reg/Force3Dof to true if your robot only moves in 2D.

Can you show TF tree? Make sure /odom -> "fixed frame on your robot" TF is published.

$ rosrun tf view_frames
$ evince frames.pdf

Can you show rqt_graph?

$ rqt_graph

Can you show what /pose looks like?

$ rostopic echo /pose

[udit]

Sir, your doubt was correct earlier I had two disjointed tf trees. Now everything works as you mentioned in this example http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_Odometry

And my exact launch file is:

<launch>
  <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="/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>
  </group>
</launch>

And before launching it I do transform between base_link and camera_link as:
$ roslaunch openni_launch openni.launch depth_registration:=true
$ rosrun tf static_transform_publisher 0.2 0 0.45 0 0 0 base_link camera_link 100
$ roslaunch rtabmap_ros kinect_odometry.launch

The frames.pdf looks like:


Also,
$ rostopic ehco /pose

header: 
  seq: 2335
  stamp: 
    secs: 1533905272
    nsecs: 818076442
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.175
      y: -0.222
      z: 0.0
    orientation: 
      x: -0.0
      y: 0.0
      z: 0.0784590957278
      w: -0.996917333733
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]

And
$ rqt_graph
rosgraph.dot


But I have two doubts:
1. I am not able to visualize the point cloud and trajectory in rtabmapviz, and hence not able to find when the loop closure happens(green boundaries)

2. When I try to form a loop and return to the same starting point then, I am getting this error:

[ERROR] (2018-08-10 18:17:02.405) OptimizerGTSAM.cpp:347::optimize() GTSAM exception caught: 
Indeterminant linear system detected while working near variable
12 (Symbol: 12).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
[ERROR] (2018-08-10 18:17:02.405) Rtabmap.cpp:3276::optimizeCurrentMap() Failed to optimize the graph! returning empty optimized poses...
[ WARN] (2018-08-10 18:17:02.405) Rtabmap.cpp:2331::process() Graph optimization failed! Rejecting last loop closures added.
[ WARN] (2018-08-10 18:17:02.405) Rtabmap.cpp:2335::process() Loop closure 99->11 rejected!

EDIT:

I tried the second approach using rtabmap.launch https://github.com/introlab/rtabmap_ros/blob/master/launch/rtabmap.launch

I did commands like this:
$ roslaunch openni_launch openni.launch depth_registration:=true
$ rosrun tf static_transform_publisher 0.2 0 0.45 0 0 0 base_link cameraink 100
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" visual_odometry:=false odom_topic:=/pose rviz:=true rtabmapviz:=true

And got the same error:

[ERROR] (2018-08-10 18:56:34.041) OptimizerGTSAM.cpp:347::optimize() GTSAM exception caught: 
Indeterminant linear system detected while working near variable
92 (Symbol: 92).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
[ERROR] (2018-08-10 18:56:34.041) Rtabmap.cpp:3276::optimizeCurrentMap() Failed to optimize the graph! returning empty optimized poses...
[ WARN] (2018-08-10 18:56:34.041) Rtabmap.cpp:2331::process() Graph optimization failed! Rejecting last loop closures added.
[ WARN] (2018-08-10 18:56:34.041) Rtabmap.cpp:2335::process() Loop closure 128->92 rejected!
[ WARN] (2018-08-10 18:56:34.041) Rtabmap.cpp:2335::process() Loop closure 128->91 rejected!

Thanks

[matlabbe]

Hi,

In your topic /pose, the variance for angular values is too high 1000000.0, causing GTSAM to fail optimization as solutions are infinite (unbounded). It should be around more around 0.001 or 0.0005.

Another approach is to use TORO optimization instead (Optimizer/Strategy parameter set to 0), which is less sensitive to underestimated and overestimated covariances. You can also try Optimizer/VarianceIgnored set to true to just ignore covariance from the optimization (should mostly be used when TORO is selected though).

cheers,
Mathieu

[udit]

Sir, your debugging solutions worked like a charm.
I added

<param name="Reg/Force3Dof" value="true"/>
<param name="Optimizer/Slam2D" value="true"/>
<param name="Optimizer/Strategy" value="0"/>
<param name="Optimizer/VarianceIgnored" value="0"/>

in rtabmap node of rtabmap.launch https://github.com/introlab/rtabmap_ros/blob/master/launch/rtabmap.launch

and did:
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" visual_odometry:=false frame_id:=/base_link odom_topic:=/pose rviz:=true rtabmapviz:=true

and the result is as shown in the video https://drive.google.com/file/d/1KO6ONJ4q2ZmpXHJyWlMSenaOrMSAsO93/view?usp=sharing

[matlabbe]

Not sure if it is because the screen recording was lagging, but the movements of the robot should be a lot smoother. For rviz, you may try to remove the Pointcloud2 display of the current cloud (voxel_cloud) when recording to save some computation power. For rtabmapviz, uncheck showing the odometry cloud under Preferences -> 3D Rendering.

[udit]

Thanks sir for reviewing the results. The motion of robot is really jerky. I would surely try your described methods.

I have 2 questions:

1. Loop closure is happening very late after returning to the original position, after I made the changes in launch file:

 <param name="Reg/Force3Dof" value="true"/>
 <param name="Optimizer/Slam2D" value="true"/>
 <param name="Optimizer/Strategy" value="0"/>
 <param name="Optimizer/VarianceIgnored" value="0"/>

and using wheel odometry, as compared to instant loop closure using default parameters and visual odometry. What parameters could be fine tuned for instant loop closure with new parametes?


2. I would like to know the approach used by RTABMap in localization mode. As according to my knowledge it is not using Monte Carlo Localization. So how on the basis of loop closure detection and odometry data RTABMap able to localize itself in the prebuilt map. I have gone through the papers mentioned in this answer https://answers.ros.org/question/245873/rtabmap_ros-how-it-works/
But they mentioned about loop closure detection and Multi session Mapping.
 
Could you provide some resources detailing the localization approach used in RTABMap, as our implementation is on prebuilt Map so we requires good accuracy in relocalization?
Or should we need to integrate some other packages like AMCL for relocalization with RTABMap for increasing accuracy during relocalization?


Thanks

[matlabbe]

1. Are there any warnings on terminal when it should be finding a loop closure? (for possible rejection reasons). Note that in recent versions, Optimizer/Slam2D is included in Reg/Force3Dof.

2. From this paper:

In this paper, the RGB image, from which the visual words are extracted, is registered with a depth image, i.e., for each 2D point in the RGB image, a 3D position can be computed using the calibration matrix and the depth information given by  the  depth  image.  The  3D  positions  of  the  visual  words are  then  known.  When  a  loop  closure  is  detected,  the  rigid transformation between the matching images is computed by a  RANSAC  approach  using  the  3D  visual  word  correspondences.  If  a  minimum  of I inliers  are  found,  loop  closure is  accepted  and  a  link  with  this  transformation  between the  current  node  and  the  loop  closure  hypothesis  node  is added  to  the  graph.  If  the  robot  is  constrained  to  operate on  a  single  plane,  the  transformation  can  be  refined  with 2D iterative-closest-point (ICP) optimization [18] using laser scans contained in the matching nodes.

Localization is exactly done the same than loop closure detection described above. To integrate AMCL to RTAB-Map, I would have to dig more on how AMCL is done. However, if a lidar is used in RTAB-Map, the resulting 2D map could be fed to AMCL package to do localization (disabling RTAB-Map's localization Kp/MaxFeatures=-1 and map->odom tf publish_tf = false for rtabmap node). It seems that AMCL coudl be implemented for other sensors than lidar:

As currently implemented, this node works only with laser scans and laser maps. It could be extended to work with other sensor data.

You may search if there is someone that already try to implement it for vision-based localization.

In general, to increase rtabmap localization accuracy (filtering noisy localization transforms), an integration of a Kalman filter (robot_localization) or a particle filter (AMCL) could indeed smooth the localizations. Note however that on RTAB-Map side, if you use only visual registration, you can increase Vis/MinInliers parameter to avoid accepting transforms with a low number of matched features (which are usually more erroneous).

cheers,
Mathieu