Feature preserving non-rigid iterative weighted closest point and semi-curvature registration