Refactored for easier switchign between implementations.
Refactored to make it easier to change between implementations.
"Added Ground Truth pose graph for comparison."
Added plot of posegraph error