icp works now in class PlaneFinder.