Working version of ICP for 2 datasets.