ICP算法

代码说明：

Point to Point就近点搜索法是ICP算法中最经典的一种方法。如图1a所示, Point to Point法根据源曲面上的一个点p，在目标曲面上找出对应于p点距离最近的q点。在这个方法中通常运用kd-tree的方法实现就近点搜索。如图1b所示，pi是源曲面点云数据中的一个点，Vi是生成目标曲面点云数据中距Pi最近的点。根据Vi点搜索出在曲面上与Vi点相邻的点构成的三角形格网，计算pi点投影到每个三角形平面上的投影点qi的坐标。对于每个三角形来说，当投影点qi位于三角形内部，则距离最近点是搜索的最近点，当投影点qi位于三角形外部，搜索的就近点应位于三角形的两条边界上，Vi是该三角形到pi点的就近距离点。将每个三角形确定的就近距离点进行比较可获得一个最近点。