Fix undistortion and improve ego-motion initialization

• Use timestamps to extrapolate current ego-motion from previous motion.
• As this extrapolation is generally precise enough, set ego-motion registration as optional.
• Fix undistortion : 2 modes are available, approximated or optimized.
• Major acceleration of Ceres cost functions (~40%).
• Clean MotionModel.h/.cxx by removing old unused and deprecated stuff.