PointCloudLib Trimmed ICP精确配准实现 C++版本

测试效果

算法简介

PCL（Point Cloud Library）中的trimmed ICP（Iterative Closest Point，迭代最近点）算法是一种用于点云配准的改进方法。传统的ICP算法在点云配准时，容易受到离群点或噪声的干扰，导致配准结果不准确。trimmed ICP算法通过剔除与估计模型之间距离较大的点（即离群点），来减少这些点对配准结果的影响，从而提高配准的精确性。

一、算法原理

trimmed ICP算法的核心思想在于利用Least Trimmed Squares（LTS，修剪最小二乘）方法求解ICP问题。在配准过程中，对于待配准点云中的每一个点，算法会寻找其在基准点云中的最近邻点，并计算它们之间的欧式距离（即individual distance）。然后，算法会按照这些距离进行排序，并剔除掉距离较大的点（即残差较大的匹配对），因为这些点对求解旋转和平移矩阵的影响较大。最后，算法利用剩余的点对求解最优的旋转和平移矩阵，以实现点云的精确配准。

二、PCL中的实现

在PCL中，trimmed ICP算法的具体实现在pcl/re