moving ICP from pointcloud to sep class

this will make it easier to unit test since it is horrible right now