Fixed error that came from applying odometry pose twice