论文:Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration
这篇论文不是原始的point-to-plane ICP,只是对线性最小二乘法优化point-to-plane 损失函数的推导。
point-to-plane会比point-to-point收敛的更快,但是对point-to-plane的优化是一个非线性问题,速度比较慢。不过幸运的是,我们可以用线性优化来近似。
目标函数
point-to-plane的损失函数是最小化源顶点到目标顶点所在的面的距离平方:
其中,s为源顶点,d为目标顶点,n为目标顶点的法向量,M和
是一个4*4的3D刚体变换矩阵。
3D刚体变换M是由旋转和平移构成的:
其中:
要最小化point-to-plane目标函数,只有6个参数: ,但是其中的 与非线性函数cos,sin有关。因此比较难求解。
线性近似
当角度
时有
,因此当
时,有:
这样一来,M可以近似为:
同时优化的目标函数为:
我们进一步的将目标函数展开:
这样,给定N个对应点,可以用矩阵的形式表示损失函数:
其中:
这样我们就可以用熟悉的SVD来求解线性方程组了,首先将A进行SVD分解:
那么我们可以得到:
其中
是 的伪逆。