Mapping with Google Cartographer ~ Parameter Tuning ~

I would like to summarize the skills I learned from this year's Tsukuba Challenge. This blog post is about mapping with Google Cartographer.

0. Mapping Target

The total length of Tsukuba Challenge 2019 is 2.X km. Since it is quite difficult to do SLAM for all the course at once, I divided it into the following 6.

Course of Tsukuba Challenge 2019

Course 1. Confirmation Section

Confirmation Section

Course 2. Up to 1st Intersection

Up to 1st Intersection

Course 3. Kenkyu Gakuen Station

Kenkyu Gakuen Station

Course 4. Kenkyu Gakuen Station Park

Kenkyu Gakuen Station Park

Course 5. Up to 2nd Intersection

Up to 2nd Intersection

Course 6. From 1st Intersection to Goal

From 1st Intersection to Goal

I thought that I should make a map with gmapping as usual, but I decided to use Cartographer because Course 4 (Kenkyu Gakuen Station Park) was quite long and I couldn't create a stable map. It is quite difficult to tune parameters for Cartographer, and it took a long time for me. So I decided to create this entry as "Tuning manual with actual examples". I also wanted to write down theory that is used inside it, but this would take so much time, so,,, I will give it a shot when I have time.

1. References & Useful Blog Posts

How to tune parameters are roughly described in the following manual that are documented by the developer. I also referenced the following 2 blog posts.

Cartographer ROS Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer-ros/latest/google-cartographer-ros.pdf

Cartographer Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf

Usefule Blog Entries.

ssk0109.hatenablog.com

ssk0109.hatenablog.com

2. Preconditions and System Configuration.

The maps shown in this entry are created with the following preconditions.

One 2d Lidar, Odometry (x, y, yaw), No IMU.

Robot used for mapping. From front.

Robot used for mapping. From side.

3. Parameter files subject to update.

The frollowing 4 files are necessary to be modified for 2d slam cases. (My case.)

backpack_2d.urdf

Urdf file that describes sensor configuration. Only horizontal_lidar is used for this entry.

backpack_2d.lua

Lua file that describes system configuration. (Topic, Frame etc ...)

trajectory_builder2d.lua

Lua file that describes parameters for Local SLAM.

pose_graph.lua

Lua file that describes parameters for Glocal SLAM (Loop Closure).

4. Procedure

Change the settings and parameters according to the following procesure.

1. Update sendor mounting position information described in backpack_2d.urdf.
2. Update topic name and system configuration described in backpack2d.lua.
3. Tune parameters so that odometry is correct to some extent.
4. Tune local SLAM parameters with already tuned odometry. (trajectory_builder2d.lua)
5. Tune global SLAM parameters so that loop closes correctly at the target location.

Step 1 : Update sendor mounting position information described in backpack_2d.urdf.

The setting here is very important! If you forget this, it will not work!

Set position information of horizontal scanner

Step 2 : Update topic name and system configuration described in backpack2d.lua

Difference in backpack_2d.lua

Regarding the modified parameters,,,,
published_frame：Since odometry is used this time, specify odometry frame as "odom". The map frame generated by cartographer will have this frame as the child.
provide_odom_frame：If true, transform between map frame and localized position by local SLAM is supplied as odom_frame. In my case, since another node supplied tf of odom_frame, I set it to false.
use_odometry：Whether to use odometry for map generation. This time, set it to true.
num_laser_scans：Number of laser scan sensors. This time, set it to one.
num_multi_echo_laser_scans：Number of echo laser scan sensor. This time, set it to zero.

Step 3 : Tune parameters so that odometry is correct to some extent.

This step may be a bit tricky, but I wanted to quickly create a correct map, so I decided to tune the yaw rate offset in advance so that the odometry trajectory gets roughly connected.

Before offset adjustment

For example, odometry not connected at all like this....

Before offset adjustment

Adjust offset so that it connects properly.

After offset adjustment

Step 4 : Tune local SLAM parameters with already tuned odometry.

Adjustment of Local SLAM. At first, turning off global SLAM by the following parameter, tune and improve local SLAM performance as described in "Cartographer parameter tuning manual".

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

Default settings are used except for the above modification.

Performance of Local SLAM Only (Default Parameters)

As you can see from the above figure, the default parameter causes the yaw angle error to accumulate, and the map does not connect even if you return to the original point after going around. So, it seems better to believe in odometry a little more than the result of Local Scan Matcher for yaw angle. Let's change the parameters as much as possible and see the result.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

Setting that strongly believes in the odometry yaw angle.

As for the rotation, the map became match better. Since I only increased the weight of odometry for rotation and not for translation, there is still a translation offset between encoder based odometry (red) and slam base odometry (purple).

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

Setting that strongly believes in the odometry yaw angle and translation

A map made is completely based on encode odometry! Even though the map seems decent, if you take a look at it closely there are some errors, since odometry contains errors both in locally and globally. For example, even though one plane is taken with LIDAR, it appears as two lines on the MAP. From here, I think that fine tuning is probably necessary. So, let's trust the scan matching result a little more.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

Put a little more weight on scan matching

Well. It doesn't change much. . . . So, let's go to next step,,, adjustment of Global SLAM.

Step 5 : Tune global SLAM parameters so that loop closes correctly at the target location.

Enable the global SLAM that was disabled in step 4.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

Map with Global SLAM turned on

I would like a loop to get closed when it re-visited the same place, but it did not happen with the default parameters. So let's modify parameters for loop closure to happen.

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

Result with loop closure

The loop closed for the part of figure 8 (left side of the figure). However, the loop is not yet closed for the last part of the run. This is probably because the way I collect the data was not good. Since I stopped recording data immediately after I returned to the same place after going around the circle, there were not enough overlap around the revisited places. To complement this, let's modify parameter to make loop closure occur more easily and frequently, and see if this achieve correct loop closure.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

Mmm.... No drastic change....

Frequent loop closure setting

Cartographer accumulates the likelihood obtained from the scan results. This likelihood is accumulated for a small chunk of ​​the map, called "Submap", and registers the submap as a global optimization target when the number of updates exceeds a certain number. This "certain number" parameter is as follows, and let's lower the threshold to achieve earlier registration of submap for global optimization.

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Wrong loop closure

Well,,, wrong loop closure detected... Probably I should believe encoder based odometry a little more.
In Cartographer's pose optimization, as with Local SLAM, you can set weights for the results of encoder based odometry and scan matching based odometry. Let's trust encoder based odometry a little more.

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

Finished!

After several parameter adjustment, the generated map seems to be OK. The encoder based odometry (red line) does not completely close when it goes around, but the odometry line generated from SLAM (purple) is connected smoothly where the loop closes.

Result

Result of all maps shown below! After all, it is quite difficult to achieve loop closure. If you increase weight on the encoder based odometry, the map becomes decent. However it is not perfect enough to achieve loop closure, and conversely, if you increase the contribution of local and global SLAM, a false match occurs in a place that is not a loop. . . I just repeated the seesaw game.

Course 1. Conrimation Section

Confirmation Section

Map of Course 1

Course 2. Up to 1st Intersection (This interval is difficult and the loop does not get closed completely.)

Up to 1st Intersection

Map of Course 2

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Course 3. Kenkyu Gakuen Station

Kenkyu Gakuen Station

Map of Course 3

Modified parameter from Cartographer's default

POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Course 4. Kenkyu Gakuen Station Park (This interval is difficult and the loop does not get closed completely.)

Kenkyu Gakuen Station Park

Map of Course 4

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8

Course 5. Up to 2nd Intersection

Up to 2nd Intersection

Map of Course 5

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 40

Course 6. From 1st Intersection to Goal

From 1st Intersection to Goal

Map of Course 6

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8