Skip to content

[fix] Fix initial pose for initial map

Jeanne Faure requested to merge fix/TransformOdom into feat/ROS2


When adding initial maps, the actual config of SetSlamInitialState() would firstly set the map, then eventually move the initial pose (odom). So the map would be moved with initial pose.

However, we would rather set a new initial position in a fixed map.


We decided to set a new initial pose by applying an offset to odom, and then initialize the initial map.

We could have set the map then use SetTworld instead of TransformOdom to just jump into another position that would become the new initial pose. But we figured out it would alter the SLAM process. For instance, if one wants to shift the initial pose kilometers away from odom, all the matrix would be filled with large values. As we apply transform with decimal factors, their effect would be reduced.

We also added a copy in TransformOdom to avoid modifying keypoints inplace.


Commands to reproduce the results

Changes in slam_config_indoor.yaml

initial_maps: "" --> initial_maps: "<path-prefix-to-pcd-map>"

e.g. : for a map of planes in /home/map_planes.pcd, I will enter initial_maps: "/home/map_"

Don't forget to change initial_pose right below.

Commands in shell

These are usual commands to launch slam in replay mode. Consider applying the changes above if you want to see the consequences of this MR.

ros2 bag play ~/<path-to-bag> --clock -d 1
ros2 launch lidar_slam outdoor:=false replay:=true


The only difference between these two captures was the order of initialization between initial pose and initial map, see the diff here.

See in blue the laser lines of the lidar right after launching the SLAM. This indicates the initial pose relative to the map.


Initial map and initial pose moved together because initial map was set before initial pose.


Initial pose was set (multiple attempts needed to fit it with the map) then initial map was logged.


Copying keypoints into a transformedKeypoints variable does not affect the performances at all (it results in the same frame processing time).

Edited by Jeanne Faure

Merge request reports