Publishing map->odom transform using custom localization and SLAM algorithms

asked 2022-06-01 14:34:42 -0500

onishly
13 ●1 ●2 ●5

I am writing my own FastSLAM and Monte-Carlo localization algorithms and I am trying to figure out how to calculate the map->odom transform. From what I understand, my SLAM and localization algorithm will give the transform from the map frame to the base_link frame when given odometry data calculated in the odom frame. I have seen that AMCL and Gmapping publish the transform from the odom frame to the map frame, but I am not sure of how to calculate this once I have the map to base_link transform from the SLAM or localization algorithm.

Would I assume that the transform from the odometry data is "correct", create transformation matrices for T_(map->base_link) and T_(odom->base_link) and then say:

T_(map->odom) = T_(map->base_link) * T_(odom->base_link)^-1

edit retag flag offensive close merge delete

add a comment

1

answered 2022-06-07 21:17:11 -0500

tianb03

710 ●16 ●39 ●33 https://github.com/tianbot

updated 2022-06-09 08:12:37 -0500

Though not all the same but similar for the localization and SLAM algorithms. The TF publishing in GMapping is easier to understand.

slam_gmapping/gmapping/src/slam_gmapping.cpp

map_to_odom_ = (odom_to_laser * laser_to_map).inverse();

The following is the most frequently used and I am sure it is correct.

T_(map->odom) = (T_(odom->base_link) * T_(map->base_link)^-1)^-1

T_(odom->base_link) is given by your odometry (either encoders or VO or anything else), T_(map->base_link) is calculated by the "localization" algorithm. Both are known.