(七) 三维点云课程---ICP (Point-to-Plane)

三维点云课程—ICP (Point-to-Plane)由于Point-to-Point的方法不允许平面之间的滑动，故提出了Point-to-Plane的ICP方法1.Point-to-Plane的原理介绍Point-to-Plane不同于Point-to-Point的方法，它是求源点云中的点pip_ipi 到目标点云中qiq_iqi组成的曲面的距离。也就是说，此时点云需要提供每个点的法向量。数

月夕花晨TS

5403人浏览 · 2021-11-04 20:02:55

月夕花晨TS · 2021-11-04 20:02:55 发布

三维点云课程—ICP (Point-to-Plane)

由于Point-to-Point的方法不允许平面之间的滑动，故提出了Point-to-Plane的ICP方法

1.Point-to-Plane的原理介绍

在这里插入图片描述

Point-to-Plane不同于Point-to-Point的方法，它是求源点云中的点 $p_i$ 到目标点云中 $q_i$ 组成的曲面的距离。也就是说，此时点云需要提供每个点的法向量。

数学表述
$E(R,t)=\sum \limits_{i=1}^N \Big((Rp_i+t-q_i)^Tn_i \Big)^2$
其中

源点云(通常是旋转的) $P={p1,...,pNp},pi∈R3P=\{p_1,...,p_{N_p}\},p_i \in R^3$

目标点云(参考点云，通常是固定的) $Q={q1,...,qN1},qi∈R3Q=\{q_1,...,q_{N_1}\},q_i \in R^3$

每个 $q_i$ 点的法向量 $n_i$

对于上式的 $E (R, t)$ ，遗憾的是没有解析解，对于这样的问题，可以采用最小二乘的方法。因为旋转矩阵 $R$ 有9个元素，但要满足 $RR^T=I,|R|=1$ 的约束，故 $R$ 不能随便选。此时可以采用欧拉角的方式描述旋转矩阵。

定义如下

$α\alpha$ 绕 $x$ 轴旋转， $β\beta$ 绕 $y$ 轴旋转， $γ\gamma$ 绕 $z$ 轴旋转,那么
${R_x}(\alpha ) = \begin{bmatrix} 1&0&0\\ 0&{\cos \alpha }&{ - \sin \alpha }\\ 0&{\sin \alpha }&{\cos \alpha } \end{bmatrix} \\ {R_y}(\beta ) =\begin{bmatrix} 1&0&0\\ 0&{\cos \beta }&{ - \sin \beta }\\ 0&{\sin \beta }&{\cos \beta } \end{bmatrix}\\ {R_z}(\gamma ) = \begin{bmatrix} 1&0&0\\ 0&{\cos \gamma }&{ - \sin \gamma }\\ 0&{\sin \gamma }&{\cos \gamma } \end{bmatrix}$
因此
$\begin {aligned} R=R_z(\gamma)R_y(\beta)R_x(\alpha) &=\begin{bmatrix} 1&0&0\\ 0&{\cos \gamma }&{ - \sin \gamma }\\ 0&{\sin \gamma }&{\cos \gamma } \end{bmatrix} \begin{bmatrix} 1&0&0\\ 0&{\cos \beta }&{ - \sin \beta }\\ 0&{\sin \beta }&{\cos \beta } \end{bmatrix} \begin{bmatrix} 1&0&0\\ 0&{\cos \alpha }&{ - \sin \alpha }\\ 0&{\sin \alpha }&{\cos \alpha } \end{bmatrix}\\ &=\begin{bmatrix} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}\\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}}\\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}} \end{bmatrix} \end {aligned}$
化简得
$\begin {aligned} r_{11}&=cos \gamma cos\beta \\ r_{12}&=-sin \gamma cos \alpha + cos \gamma sin \beta sin \alpha\\ r_{13}&=sin \gamma sin \alpha + cos \gamma sin \beta cos \alpha\\ r_{21}&=sin \gamma cos \beta\\ r_{22}&=cos \gamma cos \alpha + sin \gamma sin \beta sin \alpha\\ r_{23}&=-cos \gamma sin \alpha + sin \gamma sin \beta cos \alpha\\ r_{31}&=-sin \beta\\ r_{32}&=cos \beta sin \alpha \\ r_{33}&=cos \beta cos \alpha \end {aligned}$
这样得到的 $R$ s是一个非线性的矩阵，不能采用最小二乘的方法，需要用高斯牛顿或者列文伯格的犯法，不过特别麻烦。进而采用近似的方法
$\alpha,\beta,\gamma \to 0\\ cos \theta \approx 1,sin \theta \approx \theta,\theta^2 \approx 0,if \; \; \theta \approx 0$
那么
$\approx \begin{bmatrix} 1&{\alpha \beta - \gamma }&{\alpha \gamma + \beta }\\ \gamma &{\alpha \beta \gamma + 1}&{\beta \gamma - \alpha }\\ { - \beta }&\alpha &1 \end{bmatrix} \approx \begin{bmatrix} 1&{ - \gamma }&\beta \\ \gamma &1&{ - \alpha }\\ { - \beta }&\alpha &1 \end{bmatrix}$
所以
$(Rp_i+t-q_i)^Tn_i=(n_{iz}p_{iy}-n_{iy}p_{iz})\alpha+(n_{ix}p_{iz}-n_{iz}p_{ix})\beta+(n_{iy}p_{ix}-n_{ix}p_{iy})\gamma\\ +n_{ix}t_x+n_{iy}t_y+n_{iz}t_z\\ -(n_{ix}q_{ix}+n_{iy}q_{iy}+n_{iz}q_{iz}-n_{ix}p_{ix}-n_{iy}p_{iy}-n_{iz}p_{iz})$
化成 $A x = b$ 的形式
$\begin {aligned} A&=\begin{bmatrix} {{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{bmatrix}\\ x&=\begin{bmatrix} \alpha \\ \beta \\ \gamma \\ {{t_x}}\\ {{t_y}}\\ {{t_z}} \end{bmatrix}\\ b&=\begin{bmatrix} n_{1x}q_{1x}+n_{1y}q_{1y}+n_{1z}q_{1z}-n_{1x}p_{1x}-n_{1y}p_{1y}-n_{1z}p_{1z}\\ n_{2x}q_{2x}+n_{2y}q_{2y}+n_{2z}q_{2z}-n_{2x}p_{2x}-n_{2y}p_{2y}-n_{2z}p_{2z}\\ 0\\ n_{Nx}q_{Nx}+n_{Ny}q_{Ny}+n_{Nz}q_{Nz}-n_{Nx}p_{Nx}-n_{Ny}p_{Ny}-n_{Nz}p_{Nz} \end{bmatrix} \end {aligned}$
其中
$a_{i1}=n_{iz}p_{iy}-n_{iy}p_{iz}\\ a_{i2}=n_{ix}p_{iz}-n_{iz}p_{ix}\\ a_{i3}=n_{iy}p_{ix}-n_{ix}p_{iy}$
最后通过最小二乘的解法 $x=(A^TA)^{-1}A^Tb$ ,解的 $x$ 在恢复 $R, t$

2.Point-to-Plane的步骤

给予两组点云集

源点云，旋转的， $P={p1,...,pNp},pi∈R3P=\{p_1,...,p_{N_{p}}\},p_i \in R^3$
目标点云，固定的， $Q={q1,...,qNq},qi∈R3Q=\{q_1,...,q_{N_{q}}\},q_i\in R^3$

数据预处理
1. 对每一个点 $p_i$ 在 $Q$ 内寻找最近的邻居
2. 移除外点对，如 $p_i-q_i||$ 太大
$R,t=argR,tminE(R,t)=argR,tmin1N∑t=1N((Rpi+t−qi)Tni)2R,t=arg_{R,t}minE(R,t)=arg_{R,t}min\frac{1}{N}{\sum \limits_{t=1}^N{((Rp_i+t-q_i)^Tn_i)^2}}$
1. 通过点的三维值和法向量构建 $A, x, b$
2. $x^=arg⁡min⁡xE(x)=∣∣Ax−b∣∣2=(ATA)−1ATb\hat x=\arg \mathop {\min }\limits_x E(x)=||Ax-b||^2=(A^TA)^{-1}A^Tb$
3. $x^=[α,β,γ,tx,ty,tz]T\hat x=[\alpha,\beta,\gamma,t_x,t_y,t_z]^T$
4. 从 $x^\hat x$ 恢复 $R, t$
校验
1. 评估迭代的收敛性
  1. $E (R, t)$ 比较小
  2. $ΔR,Δt\Delta R,\Delta t$ 比较小
2. 迭代没有终止
  1. $\leftarrow RP+t$
  2. 重复1-3步，直至迭代成功。