ICP/optimization refactoring

Important refactoring of the SLAM in order to have a more C++ like code.

• factorization of recurrent parts
• introduction of new structures to simplify arguments passing, parameters settings and code re-use

TODO :

• Build Ceres residuals or problem in ICP functions
• Perform ICP and optimization in separate class
• Declare Undistortion and Keypoints enums in a separate file in a given namespace to avoid including headers just to have some enums definitions.
• Introduce Optimization result structure
• Factorize Slam member attributes (ICP/optim parameters or keypoints specialized attributes)

@nick.laurenson @julia.sanchez PTAL