[fix][ROS1] Fix initial pose for initial map

Same MR done for ROS2 see !338 (merged).

Goal

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.

Changes

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.

Results

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.

rosbag play <path-to-bag> --clock -d 1

roslaunch lidar_slam slam_velodyne.launch outdoor:=false replay:=true

Results

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.

Performances

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