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 1. Confirmation Section

Course 2. Up to 1st Intersection

Course 3. Kenkyu Gakuen Station

Course 4. Kenkyu Gakuen Station Park

Course 5. Up to 2nd Intersection

Course 6. 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

##### Cartographer Documentation

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

### 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.

### 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.

### 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!

### Step 2 : Update topic name and system configuration described in backpack2d.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....

Adjust offset so that it connects properly.

### 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.

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

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

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

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

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

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....

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

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

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

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

##### 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

##### 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.)

##### 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

##### 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

##### 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