Pushing all code from previous repo. Cleaned it up a little, John has to REALLY clean up the files in the src folder (i.e. odometry_example.cpp )