Bad MapCloud with RtabMap

edit

Hi all,

I'm trying to use RtabMap for 3D map reconstruction in ROS Kinetic. My robot is equipped with:

• odometry (MD25 + EMG)
• lidar (RPLidar)
• depth camera (RealSense R200)

I followed the tutorial from ROS wiki but the resulting map is really strange:

It looks like the map follow the rebot during movement: i checked the TF tree and messages frames and all looks good (by the fact if I show depth cloud topic from camera it looks to have right frame). Furthermore the map generated by laser scan seems to not be affected by same problem.

Here the TF tree:

and RtabMap parameters:

 * /rtabmap/rtabmap/Grid/FromDepth: false
 * /rtabmap/rtabmap/Mem/RehearsalSimilarity: 0.45
 * /rtabmap/rtabmap/RGBD/AngularUpdate: 0.01
 * /rtabmap/rtabmap/RGBD/LinearUpdate: 0.01
 * /rtabmap/rtabmap/RGBD/NeighborLinkRefining: true
 * /rtabmap/rtabmap/RGBD/OptimizeFromGraphEnd: false
 * /rtabmap/rtabmap/RGBD/ProximityBySpace: true
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/Rtabmap/TimeThr: 700
 * /rtabmap/rtabmap/Vis/InlierDistance: 0.1
 * /rtabmap/rtabmap/Vis/MinInliers: 5
 * /rtabmap/rtabmap/frame_id: base_link
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/queue_size: 100
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: True

EDIT

Here the transformation from base_link to camera_rgb_optical_frame:

$ rosrun tf tf_echo base_link camera_rgb_optical_frame
At time 0.000
- Translation: [0.125, -0.045, 0.130]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]
        in RPY (radian) [-1.571, -0.000, -1.571]
        in RPY (degree) [-90.000, -0.000, -90.000]
At time 0.000
- Translation: [0.125, -0.045, 0.130]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]
        in RPY (radian) [-1.571, -0.000, -1.571]
        in RPY (degree) [-90.000, -0.000, -90.000]
[...]

The transformation never change accordling to the fact that camera is fixed to the robot base.

EDIT 2

The RtabMap DB can be found here.

You can open it with rtabmap-databaseViewer and see that while the 2D map by scan is quite good the 3D map is really bad!

EDIT 3

Here the topic published frequencies:

subscribed to [/scan]
average rate: 7.733
    min: 0.060s max: 0.214s std dev: 0.05153s window: 7
subscribed to [/odom]
average rate: 30.019
    min: 0.029s max: 0.039s std dev: 0.00152s window: 30
subscribed to [/camera/depth_registered/image_rectified]
average rate: 29.917
    min: 0.019s max: 0.054s std dev: 0.00792s window: 29
subscribed to [/camera/rgb/image_rectified]
average rate: 30.136
    min: 0.016s max: 0.055s std dev: 0.00766s window: 28
subscribed to [/camera/rgb/camera_info]
average rate: 29.909
    min: 0.022s max: 0.046s std dev: 0.00628s window: 28

Laser scan is published at about 8 Hz where odometry and depth are published at 30 Hz. Since odometry and depth came from different sensors the publish rate is the same but they are not synchronized.

Any help will be appreciated!

Thanks Ale