The previous article explained how Karto's front end works.
This article adds the code of the back-end optimization part in slam_karto to lesson6, and I will take everyone to experience the function and function of back-end optimization.
1 Outdoor dataset
Since the scene of the data package used in the previous article is too small, the range of the room is only 30m, and the cumulative error generated by the front end is not obvious when walking back and forth, so I uploaded a new data set .
Since I didn't plan well before, I made several data sets throughout the tutorial. In fact, only one indoor and one outdoor are enough. ．．I self-examine.
1.1 Dataset Introduction
The name of the new dataset is lesson6-rslidar-outdoor-gps.bag .
This is in an outdoor environment, walking around a row of flower beds in the park, using Sagitar's 16-line radar, RTK-level GPS, lpms 6-axis IMU, and a four-wheel differential car as the experimental platform.
recorded
- imu data, topic name is imu
- Odometer data, topic name is odom_scout
- GPS latitude and longitude data, topic name is fix
- The heading data obtained by dual antennas, the topic name is heading
- 16-line radar point cloud data, topic name is rslidar_points
- Single-line radar data formed by pointcloud2Laserscan package, topic name is front_scan
- TF data: odom->footprint's tf
- tf_static数据: footprint->base_link->front_laser_link, imu_link的 tf_static
The details are as follows
$ rosbag info lesson6-rslidar-outdoor-gps.bag
path: lesson6-rslidar-outdoor-gps.bag
version: 2.0
duration: 3:33s (213s)
start: Dec 01 2020 15:44:09.49 (1606808649.49)
end: Dec 01 2020 15:47:43.48 (1606808863.48)
size: 2.1 GB
messages: 41623
compression: none [2064/2064 chunks]
types: geometry_msgs/QuaternionStamped [e57f1e547e0e1fd13504588ffc8334e2]
nav_msgs/Odometry [cd5e73d190d741a2f92e81eda573aca7]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48]
sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
tf2_msgs/TFMessage [94810edda583a504dfda3829e70d7eec]
topics: /fix 1070 msgs : sensor_msgs/NavSatFix
/front_scan 4127 msgs : sensor_msgs/LaserScan
/heading 214 msgs : geometry_msgs/QuaternionStamped
/imu 21400 msgs : sensor_msgs/Imu
/odom_scout 4279 msgs : nav_msgs/Odometry
/rslidar_points 4126 msgs : sensor_msgs/PointCloud2
/tf 6406 msgs : tf2_msgs/TFMessage (2 connections)
/tf_static 1 msg : tf2_msgs/TFMessage
This is a data packet with relatively complete data, and there is a loop in the walking track.
Since there are 16 lines of point cloud data, the data set is 2G, and it is 700M after being compressed into zip.
1.2 Dataset link
The link of the dataset is
https://pan.baidu.com/s/1UB7mjw3vhXlItsaqdNUw-g
and the password is slam
I put all the data sets I use in the Tencent document, and the address of the Tencent document is as follows:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2
2 Only use Karto's front-end for outdoor 2D mapping
2.1 Brief description of new code
This article adds back-end optimized code to the previous article, and the corresponding files are:
lesson6/include/lesson6/spa_solver.h
lesson6/src/spa_solver.cpp
At the same time, karto_slam.cc has also been changed, adding the function of configuring whether to enable back-end optimization and loopback detection according to parameters .
Changing the relevant content in the configuration file can realize whether to enable back-end optimization and whether to enable the loopback detection function.
Added launch and config files for running outdoor mapping:
lesson6/launch/karto_slam_outdoor.launch
lesson6/config/mapper_params_outdoor.yaml
2.2 Running the code
After downloading the data package, you need to change the bag_filename in launch to the directory name where you actually store the bag.
First, set the following two lines in the mapper_params_outdoor.yaml file to false to turn off the backend optimization and loopback detection functions.
# back-end
use_back_end: false # 是否启用后端优化
do_loop_closing: false # 是否启用回环检测
Run the program corresponding to this article through the following command
roslaunch lesson6 karto_slam_outdoor.launch
2.3 Analysis of results
After running, the following map will be generated. The effect of each run is different. The following figure is a screenshot of the worst running effect.
The length of the blank area on this map is about 100m. The width is about 27m, and the resolution is 0.05m.
During the actual operation, a red track will be displayed in Rviz. This track is the data of the odometer. The data of the odometer has an initial value and an initial direction, so the data of the odometer does not overlap with the pose of the robot.
Zooming in on the map constructed by this arc-shaped building, it can be seen that a serious overlay phenomenon has occurred.
3 Use Karto's front-end and back-end for outdoor 2D mapping
3.1 Introduction to Backend Optimization Module
Due to the cumulative error in the front-end scan matching, this error will gradually increase with time.
Moreover, scan matching is to match the current scan with the local map formed by the scan in the previous period, and does not use all the scan data from the start of mapping to the current moment.
Therefore, the big guys thought of a way to optimize all scan data together to reduce their cumulative error. This is back-end optimization .
In graph-optimized SLAM, the front-end and back-end are generally processed separately,
The front end needs to search and match in a certain range of maps to find the potential constraints of the current scan.
The purpose of back-end optimization is to find a configuration relationship between nodes to maximize the measurement probability of constraints between nodes. Generally speaking, it is to optimize all nodes and their constraints to find a set of solutions that are closest to the actual situation. The solution represents the new node pose and the new constraints between nodes.
The more accurate the post-optimized pose of the back-end is, the more accurate the local map generated by the front-end is, and the more accurate and faster the scan matching result is. At the same time, the cumulative error of the front-end can also be reduced.
Therefore, the accuracy and performance of the back-end optimization problem has a decisive impact on the entire mapping system.
For a more detailed introduction, see this article
K. Konolige, G. Grisetti, R. K¨ummerle, B. Limketkai, R. Vincent, Efficient Sparse Pose Adjustment for 2D Mapping, In Proc. of Int. Conf. on Intelligent Robots and Systems (IROS), Oct. 2010.
So why didn't you use backend optimization before?
Since all poses and map points are put together for optimization, the dimension of the formed matrix is very large, and it is impossible to find the Jacobian matrix.
In recent years, the improvement of computing speed and computing power has alleviated this problem, but it is still difficult to find the Jacobian matrix.
Until 2010, some great gods discovered that the matrix formed by all poses and map points is very sparse, which is an arrow-shaped structure.
Taking advantage of the sparsity of the matrix, this sparse large matrix can be reset into a more compact and smaller-dimensional matrix in some way. This makes the Jacobian matrix can be obtained.
3.2 Enable backend but not enable loopback detection
First, set the following two lines in the mapper_params_outdoor.yaml file as shown below to enable the backend optimization function, but not enable the loopback detection function.
# back-end
use_back_end: true # 是否启用后端优化
do_loop_closing: false # 是否启用回环检测
Start the program again through the above roslaunch command, and the following results will be produced after the operation is completed.
The red track in the figure above is the data of the odometer. Because there is an initial value, the value of the odometer does not coincide with that of the robot.
The blue traces are the nodes and edges optimized by the back-end optimization.
The node is the radar data at a certain moment, represented by a red ball.
An edge is a constraint between two nodes, that is, a coordinate transformation between two nodes, represented by a blue line,
Because the front-end scan matching has cumulative error, the back-end optimization can reduce the front-end cumulative error. These errors are not removed, but averaged to each node.
First, zoom in to see the effect of the constraints.
It can be seen that even if the back-end optimization function is enabled, after the map is finally built, the map of this arc-shaped building is still overlaid.
It can be seen that the back-end optimization module does not completely eliminate the accumulated error.
3.3 Enable the backend and enable loopback detection at the same time
3.3.1 Running Results
Let's take a look at the effect of enabling backend optimization and enabling loopback detection.
Set the following two lines in the mapper_params_outdoor.yaml file as shown below to enable the backend optimization function, but not enable the loopback detection function.
# back-end
use_back_end: true # 是否启用后端优化
do_loop_closing: true # 是否启用回环检测
Start the program again through the above roslaunch command, and the following results will be produced after the operation is completed.
This time the map does not overlap in the arc-shaped building!!!
great!
Zoom in to see the effect of the constraint. In the figure, there is a short blue line between the start point and the end point of the trajectory, which is the loop constraint.
The optimized graph is very neat. The final construction effect is like this
So why?
3.3.2 Loop detection
As mentioned above, back-end optimization cannot completely eliminate cumulative errors, so is there a way to eliminate cumulative errors?
The answer is loopback detection . Loop detection can greatly eliminate the cumulative error, but not completely.
Loop detection means that after the robot walks for a while, it returns to a place it has been to. The grid map corresponding to this place should be the same.
Therefore, some method can be used to detect whether the robot has returned to the scene it has been to before. If it is detected that the robot has returned to this scene, then the current pose should be close to the pose of the scene seen before, and the relative coordinate transformation between the two poses can be obtained according to the same scene.
This relative coordinate transformation can be added as a constraint, that is, an edge , to the back-end optimization. This constraint is called a loopback constraint .
Since the scene seen by these two nodes is the same real scene, the confidence of this edge is very high and the weight is very large, so the cumulative error can be greatly eliminated.
Therefore, the purpose of loopback detection is to find the constraint with a high weight, and then put this constraint into the graph structure and optimize it together to achieve the purpose of eliminating the cumulative error.
4 Summary and Next
The backend optimization module of slam_karto has been put in the code,
This article first conducts a comparative test of the back-end optimization and loopback detection modules, and analyzes the effect of adding backend optimization and loopback detection functions from the perspective of map quality.
At the same time, it also briefly introduces the concepts of back-end optimization and loopback detection.
The next article first briefly analyzes the implementation of slam_karto's loopback detection and back-end optimization code.
The next article will use G2O to realize the back-end optimization function of karto.
Due to my limited level, it is inevitable that there will be some errors in the code. If you find errors or bad places, please contact me and I will correct them.
The article will be updated synchronously on the official account: SLAM from scratch , everyone is welcome to pay attention, you can add my WeChat in the official account, enter the laser SLAM communication group , and exchange SLAM technology together.
If you have any suggestions for the articles I wrote, or want to see how the functions are implemented, please reply directly in the official account, I can receive them, and will seriously consider your suggestions.
