icp: develop matching tools

Based on the Censi library, do wrappers functions to work with laser_scan object type.