目录
- 一、算法原理
- 二、代码实现
- 1、原始版本
- 2、2026新版
- 三、结果展示
- 以下是2026全新版
- 一、概述
- 1.1 原理
- 1.2 实现步骤
- 1.3应用场景
- 二、关键函数
- 2.1 头文件
- 2.2 计算法线
- 2.3 读取点云数据
- 2.4 计算源点云和目标点云的法线
- 2.5 设置加权的点到面ICP变换估计
- 2.6 设置ICP参数
- 2.7 执行优化算法
- 2.8 可视化
- 三、完整代码
- 四、结果展示
博客长期更新，本文最近一次更新时间为：2026年5月4日。
一、算法原理
非线性加权最小二乘法在点到面ICP算法通常用于计算机视觉、机器人导航等领域的三维模型配准，目的是找到两个形状相似的模型之间的最佳对应关系。在传统的ICP中，最简单的形式就是线性权重，但它往往难以处理噪声和不匹配的情况。
非线性加权最小二乘法引入了一个权重函数，该函数会考虑每个匹配点的距离及其邻域内的信息。这个函数可以是对距离的指数衰减、高斯分布或其他平滑函数。优化的目标函数不再是简单的均方误差（MSE），而是变成了加权后的误差：
[ min transform ∑ i w i ( x i − transform ( y i ) ) 2 ] [ \min_{\text{transform}} \sum_{i} w_i(\mathbf{x}_i - \text{transform}(\mathbf{y}_i))^2 ]
