Add ICP matcher implementation using libpointmatcher. Needs to have the same functionality as the existing ICP matcher using PCL.