errors with icp_odometry node when using ground_truth_frame_ids

[Adam Caccavale]

Hello,

I am hoping to use RTABmap to provide odometry for my lidar-equipped robot. Since the robot is not starting aligned with the world frame, I wanted to use ground_truth_frame_id and ground_truth_base_frame_id to set the initial value of the odometry. I am using an optitrack system, which provides a ground truth frame and its parent frame which I call world (my tf tree is shown below). When I try to set the ground truth frame parameters, the odometry immediately fails (code output is below). If I do not set these values, the odometry values seem correct except they are not expressed in the correct frame.

It seems like the correct ground truth frame offset is captured by the program, but when it tries to find the initial correspondences it applies this transform which results in no correspondences with the scan.


rosrun tf view_frames output:



my launchfile code:
<node name="rtabmap1" pkg="rtabmap_ros" type="icp_odometry" output="screen" args="--delete_db_on_start --udebug">
      <param name="frame_id" type="string" value="ouijabot1_odom"/>
      <param name="odom_frame_id"    value="ouijabot1_odom_parent"/>
      <param name="ground_truth_frame_id" type="string" value="world"/>
      <param name="ground_truth_base_frame_id" type="string" value="ouijabot1"/>
      <remap from="/scan" to="/ouijabot1/scan" /> 
</node>


console output:

[matlabbe]

ICP is failing because there are not enough correspondences. If you start from the same place but without ground_truth_frame_id set, does ICP fail too?

[Adam Caccavale]

ICP does not fail if I do not use the ground_truth_frame_id

[matlabbe]

which rtabmap version are you using?

[matlabbe]

Nevermind, I reproduced the bug. It is because the ground truth is 6DOF (z, roll and pitch are not all zero). Could you have a ground truth that is 3DOF instead?

[Adam Caccavale]

I dont think I have a way I can edit the frame, especially not the z axis which is centered where the lidar is located. Is there another work-around? I would need a way to copy a frame, and then zero out those specific elements in my launch file. The only way I know to do that would be to write a node, publish the modified frame, and delay the rest of my launch file until that frame can be used.

[matlabbe]

Note that you should use the base_footprint frame as frame_id for odometry/rtabmap. That frame is on the ground with zero Z. The ground truth should be the frame referencing the base_footprint, not the lidar, so it would be Z=0. Here it seems the ground truth doesn't match the right frame.

cheers,
Mathieu