Receiving updated odometry

[cpsarras]

Hello,

I am using RTAB-Map-ROS to get visual odometry measurements. However, I would like to receive the updated trajectory, after loop closure has been detected and the odometry has been corrected. Is there a service/topic in the RTAB-Map-ROS that provides this updated trajectory? If not, how would I go about adding it?

Thank you

[matlabbe]

Hi,

You can get the graph of the map (corrected trajectory) with /rtabmap/mapGraph topic. You can also visualize it in RVIZ by adding rtabmap_ros/MapGraph display.

The graph can be also exported from rtabmapviz (File->Export poses...) or offline by reopening the database with standalone version ("$ rtabmap ~/.ros/rtabmap.db") and do also File->Export poses...

cheers,
Mathieu

[cpsarras]

Thank you, the mapGraph topic is exactly what i was looking for. However i am noticing a strange pattern in the trajectory generated.

More specifically, i am running a gazebo simulation with the p3d plugin for ground truth odometry (red). The odometry produced by RTAB-Map (yellow) and the trajectory from the mapGraph topic (blue) are also visible. Even though the trajectory in the center path (where the robot has passed 3 total times) is almost identical to the ground truth, the trajectory generated after a wide turn to the left or to the right has a considerable offset (about 15cm). This offset is not corrected after loop closures. The accuracy of the generated /rtabmap/MapCloud was evaluated using landmarks, which indicate a millimeter level accuracy.

So the generated Point Cloud of the map is very accurate but the corrected trajectory posted by the mapGraph topic has a considerable offset from the ground truth. Is this reasonable?

[matlabbe]

Hi,

The odometry is computed by which node? There is indeed not so much or no correction done by the loop closures. Can you share the database? Maybe the covariance of the odometry is very small, so that loop closures (which would have higher covariance) won't be able to have an effect on graph optimization. Which parameters did you use for rtabmap node?

cheers,
Mathieu

[cpsarras]

matlabbe wrote

 The odometry is computed by which node?

The odometry is computed by RTAB-Map from a stereo camera input. No other source of odometry is used.

matlabbe wrote

 Maybe the covariance of the odometry is very small

What range of covariance values are considered 'high' or 'low'? I included a file with the odometry std deviation, as exported by RTAB-Map.

matlabbe wrote

 Can you share the database?

Gladly, rtabmap.db.

matlabbe wrote

 Which parameters did you use for rtabmap node?  

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.7
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/approx_sync: True
 * /rtabmap/rtabmap/config_path:
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: base_footprint
 * /rtabmap/rtabmap/ground_truth_base_frame_id:
 * /rtabmap/rtabmap/ground_truth_frame_id:
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/odom_frame_id:
 * /rtabmap/rtabmap/odom_tf_angular_variance: 1.0
 * /rtabmap/rtabmap/odom_tf_linear_variance: 1.0
 * /rtabmap/rtabmap/queue_size: 10
 * /rtabmap/rtabmap/subscribe_depth: False
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_stereo: True
 * /rtabmap/rtabmap/subscribe_user_data: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmapviz/approx_sync: True
 * /rtabmap/rtabmapviz/frame_id: base_footprint
 * /rtabmap/rtabmapviz/odom_frame_id:
 * /rtabmap/rtabmapviz/queue_size: 10
 * /rtabmap/rtabmapviz/subscribe_depth: False
 * /rtabmap/rtabmapviz/subscribe_odom_info: True
 * /rtabmap/rtabmapviz/subscribe_scan: False
 * /rtabmap/rtabmapviz/subscribe_scan_cloud: False
 * /rtabmap/rtabmapviz/subscribe_stereo: True
 * /rtabmap/rtabmapviz/wait_for_transform_duration: 0.2
 * /rtabmap/stereo_odometry/approx_sync: True
 * /rtabmap/stereo_odometry/config_path:
 * /rtabmap/stereo_odometry/frame_id: base_footprint
 * /rtabmap/stereo_odometry/ground_truth_base_frame_id:
 * /rtabmap/stereo_odometry/ground_truth_frame_id:
 * /rtabmap/stereo_odometry/odom_frame_id: odom
 * /rtabmap/stereo_odometry/queue_size: 10
 * /rtabmap/stereo_odometry/wait_for_transform_duration: 0.2

NODES
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)
    stereo_odometry (rtabmap_ros/stereo_odometry)

[matlabbe]

Hi,

It looks like that odometry is almost perfect (simulated environment), so loop closures won't correct a lot of errors. I think the error you are seeing may be related to camera calibration parameters as there is no drift, maybe it is just a scale problem (related to fx/fy/baseline camera parameters slightly off).

1080x720 
fx=378.113 
fy=378.113
cx=540.5 
cy=360.5 
baseline=0.06m 
T=xyz=0.300000,0.060000,0.417000 rpy=-1.745329,-0.000000,-1.570796

cheers,
Mathieu

[cpsarras]

Hello Mathieu,

I am using a simulated stereo camera in the Gazebo environment which has been tested to work fine in other applications. I am going to examine the simulation environment and compare it with real datasets. I will be posting any useful results or conclusions.

Thank you for your time and useful input,

Chris

[cpsarras]

Hello Mathieu,

I figured out the source of the problem. The horizontal resolution was incorrectly set to 1080 pixels (instead of 1280), which was too small, resulting in that noticeable error while turning. Also, increasing the HFOV of the simulated camera (from 110 to 120 degrees) has a similar effect in correcting this error, but it is less common to change the FOV of a real camera. Now the error is considerably smaller.

Thanks again for your help,

Chris