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
First commimt for josh's branch