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:
The robot has moved more than .5 meters or rotates more than .5 radians
We detect a loop closure
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.