Using external odometry with rtabmap slam and aligning to gravity

[dinossht]

Is it possible to align rtabmap slam map to gravity if external odometry is used? I am using rtabmap_slam node which takes in Odometry topic. The odometry is not aligned with gravity. But I have access to imu data, so can I use this? I tried to set the imu topic  "imu" in rtabmap_slam. But this did not help.

[matlabbe]

If rtabmap_slam is correctly receiving your imu topic (which should contain estimated orientation, if not, you can use Madgwick imu filter ros node to estimate it), it should add the gravity constraints to the graph and re-optimize at each new update.

Another way to debug, if you open the database in rtabmap-databaseViewer, you should see if the nodes have gravity constraints:

[dinossht]

I can confirm that the gravity is showing similar to your screenshot. However, rtabmap does not export neither show in rtabmap_viz, the properly oriented map. Can this be related to using external odometry?

[matlabbe]

Optimizer/GravitySigma should not be zero (default is 0.3) so that gravity constraints are used during graph optimization. Another thing is if the imu local transform is wrong (TF transform between base link and imu link, and/or camera link), the optimization may even align less the map to gravity. If you can share the database, I can take a look.

[dinossht]

output.db

It rtabamap_viz I can see that the map is aligned, but then when I use the rtabmap viewer, the map is not aligned.

[matlabbe]

The database looks empty (only 860 KB).

[dinossht]

Is this good enough? rtabmap_backup.db

[matlabbe]

The problem is that the lidar point cloud seems not in the base frame, but may be using odom frame computed externally as the base frame. What is the frame_id in the point cloud topic? In rviz, if you set the global frame to base_link, the lidar point cloud should not rotate when you rotate the lidar. It should rotate only if you set your odom frame in rviz config.

Here I show the point clouds in the supposed base frame. On one frame the Up vector of the point cloud is y+, and in the other one it is combination of y+ and z+.




Note also in the first frame the gravity vector is pointing to the ground of the point cloud, and the others the ceiling.

[dinossht]

You are correct. If I set the global frame to base_link, the lidar point cloud does rotate when I rotate the lidar. This seems to be the issue. I expected the cloud_registered_body topic from fastlio to be given in body (otherwise being base_link). But it seems to rotate with body frame.