本文介绍了point-to-plane ICP算法，它通过源曲面上的点到目标曲面法线的距离作为误差度量，以提高配准速度。在位姿变换较小的情况下，通过线性近似将非线性优化问题转换为线性最小二乘问题，从而实现高效求解。
摘要
原始的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采用的是源曲面点到目标曲面点处的切平面之间的距离作为误差度量。
具体来说，设si=(six,siy,siz)Ts_{i}=(s_{ix},s_{iy},s_{iz})^{T}si=(six,siy,siz)T是源曲面上的一点，di=(dix,diy,diz)Td_{i}=(d_{ix},d_{iy},d_{iz})^{T}di=(dix,diy,diz)T是sis_{i}si在目标曲面上的对应点，ni=(nix,niy,niz)Tn_{i}=(n_{ix},n_{iy},n_{iz})^{T}ni=(nix,niy,niz)T是did_{i}di处的法线向量，那么point-to-plane ICP算法中每次迭代过程计算对应性位姿矩阵的优化目标为：
Mopt=minM∑i((M⋅si−di)⋅ni)2
M_{opt}={min}_{M}\sum_{i}((M\cdot s_{i}-d_{i})\cdot n_{i})^{2}
Mopt=minMi∑((M⋅si−di)⋅ni)2
式中，M,MoptM,M_{opt}M,Mopt是4x4的刚体变换矩阵。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PKnjMZEX-1605417765155)(resources/db688f116d5e4d6ea03bbad41ffc867e.png)]
如上图所示，point-to-plane ICP最小化的目标函数是l1+l2+l3l_{1}+l_{2}+l_{3}l1+l2+l3。
3. 线性近似
刚体变换矩阵
3D刚体变换矩阵MMM可以由一个旋转矩阵和一个平移矩阵组成：
M=T(tx,ty,tz)⋅R(α,β,γ)
M=T(t_{x},t_{y},t_{z})\cdot R(\alpha,\beta,\gamma)
M=T(tx,ty,tz)⋅R(α,β,γ)
式中：
T(tx,ty,tz)=[100tx010ty001tz0001]
T(t_{x},t_{y},t_{z})=\begin{bmatrix}
1 & 0 & 0 &t_{x} \\
0 & 1& 0& t_{y}\\
0& 0 &1 &t_{z} \\
0& 0& 0& 1
\end{bmatrix}
T(tx,ty,tz)=⎣⎢⎢⎡100001000010txtytz1⎦⎥⎥⎤
R(α,β,γ)=[r11r12r130r21r22r230r31r32r3300001] R(\alpha,\beta,\gamma)=\begin{bmatrix} 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{bmatrix} R(α,β,γ)=⎣⎢⎢⎡r11r21r310r12r22r320r13r23r3300001⎦⎥⎥⎤
式中：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-z8KS9JSQ-1605417765158)(resources/9c2cfc42a77c425c9a1bed17523e603d.png)]
Rx(α),Ry(β),Rz(γ)R_{x}(\alpha),R_{y}(\beta),R_{z}(\gamma)Rx(α),Ry(β),Rz(γ)分别绕着x,y,zx,y,zx,y,z轴的旋转。
将上式代入point-to-plane ICP优化的目标函数中，可以得到一个关于α,β,γ,tx,ty,tz\alpha,\beta,\gamma,t_{x},t_{y},t_{z}α,β,γ,tx,ty,tz六个参数的方程组。本质上，这是一个最小二乘问题。但是由于α,β,γ\alpha,\beta,\gammaα,β,γ是非线性三角函数的参数，因此线性最小二乘无法对该系统进行求解。接下来将介绍如何把上面的非线性优化问题近似为线性最小二乘问题。
理论基础：
当θ≈0\theta\approx0θ≈0，可以使用如下近似等式：
sinθ≈θ,cosθ≈1 sin\theta \approx\theta,cos\theta\approx1 sinθ≈θ,cosθ≈1
根据上面的理论基础，可以得到：
R(α,β,γ)=[1αβ−γαγ+β0γαβγ+1βγ−α0−βα100001]≈[1−γβ0γ1−α0−βα100001]=R^(α,β,γ)
R(\alpha,\beta,\gamma)=\begin{bmatrix}
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{bmatrix}\approx
\begin{bmatrix}
1 & -\gamma & \beta & 0\\
\gamma &1 &-\alpha & 0\\
-\beta &\alpha &1 &0 \\
0 & 0 & 0 &1
\end{bmatrix}=
\hat{R}(\alpha,\beta,\gamma)
R(α,β,γ)=⎣⎢⎢⎡1γ−β0αβ−γαβγ+1α0αγ+ββγ−α100001⎦⎥⎥⎤≈⎣⎢⎢⎡1γ−β0−γ1α0β−α100001⎦⎥⎥⎤=R^(α,β,γ)
因此，point-to-plane ICP的最小化目标函数中的M可以近似为：
M^=T(tx,ty,tz)⋅R^(α,β,γ)=[1−γβtxγ1−αty−βα1tz0001]
\hat{M}=T(t_{x},t_{y},t_{z}) \cdot \hat{R}(\alpha,\beta,\gamma)=
\begin{bmatrix}
1 & -\gamma & \beta & t_{x} \\
\gamma & 1 & -\alpha & t_{y} \\
-\beta & \alpha &1 & t_{z} \\
0 & 0 & 0 & 1
\end{bmatrix}
M^=T(tx,ty,tz)⋅R^(α,β,γ)=⎣⎢⎢⎡1γ−β0−γ1α0β−α10txtytz1⎦⎥⎥⎤
因此，最小化目标可以改写为：
M^opt=minM^∑i((M^⋅si−di)⋅ni)2
\hat{M}_{opt}=min_{\hat{M}}\sum_{i}((\hat{M} \cdot s_{i}-d_{i})\cdot n_{i})^{2}
M^opt=minM^i∑((M^⋅si−di)⋅ni)2
式中，每个(M^⋅si−di)⋅ni(\hat{M}\cdot s_{i}-d_{i})\cdot n_{i}(M^⋅si−di)⋅ni可以重写为如下含有六个参数的线性表达式：
(M^⋅si−di)⋅ni=(M^⋅(sixsiysiz1)−(dixdiydiz1))⋅(nixniyniz0)
(\hat{M}\cdot s_{i}-d_{i})\cdot n_{i}=
\left ( \hat{M} \cdot \begin{pmatrix}
s_{ix}\\
s_{iy}\\
s_{iz}\\
1
\end{pmatrix}-\begin{pmatrix}
d_{ix}\\
d_{iy}\\
d_{iz}\\
1
\end{pmatrix}\right ) \cdot \begin{pmatrix}
n_{ix}\\
n_{iy}\\
n_{iz}\\
0
\end{pmatrix}
(M^⋅si−di)⋅ni=⎝⎜⎜⎛M^⋅⎝⎜⎜⎛sixsiysiz1⎠⎟⎟⎞−⎝⎜⎜⎛dixdiydiz1⎠⎟⎟⎞⎠⎟⎟⎞⋅⎝⎜⎜⎛nixniyniz0⎠⎟⎟⎞
已知N对对应点，可以把上面的线性表达式写为如下矩阵形式：
Ax=b
Ax=b
Ax=b
式中：
[n1xd1x+n1yd1y+n1zd1z−n1xs1x−n1ys1y−n1zs1zn2xd2x+n2yd2y+n2zd2z−n2xs2x−n2ys2y−n2zs2z⋮nNxdNx+nNydNy+nNzdNz−nNxsNx−nNysNy−nNzsNz]
\begin{bmatrix}
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} \\
\vdots \\
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{bmatrix}
⎣⎢⎢⎢⎡n1xd1x+n1yd1y+n1zd1z−n1xs1x−n1ys1y−n1zs1zn2xd2x+n2yd2y+n2zd2z−n2xs2x−n2ys2y−n2zs2z⋮nNxdNx+nNydNy+nNzdNz−nNxsNx−nNysNy−nNzsNz⎦⎥⎥⎥⎤
x=(αβγtxtytz) x=\begin{pmatrix} \alpha\\ \beta\\ \gamma\\ t_{x}\\ t_{y}\\ t_{z} \end{pmatrix} x=⎝⎜⎜⎜⎜⎜⎜⎛αβγtxtytz⎠⎟⎟⎟⎟⎟⎟⎞
A=(a11a12a13n1xn1yn1za21a22a23n2xn2yn2z⋮⋮⋮⋮⋮⋮aN1aN2aN3nNxnNynNz) A= \begin{pmatrix} a_{11} &a_{12} & a_{13} &n_{1x} & n_{1y} &n_{1z} \\ a_{21} &a_{22} &a_{23} &n_{2x} &n_{2y} &n_{2z} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots\\ a_{N1} & a_{N2} &a_{N3} & n_{Nx} & n_{Ny} & n_{Nz} \end{pmatrix} A=⎝⎜⎜⎜⎛a11a21⋮aN1a12a22⋮aN2a13a23⋮aN3n1xn2x⋮nNxn1yn2y⋮nNyn1zn2z⋮nNz⎠⎟⎟⎟⎞
ai1=nizsiy−niysiz,aiz=nixsiz−nizsix,ai3=niysix−nixsiy 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} ai1=nizsiy−niysiz,aiz=nixsiz−nizsix,ai3=niysix−nixsiy
求解该线性方程组即可计算处位姿矩阵M。
扫一扫
1467
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
