Hierarchical Pose Graph Optimization

README.md

1. Experiment performed on Hierarchical Pose Graph Optimization.
2. In our formulation topological nodes come at higher level of hierarchy tree followed by dense pose graph nodes.
3. We are currently experimenting that whether just optimizing the pose graph of higher topological nodes is sufficient to recover the trajectory close to the groundtruth.
4. The aim is to exploit topological information to get faster pose graph optimization time and get good enough map for robot navigation from one topology to another topology.
5. Here we are assuming that our frontend would be able to detect loop closures at the intersection of topological nodes.

RESULTS

1. Synthetic environment in Gazebo.
   

2. Skematic of map and trajectory taken by robot.
   

3. Ground Truth trajectory taken by robot.
   

4. Optimization on whole map:

4.1. Noisy trajectory generated by introducing perturbation in odometry.

4.2. Optimized trajectory using keypoints poses only. Keypoints poses are located at the starting and ending points of a topological structure.

5. Optimization on keypoints poses only:

5.1. Noisy keypoints locations extracted from noisy dense trajectory.

5.2. Optimized keypoints. Keypoints are optimized using same transformation as given for optimizing dense noisy trajectory.

6. Optimizing each submap based on the optimized keypoints. Optimization of three submaps is shown below:

6.1.a. Submap before optimization:

6.1.b. Submap after optimization:

6.2.a. Submap before optimization:

6.2.b. Submap after optimization:

6.3.a. Submap before optimization:

6.3.b. Submap after optimization:

7. Final result after concatanation of all 25 submaps:
   

8. Benchmarking the time taken for optimizing on full pose graph vs (first optimizing on the keypoints and then optimizing each subgraph individualy based on the optimized keypoints):
   a. Full Pose Graph: 0.28 seconds
   b. Hierarchial subgraphs optimization: 1.15 seconds