Added possibility to compute odometry error, show its pose path in rviz
and output it to file.

Refactored code and remove old ground truth estimation from gazebo link
states.