一、算法原理
ICP算法的核心目标是通过迭代计算，寻找最优的刚性变换矩阵，使源点云与目标点云中对应点之间的误差（通常是欧几里得距离）最小化。该算法通过不断更新变换矩阵，逐步减小两个点云间的配准误差，直至满足预设的收敛条件为止。收敛判断在ICP应用中尤为关键：过早终止会影响配准精度，而过度迭代则会造成计算资源浪费。因此，设计合理的收敛条件对保证算法效率和准确性至关重要。PCL库中的ICP算法提供了四种迭代终止判断条件：
- 最大迭代次数
迭代次数达到了预设的最大值 (max_iterations_)，确保算法在一定的时间内停止，避免在不收敛的情况下无限迭代。
- 连续两次迭代变换差异（旋转和位移）小于阈值
当源点云与目标点云之间的旋转和平移变换趋于微小时，表明点云已基本对齐，此时可终止ICP算法。具体判断标准为：计算当前变换矩阵旋转部分对角线元素与上一轮旋转部分对角线元素的余弦角度差(cos_angle)，若该值低于预设旋转阈值，则判定旋转差异足够小；同时计算当前平移向量与上一轮平移向量的平方差(translation_sqr)，若该值小于预设平移阈值，则认为平移差异符合要求，若旋转和平移差异均小于设定阈值，且达到设定的最大次数（max_iterations_similar_transforms_
