Absurd treatment to odometry bug, explicitly choose if we fast walk, decrease reconstruct threshold
Add debug loc functionality, cleanup an old function