Dataset 2 g2o optimization using manhattan constraints

README.md

1. Following are the results of dataset 2 optimization just using manhattan constraints.

1. Dense ground truth trajectory:
   

2. Dense noisy trajectory:
   

3. Manhattan sparse trajectory:
   

4. Transformation of all the nodes with respect to the first node in above manhattan trajectory is caculated and given as loop closing constraints. It can be seen as overlying 3rd image constraints over 2nd image dense noisy trajectory.
   Before g2o optimiztion:
   

5. After g2o optmization:
   Note that left rackspaces are shifted upwards, this is due to drift in manhattan constraints itself which were given as constraints. So there is scope of inprovement using ICP or incremental manhattan formulation.
   

6. Comparison of noisy, optimized and ground truth trajectory:
   

7. As all the constraints are consistenly given from manhattan graph only, so there is no hardcoding involved. And as the manhattan is close to ground truth so we can always extract relative transfomtion between 2 nodes when MLP say two nodes are same (instead of del_x=0.2, del_y=0.2)