Add functions for Curvatura and Normal numerical estimations
Refactor for using Rawlog object for LRF and improvements to scan treatment
Refactor towards final object CGoptimizer