Lidar SLAM

SLAM is not the easiest thing to wrap your head around because of the probabilistic math around it. But it’s not so tough when you boil it down to its components.

Odometry

For systems using lidar, I need a lidar odometry system to track where the vehicle has been in order to use those poses to build a map. I used KISS-ICP, which works great and is open source. The problem is then to correct the drift that comes with odometry.

Correcting Drift with Pose Graph Optimization

To correct the drift, we can start looking at pose graph optimization (PGO). PGO can be represented as a optimization problem to satisfy edge constraints. The vertices in the graph are the poses of where the robot has been, and the edges are the relative poses between these vertices. At first, these relative poses are just odometry estimations. But when we stumble upon a vertex that we’ve previously been before, we can impose a relative pose between the current vertex and vertex we happened to visit. This is called a loop closure, because we looped around to a vertex that has been visited before and we closed it by imposing an edge constraint between the current vertex and the visited vertex.

I recommend this video for intuition.

Then what PGO does is satisfy the edge constraints by formulating it as a non-linear least squares problem that can be solved by an algorithm like Levenberg-Marquardt. I just like to abstract it away as a fancy minimization problem and not worry too much about it. In this project, I used g2o graph optimization library because it is the easiest to use.

This is pretty much the core of modern SLAM. It’s pretty easy when you think of it in the way I described above. What isn’t easy is how to find these loop closures.

Loop Closure

Loop closure is the hardest part of the SLAM pipeline. As in, it’s still an open problem because it is pattern recognition, since it is just figuring out where you are by pattern matching current sensor data with previous sensor data. For this project, I used MapClosures for lidar-based loop closure because it is effective and also because the software is from the same people that made KISS-ICP.

Summary

I present the completed offline SLAM solution in this repository. You can view the documentation of the same software I integrated into Nova’s mapping stack.

Full video demonstrating the SLAM pipeline on the same dataset: