PointCloudLib 点云配准LM-ICP算法实现 C++版本

0.实现效果

上图中，紫色点云是源点云，蓝色点云是目标点云，绿色点云是源点云经过齐次变换矩阵后的点云，可以看出和目标点云重合度非常高。

1.算法原理

LM-ICP（Levenberg-Marquardt Iterative Closest Point）算法是一种在点云配准领域广泛使用的技术，它是ICP（Iterative Closest Point）算法的一种改进版本。ICP算法是一种通过迭代寻找两个点云之间最优变换矩阵的方法，用于将两个或多个点云进行对齐和匹配。而LM-ICP算法则通过引入Levenberg-Marquardt优化算法来提高ICP算法的精度和收敛性。

Levenberg-Marquardt优化算法（也称为列文伯格-马夸尔特法）是一种用于求解非线性最小二乘问题的优化算法。它结合了梯度下降法和高斯-牛顿法的优点，能够在迭代过程中自适应地调整步长，从而更快地收敛到最优解。

该算法的原理主要是通过迭代的方式来寻找非线性最小二乘问题的最优解。在每一次迭代中，算法会计算函数在当前参数值下的一阶导数和二阶导数（或近似值），然后根据这些信息来更新参数的值。具体来说，Levenberg-Marquardt算法使用了一个称为LM步长的修正策略，该策略可以通过牛顿方法在初始迭代步骤中进行矢量修正。

此外，Levenberg-Marquardt算法还引入了一个控制参数λ，用于调整梯度下降法和高斯-牛顿法之间的权重比例。在算法的早期阶段，λ通常