- 记录PCL中涉及到点云配准的代码
[TOC]
PCL中ICP算法实现的核心代码注释
点云配准
给定两个点云\(\mathbf{X}=\{x_1,...,x_n\},\mathbf{Y}=\{y_1,...,y_n\}\)，点云配准是为了求解两个点云之间的相对旋转\(\mathbf{R}\)和相对平移\(\mathbf{t}\)，他们满足如下的优化方程： \[ E(\mathbf{R},\mathbf{t})=\frac{1}{N_p}\Sigma_{i=1}^{N_p}||x_i-\mathbf{R}q_i-\mathbf{t}||^2 \] 如果知道两个点云中的同名对一点关系，那么就可以直接计算得到上述非线性函数的最优解，因此ICP算法中一个核心的突破点就是如何建立正确的同名点、如何从大量的包含错误对应的同名点中删选出正确的关系。
作为基础库，ICP的代码实现中最核心的一个过程是computeTransformation函数。
对应点类
求解位姿时需要根据同名对应点来计算，因此PCL中设计了一个同名对应类来保存这种数据。
一个有争议的地方在于：不少学者认为点云中不存在同名点的说法。个人认为，在当前激光雷达获取的点云密度非常高的情况下，同一区域重复扫描的点云之间可以认为是存在同名点的，只是同名点可以是一定距离阈值内的对应。比如2cm等。这个概念和图像领域中的匹配也是一致的，像素分辨率低的时候还不是一样可以进行配准对齐。
变换矩阵求解
基于SVD分解的变换矩阵求解
基于LM的变换矩阵求解
基于LLS的变换矩阵求解
基于LM优化点到面距离变换矩阵求解
判断是否收敛的标准
- MSE值
- 变换矩阵之间的差异
- 迭代次数
