Added possibility to free the scale parameter during ICP registration (SICP)
Merge remote-tracking branch 'origin/SICP' + simplification