Add vtk-based icp registration