2020-11-15

摘要

原始的ICP point-to-plane算法出自《Object Modeling by Registration of Multiple Range Images》，该算法收敛速度要快于传统ICP point-to-point算法。不过在根据对应点对计算位姿变换矩阵时，由于point-to-plane形式的目标函数没有封闭解，因此只能采用非线性优化计算，这个优化过程通常比较慢。而本文在相对位姿变换比较小时，把非线性优化问题近似为线性最小二乘问题，可以进行高效求解。

2. point-to-plane ICP算法

已知源曲面和目标曲面，ICP每次迭代中首先计算源曲面和目标曲面之间的对应性。例如，在对于源曲面上的没一点，将会选择目标曲面上的最近点作为对应性。ICP每次迭代的结果是一个3D刚体变换矩阵，该矩阵使得变换后的源曲面和目标曲面之间的对应点在指定的误差度量意义下最小。

传统的point-to-point ICP算法采用的误差度量是对应点对之间的欧式距离。而point-to-plane采用的是源曲面点到目标曲面点处的切平面之间的距离作为误差度量。

具体来说，设$s_i=(s_{ix},s_{iy},s_{iz}{)}^T$是源曲面上的一点，$d_i=(d_{ix},d_{iy},d_{iz}{)}^T$是$s_i$在目标曲面上的对应点，$n_i=(n_{ix},n_{iy},n_{iz}{)}^T$是$d_i$处的法线向量，那么point-to-plane ICP算法中每次迭代过程计算对应性位姿矩阵的优化目标为：
$$M_{opt}={min}_M\sum _i((M\cdot s_i-d_i)\cdot n_i{)}^2$$
式中，$M,M_{opt}$是4x4的刚体变换矩阵。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PKnjMZEX-1605417765155)(resources/db688f116d5e4d6ea03bbad41ffc867e.png)]

如上图所示，point-to-plane ICP最小化的目标函数是$l_1+l_2+l_3$。

3. 线性近似

刚体变换矩阵

3D刚体变换矩阵$M$可以由一个旋转矩阵和一个平移矩阵组成：
$$M=T(t_x,t_y,t_z)\cdot R(\alpha ,\beta ,\gamma )$$
式中：
$$T(t_x,t_y,t_z)=\left[\begin{array}{cccc}1& 0& 0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1\end{array}\right]$$

$$R(\alpha ,\beta ,\gamma )=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& 0\\ r_{21}& r_{22}& r_{23}& 0\\ r_{31}& r_{32}& r_{33}& 0\\ 0& 0& 0& 1\end{array}\right]$$

式中：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-z8KS9JSQ-1605417765158)(resources/9c2cfc42a77c425c9a1bed17523e603d.png)]

$R_x(\alpha ),R_y(\beta ),R_z(\gamma )$分别绕着$x,y,z$轴的旋转。

将上式代入point-to-plane ICP优化的目标函数中，可以得到一个关于$\alpha ,\beta ,\gamma ,t_x,t_y,t_z$六个参数的方程组。本质上，这是一个最小二乘问题。但是由于$\alpha ,\beta ,\gamma$是非线性三角函数的参数，因此线性最小二乘无法对该系统进行求解。接下来将介绍如何把上面的非线性优化问题近似为线性最小二乘问题。

理论基础：

当$\theta \approx 0$，可以使用如下近似等式：
$$sin\theta \approx \theta ,cos\theta \approx 1$$

根据上面的理论基础，可以得到：
$$R(\alpha ,\beta ,\gamma )=\left[\begin{array}{cccc}1& \alpha \beta -\gamma & \alpha \gamma +\beta & 0\\ \gamma & \alpha \beta \gamma +1& \beta \gamma -\alpha & 0\\ -\beta & \alpha & 1& 0\\ 0& 0& 0& 1\end{array}\right]\approx \left[\begin{array}{cccc}1& -\gamma & \beta & 0\\ \gamma & 1& -\alpha & 0\\ -\beta & \alpha & 1& 0\\ 0& 0& 0& 1\end{array}\right]=\hat{R}(\alpha ,\beta ,\gamma )$$

因此，point-to-plane ICP的最小化目标函数中的M可以近似为：
$$\hat{M}=T(t_x,t_y,t_z)\cdot \hat{R}(\alpha ,\beta ,\gamma )=\left[\begin{array}{cccc}1& -\gamma & \beta & t_x\\ \gamma & 1& -\alpha & t_y\\ -\beta & \alpha & 1& t_z\\ 0& 0& 0& 1\end{array}\right]$$
因此，最小化目标可以改写为：
$${\hat{M}}_{opt}=mi{n}_{\hat{M}}\sum _i((\hat{M}\cdot s_i-d_i)\cdot n_i{)}^2$$
式中，每个$(\hat{M}\cdot s_i-d_i)\cdot n_i$可以重写为如下含有六个参数的线性表达式：
$$(\hat{M}\cdot s_i-d_i)\cdot n_i=\left(\hat{M}\cdot \left(\begin{array}{c}{s}_{ix}\\ s_{iy}\\ s_{iz}\\ 1\end{array}\right)-\left(\begin{array}{c}{d}_{ix}\\ d_{iy}\\ d_{iz}\\ 1\end{array}\right)\right)\cdot \left(\begin{array}{c}{n}_{ix}\\ n_{iy}\\ n_{iz}\\ 0\end{array}\right)$$
已知N对对应点，可以把上面的线性表达式写为如下矩阵形式：
$$Ax=b$$
式中：
$$\left[\begin{array}{c}{n}_{1x}{d}_{1x}+n_{1y}{d}_{1y}+n_{1z}{d}_{1z}-n_{1x}{s}_{1x}-n_{1y}{s}_{1y}-n_{1z}{s}_{1z}\\ n_{2x}{d}_{2x}+n_{2y}{d}_{2y}+n_{2z}{d}_{2z}-n_{2x}{s}_{2x}-n_{2y}{s}_{2y}-n_{2z}{s}_{2z}\\ ⋮\\ n_{Nx}{d}_{Nx}+n_{Ny}{d}_{Ny}+n_{Nz}{d}_{Nz}-n_{Nx}{s}_{Nx}-n_{Ny}{s}_{Ny}-n_{Nz}{s}_{Nz}\end{array}\right]$$

$$x=\left(\begin{array}{c}\alpha \\ \beta \\ \gamma \\ t_x\\ t_y\\ t_z\end{array}\right)$$

$$A=\left(\begin{array}{cccccc}{a}_{11}& a_{12}& a_{13}& n_{1x}& n_{1y}& n_{1z}\\ a_{21}& a_{22}& a_{23}& n_{2x}& n_{2y}& n_{2z}\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ a_{N1}& a_{N2}& a_{N3}& n_{Nx}& n_{Ny}& n_{Nz}\end{array}\right)$$

$$a_{i1}=n_{iz}{s}_{iy}-n_{iy}{s}_{iz},a_{iz}=n_{ix}{s}_{iz}-n_{iz}{s}_{ix},a_{i3}=n_{iy}{s}_{ix}-n_{ix}{s}_{iy}$$

求解该线性方程组即可计算处位姿矩阵M。