Pose Graph Optimization From Scratch (pogo-fs) Part II: Front-End in Python

In this tutorial, we are going to develop the front end of the pose graph optimization framework in Python. This will serve as a log of process of creating an initial pose graph, getting transformations from ICP and, will lead to the back-end solver in the next tutorial.

Front End

First, we are going to use a ROS Bag recorded from the Scarab autonomously exploring at GRASP. The download of this ROS Bag can be found here.

On the figure to the right, we have an excerpt from "A Tutorial on Graph-Based SLAM" outlining what the construction of a pose-graph looks like. In summary there are two conditions to add nodes/constraints:

We perform the optimization whenever we detect a loop closure. With this information determined let's see what the implementation will look like.

For this part of the project, we will be strictly using Python. We will use a medley of libraries, but namely NetworkX for the graph manipulation aspect and Open3D for the Pointcloud matching.

Below is an example of the pointclouds in the ROS Bag.

Make sure to convert the bags to .pcds using "rosrun pcl_ros bag_to_ros <bag_name.bag> <output dir>"

Prior (Green) and Subsequent (Blue) Lidar Scans

Motion Model Transformation

The image above takes the motion model (or pose changes between the two scans) and applies the transformation to the green scan of the image on the right. As we can see, the lidar scans are much closer, but with error. This is where Iterative Closest Point (ICP) comes into play!

ICP Output

On the left is the output of the ICP Registration algorithm. We have taken the output transformation and applied it to the green scan. Herein lies the true beauty of scan matching, we can clearly see a lower error between the motion model and the ICP output.

Keep in mind, to keep our registration robust, we are actually using the motion model transformation as our initial guess to reduce estimation travel for our algorithm.