Note: This is a live article and as I get time I will update it…
In this post, we are going to understand the pose-graph SLAM approach with ROS where we can run the robot around some environment, gather the data, solve a non-linear optimization and generate a map which can then be used by the robot for localization. There are 4 major components to this problem; (i) how to generate the pose to pose odometry ?(relatively easy these days in ROS), (ii) how to generate loop closure constraints between various poses? (iii) how to solve the graph optimization problem? (iv) how to take an optimized graph and construct a usable map? Finally once we have a map, we should technically be able to use it to localize.
Before we dive into the details, here a few resources that deal with SLAM in ROS:
- This paper provides a comparison of SLAM techniques in ROS.
- In this ros answers question, I raised a discussion about doing pose-graph SLAM with ROS.
- open_karto is an open source library that is used by nav2d and slam_karto to do pose-graph SLAM in ROS. It provides loop closure and other capabilities required for autonomous mapping and navigation. It does not provide a backend.
- I have developed a ROS package called slam_karto_g2o for pose-graph SLAM using g2o as backend and open_karto as front-end.
- I have developed a ROS package called slam_karto_gtsam for pose-graph SLAM using GTSAM as backend and open_karto as front-end.
- I have developed a package called slam_karto_sesync for pose-graph SLAM using SE-Sync as backend and open karto as front-end. SE-Sync is an awesome work on globally optimal SLAM solutions from relative measurements.
- Luca Carlone’s website has an excellent description of the file formats that are used by pose-graph SLAM solvers.
- A Udacity video by Sebastian Thrun.
- A tutorial style video by Cyril Stachniss.
- g2o, GTSAM and LAGO are good options for the backend. LAGO solver by Luca Carlone @ MIT is quite robust when the odometry is bad as it separates orientation and position estimation.
- Google Cartographer is another option for a pose-graph SLAM solver, I have never tested or tried it before.
- amcl is a popular particle-filter based approach to robot localization with LiDAR.
Here are some key terms:
- Pose-graph: a network of nodes and edges where the nodes are robot poses and edges are constraints between poses.
- Loop closure: a constraint between the a recent robot pose and a past pose when the robot revisits a previously visited location. Loop closure is highly sensitive to the current estimate of where the robot is. If your current estimate is bad you may not realize you are visiting a previously visited location! There are global loop closure approaches which try to match what the robot sees to everything seen in the past in order to find a closure, such approaches may be computationally expensive.
- Back-end: a non-linear optimizer that solves a given graph structure and computes the history of robot poses.
- Front-end: detects when a loop closure occurs. This is usually the perception side of SLAM.
Now lets look at each piece of the puzzle that is pose-graph SLAM:
- Pose to pose odometry: First, I tried using laser_scan_matcher to compute the transformation between poses (or keyframes) but soon realized that laser_scan_matcher (which uses CSM – Canonical Scan Matcher) doesn’t work very well in cluttered environments. We have switched to using the scan matching from Hector SLAM which appeared to be doing better than laser scan matcher. But now I have switched to using open karto. It come with a very robust scan matcher.
- Loop closure detection (a.k.a Front-end): The front-end relies heavily on the current estimate of where the robot. This is quite sensitive to the odometry. If the odometry is bad then the front-end may never detect a loop closure. This is one of the main reasons for SLAM failure. FLIRT features are one good way to detect loop closure, however currently I rely on the open karto implementation. You can download the FLIRT library from openslam.
Let’s start talking about open karto as that seems to be the most user friendly library for pose-graph SLAM to my knowledge.
open_karto is an open source BSD licensed version of Karto SLAM SDK that is marketed by Karto Robotics from SRI International. Open karto is quite popularly used in ROS, I have seen nav2d and slam_karto both use this package. Open karto does does not inherently provide a bundle adjustment or optimization solver. Rather you are supposed to provide an external solver. A commonly used library to solve the pose-graph with open karto is sparse bundle adjustment (sba package). Now lets elaborate more on what we know so far about these packages.
open_karto
Open karto appears to handle scan matching, loop closure detection and maintaining the underlying pose-graph structure for the SLAM problem. It requires an external solver to solve the non-linear optimization or bundle adjustment. In Mapper.cpp, Mapper class has a Process function which processes a new incoming laser scan. If scan matching is set to true, it will do scan matching to compute local corrections, if loop closure is set to true it will also check for loop closure. In OpenMapper.cpp, MapperGraph class has a TryCloseLoop function that uses the internal MatchScan function to find transforms for loop closure. To add our bundle adjustment solver, all we need to do is to write a wrapper class around GTSAM, g2o etc. which provides the same interface as the SpaSolver class in slam_karto.
slam_karto
The slam karto package basically uses open karto to create and maintain the pose-graph and sba package to solve the pose-graph SLAM problem. The sba package provides for a way to add a vertex and a constraint to the pose-graph. The slam_karto.cpp file handles incoming laser scans using laserCallback function, this function internally calls addScan. In addScan, first the odometry pose is retrieved using getOdomPose function, getOdomPose looks for the transform from odom frame to base_link by default. If no odometry pose transform is found, the addScan function returns false. If an odometry transform is found, addScan proceeds and converts the ros laser scan message into a karto localized range scan and places it at the odometric pose. The range scan is then processed by the Karto Mapper class, and the corrected pose of the newly added vertex is retrieved, these corrections usually happen when loop closure is detected. The SpaSolver class in spa_solver.h is basically a wrapper that connects karto’s mapper to sba library.
sparse bundle adjustment (sba)
The sba package is based on work by Kurt Konolige (pdf). The sba package provides in spa2d.h a class called SysSPA2d. This class allows adding nodes and constraints for the bundle adjustment problem. It uses Levenberg-Marquadt approach to non-linear optimization and uses CHOLMOD internally to solve the linear sub-problems. Ideally, it should not be difficult to use any other solver like g2o or GTSAM instead of sba to do slam with open karto.