3D【23】ICP配准：point-to-plane

论文：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和$M_{opt}$是一个4*4的3D刚体变换矩阵。

3D刚体变换M是由旋转和平移构成的：

其中：

要最小化point-to-plane目标函数，只有6个参数：$\alpha,\beta,\gamma,t_x,t_y,t_z$，但是其中的$\alpha,\beta,\gamma$与非线性函数cos，sin有关。因此比较难求解。

线性近似

当角度$\theta \approx0$时有$sin\theta \approx \theta ,cos \theta \approx 1$，因此当$\alpha,\beta,\gamma \approx0$时，有：

这样一来，M可以近似为：

同时优化的目标函数为：

我们进一步的将目标函数展开：

这样，给定N个对应点，可以用矩阵的形式表示损失函数：

其中：

这样我们就可以用熟悉的SVD来求解线性方程组了，首先将A进行SVD分解：

$$A = U\Sigma U^T$$
那么我们可以得到：

其中

扫描二维码关注公众号，回复： 2239172 查看本文章

$$A^+=V\Sigma^+U^T$$
$\Sigma^+$是 $\Sigma$的伪逆。