点云精细配准最传统的方法是ICP(Iterative Closest Point)
Point-to-Point ICP
现在假设有
个目标点云
个源点云
那么点云配准就是在求解最优的使得满足如下的式子
如果认为第个点拥有权重,那么上面的式子将会变成
令
假设已知的情况下,对求导可以得到
定义,那么上式可以表达成
这是固定下求解的方法,如果令,那么有
考虑到,因此有
因此
对做 SVD 分解后得到的结果 ,则上式变为
考虑到为正交矩阵,该矩阵行(列)向量满足,因此该矩阵内每个元素(包括对角线元素)均有,当 时,上式取到最大值,即
最后对Point-to-Point ICP配准进行总结:
- 从目标点云和源点云中找到距离小于的点对
- 根据权重,计算等效中心
- 将每个点去中心化
- 对协方差矩阵进行SVD分解,其中协方差矩阵为
- 求解得到旋转矩阵
- 求解得到平移向量
注:在三维情况下,需要求解旋转矩阵,如前面所示
在某些情况下该问题可以退化为BEV二维平面上的问题,忽略高度,则上式变成
其中
上式的最优解在的时候得到,此时有
Point-to-Plane ICP
点面配准在点点配准的基础上,需要额外知道个目标点云 所在表面的单位法向量 ,而优化方程也变成了
如果已经完成粗配准阶段,可以近似认为 ,如果用欧拉角来表示旋转矩阵则
原优化方程可以写成
该问题变成典型的 最小值问题,在超定情况下该问题的解为
其中 ,为伪逆
最后对Point-to-Plane ICP配准进行总结:
- 从目标点云和源点云中找到距离小于的点对
- 根据权重,和目标点云的单位法向量,计算
- 对矩阵 进行 SVD 分解
- 得到结果 ,从中分离出,并且进一步求解出