Map correction decays to identity when static about rtabmap

[matlabbe]

the odometry correction would not decay to identity, unless odometry was indeed perfect, but the tag detection was wrong... and the more tag detections you have, the more you can correct the tag orientation (which individually are often bad), thus slowly "fix" the initial bad tag orientation error.

[DennisMelamedKitware]

Thanks @matlabbe, I really appreciate your response and help.

I think I'm starting to understand the issue here - odometry is assuming it starts at the origin (identity transform), which doesn't match the fact that I've set one of my arucos to be the origin (set the prior pose to 0,0,0,0,0,0). Eventually this could cause the decay I mention above, I think.

Is there any ability in rtabmap standalone to set the initial pose of the camera/robot in mapping mode? If I set it to be the correct transform relative to the tag I'm using as my map origin, that should lead to a consistent estimation (assuming everything else is correct), right? From this (

rtabmap/corelib/src/Rtabmap.cpp

Line 852 in a11ea29

void Rtabmap::setInitialPose(const Transform & initialPose)

) it appears we can only set the initial pose in localization mode, is that accurate?

Or perhaps I'm misunderstanding, the help text in Marker Detection says:

World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. 

I'm not understanding what that second sentence means, could you provide any clarification?

[matlabbe]

Odometry will always start at (0,0) in its odometry frame (i.e.: "odom -> base_link" would be Identity on startup). On the mapping side, the "map -> odom" transform will be Identity till a loop closure is detected.

Marker/Priors parameter can be used to set a "world" coordinate to a tag. To include that prior when detecting a tag, we should set Optimizer/PriorsIgnored=false. So for:

The map will be transformed in marker's world frame when a tag is detected.

it means that the initial pose of the camera will be Identity until a tag is detected. When the tag is detected, the graph will be transformed in tag's coordinate frame, so that "map->odom" will be updated to make the camera pose (map -> odom -> base_link) appears in tag's frame. For example: If you bringup the camera, move forward 1 meter along x axis (so that odom->base_link is now x=1, y=0), then detect a tag 1 meter away with a prior at (x=10 y=0), the "map -> odom" will be updated to( x=8, y=0), so that the camera pose in the map frame "map->odom->base_link" will be (x=9, y=0).

[DennisMelamedKitware]

@matlabbe thanks for the clarification - the "map->odom->base_link" transform is what is computed in this example (https://github.com/introlab/rtabmap/wiki/Cplusplus-RGBD-Mapping) by odometryCorrection_*odom.pose(), right?

[matlabbe]

Yes, map->odom would be odometryCorrection_, and odom->base_link would be odom.pose().