Adding Odometry feature and opencv based visualization