Feature Preserving Non-Rigid Iterative Weighted Closest Point and Semi-Curvature Registration