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

三维点云课程—ICP(Point-to-Point)三维点云课程---ICPPoint-to-Point三维点云课程---ICP(Point-to-Point)1.ICP问题描述2.ICP问题分析3.ICP证明3.11N1T(B−RA)T(B−RA)1\frac{1}{N}1^T(B-RA)^T(B-RA)1N11T(B−RA)T(B−RA)1的化简3.2 2tr((B−RA)11N1T(B−R

月夕花晨TS

1143人浏览 · 2021-11-04 15:58:18

月夕花晨TS · 2021-11-04 15:58:18 发布

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

三维点云课程---ICP Point-to-Point

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

1.ICP问题描述

Iterative Closest Point (ICP)作为点云中的经典配准方法，现在进行原理的介绍。
给予两组点云数据，源点云(通常是旋转的) $P=\{p_1,...,p_{N_p}\},p_i \in R^3$ ，目标点云(参考点云，通常是固定的) $Q=\{q_1,...,q_{N_1}\},q_i \in R^3$ ，找到一个旋转矩阵 $R\in R^3,RR^T=I,|R|=1$ 和平移向量 $t\in R^3$ ,使得 $P, Q$ 达到"最好的配准"。那么"最好的配置"，用数学公式表达如下
$E(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}$
上式假设已经知道哪几个点一一对应了。

2.ICP问题分析

将上述问题写成数学的表达如下
$R,t=arg_{R,t}minE(R,t)=arg_{R,t}min\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}$
此问题还可以扩展到 $m$ 维,其中
$A=[a_1,...,a_N] \in R^{m\times N},\\ B=[b_1,...,b_N] \in R^{m\times N},\\ 1=[1,...,1] \in R^N$
那么
$标量形式\to f(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||b_i-Ra_i-t||^2}}=||B-(RA+t1^T)||_F^2 \leftarrow 矩阵形式\\ \mathop {\min }\limits_{R,t}f(R,t),s.t.RR^T=I_m,|R|=1$
关于上述如何将标量形式转为矩阵的形式，请参考附录1的证明。

现在给出ICP的解法

1.减去中心值进行归一化 $A, B$ 为 $A^{'}, B^{'}$
$L=I_N-\frac{1}{N}11^T\\ A'=AL\\ B'=BL$
2.对 $B'A'^T$ 进行SVD分解
$B'A'^T=U \Sigma V^T$
3. $R, t$ 的最优解为
$R^*=UV^T\\ t^*=\frac{1}{N}{(B-R^*A)1}$
其中 $\frac{A1}{N},\frac{B1}{N}$ 分别为 ${a_i\},\{b_i\}$ 的平均值，记为 $\mu_a,\mu_b$ ,那么上式化简为
$R^*=UV^T\\ t^*=\frac{1}{N}{(B-R^*A)1}=\mu_b-R^*\mu_a$

3.ICP证明

化简 $f (R, t)$
$\begin{aligned} f(R,t)&=||B-RA-t1^T||_F^2\\ &=||(B-RA)+(-t1^T)||_F^2\\ &=||B-RA||_F^2+||-t1^T||_F^2+2\left\langle {B - RA, - t{1^T}} \right\rangle\\ &=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T) \\ \end{aligned}$
关于这部分的推导，请参考附录2的Frobenius范数的性质。

那么问题转化为求解 $\mathop {\min }\limits_{R,t}f(R,t)$ ,第一想法是将 $f (R, t)$ 对 $R, t$ 分别进行求导，那么是先对 $R$ 进行求导还是先对 $t$ 进行求导呢？应该是先对 $t$ 进行求导，因为 $R$ 要满足 $RR^T=I,|R|=1$ 的约束。

对 $t$ 进行求导
$\begin {aligned} \frac{{\partial f}}{{\partial t}} = 0 \\ 2Nt-2(B-RA)1=0 \end {aligned}$
关于矩阵求导的一些性质参考附录3的矩阵求导
$t=\frac{1}{N}{(B-RA)1}$
将 $t$ 代入 $f (R, t)$ 得
$\begin {aligned} f(R,t)&=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T)\\ &=||B-RA||_F^2+\frac{1}{N}1^T(B-RA)^T(B-RA)1-2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T) \end {aligned}$

3.1 $\frac{1}{N}1^T(B-RA)^T(B-RA)1$ 的化简

化简如下，关于如何将矩阵形式转为标量形式，请参考附录4的证明
$\begin {aligned} \frac{1}{N}1^T(B-RA)^T(B-RA)1 &=\begin{bmatrix} 1&{...}&1 \end{bmatrix} \begin{bmatrix} {{{({b_1} - R{a_1})}^T}}\\ {...}\\ {{{({b_N} - R{a_N})}^T}} \end{bmatrix} \begin{bmatrix} {({b_1} - R{a_1})}&{...}&{({b_N} - R{a_N})} \end{bmatrix} \begin{bmatrix} 1\\ {...}\\ 1 \end{bmatrix}\\ &=\frac{1}{N}{\bigg(\Big((b_1-Ra_1)^T+...+(b_N-Ra_N)^T\Big) \Big((b_1-Ra_1)+...(b_N-Ra_N)\Big)\bigg)}\\ &=\frac{1}{N}{\Big(\sum \limits_{i=1}^N{(b_i-Ra_i)^T}\Big) \Big(\sum \limits_{i=1}^N{(b_i-Ra_i)}\Big)}\\ \end {aligned}$

又因为
$\mu_a=\frac{1}{N}{\sum \limits_{i=1}^N {a_i} \in R^m}\\ \mu_b=\frac{1}{N}{\sum \limits_{i=1}^N {b_i} \in R^m}$
那么上式可化简为
$\begin {aligned} &=\frac{1}{N}{N(\mu_b-R\mu_a)^T(\mu_b-R\mu_a)}\\ &=N||\mu_b-R\mu_a||^2 (注意这是一个数) \end {aligned}$
故
$\frac{1}{N}1^T(B-RA)^T(B-RA)1=N||\mu_b-R\mu_a||^2，\mu_a=\frac{1}{N}{\sum \limits_{i=1}^N {a_i} \in R^m}，\mu_b=\frac{1}{N}{\sum \limits_{i=1}^N {b_i} \in R^m}$

3.2 $2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T)$ 的化简

化简如下
$\begin {aligned} 2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T)&=2\frac{1}{N}tr(\begin{bmatrix} {({b_1} - R{a_1})}&{...}&{({b_N} - R{a_N})} \end{bmatrix} \begin{bmatrix} 1\\ {...}\\ 1 \end{bmatrix} \begin{bmatrix} 1&{...}&1 \end{bmatrix} \begin{bmatrix} {{{({b_1} - R{a_1})}^T}}\\ {...}\\ {{{({b_N} - R{a_N})}^T}} \end{bmatrix})\\ &=2\frac{1}{N}{tr\bigg(\Big((b_1-Ra_1)+...(b_N-Ra_N)\Big) \Big((b_1-Ra_1)^T+...+(b_N-Ra_N)^T)\Big)\bigg)}\\ &=2\frac{1}{N}{tr\Big((\sum \limits_{i=1}^N{(b_i-Ra_i)}\Big) \Big(\sum \limits_{i=1}^N{(b_i-Ra_i)^T})\Big)}\\ &=2\frac{1}{N}tr(N(\mu_b-R\mu_a)N(\mu_b-R\mu_a)^T)\\ &=2Ntr\Big((\mu_b-R\mu_a)(\mu_b-R\mu_a)^T\Big)\\ &=2N||\mu_b-R\mu_a||^2 \end {aligned}$
其实上式的推导的过程中用到一些关于矩阵迹的知识，详情参见附录5

3.3 进一步化简代价函数 $f (R, t)$

结合3.1和3.2的式子，化简如下
$\begin {aligned} f(R,t) &=||B-RA||_F^2+Nt^Tt-2tr((B-RA)1t^T)\\ &=||B-RA||_F^2+\frac{1}{N}1^T(B-RA)^T(B-RA)1-2tr((B-RA)1\frac{1}{N}1^T(B-RA)^T)\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+N||\mu_b-R\mu_a||^2-2N||\mu_b-R\mu_a||^2\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+\sum \limits_{i=1}^N{||\mu_b-R\mu_a||^2}-2N(\mu_b-R\mu_a)^T(\mu_b-R\mu_a)\\ &=\sum \limits_{i=1}^N{||b_i-Ra_i||^2}+\sum \limits_{i=1}^N{||\mu_b-R\mu_a||^2}-2\sum \limits_{i=1}^N{(b_i-Ra_i)^T}(\mu_b-R\mu_a)\\ &=\sum \limits_{i=1}^N{||(b_i-Ra_i)-(\mu_b-R\mu_a)||^2}\\ &=\sum \limits_{i=1}^N{||(b_i-\mu_b)-R(a_i-\mu_a)||^2}\\ &=\sum \limits_{i=1}^N{||b'_i-Ra'_i||^2}\\ &=||B'-RA'||_F^2 \end {aligned}$
其中注意 $A$ 与 $A^{'}$ 和 $B$ 与 $B^{'}$ 的关系
$a'=a_i-\mu_a,A'=A(I_N-\frac{1}{N}11^T)\\ b'=b_i-\mu_b,B'=B(I_N-\frac{1}{N}11^T)$
继续对上式的代价函数进行化简
$\begin {aligned} f(R,t) &=||B'-RA'||_F^2\\ &=||B'||_F^2+||-RA'||_F^2+2\left\langle{B',-RA'}\right\rangle\\ &=||B'||_F^2+||RA'||_F^2-2\left\langle{B',RA'}\right\rangle\\ &=||B'||_F^2+||A'||_F^2-2tr(RA'B'^T)\\ &=||B'||_F^2+||A'||_F^2-2tr(RV\Sigma U^T)\\ &=||B'||_F^2+||A'||_F^2-2tr(U^T RV\Sigma )\\ &=||B'||_F^2+||A'||_F^2-2tr(T \Sigma)\\ &=||B'||_F^2+||A'||_F^2-2\sum \limits_{i=1}^m{T_{ii}\sigma_i} \end {aligned}$
思考一下上式的推导过程中 $RA'||_F^2=||A'||_F^2$ 是怎么成立的，现在问题转化为最小化代价函数即最大化 $\sum \limits_{i=1}^m{T_{ii}\sigma_i}$ ,又因为 $T=U^TRV$ ，而 $U, R, V$ 均是正交矩阵，所以 $T$ 也是正交矩阵。由于正交矩阵的每一个元素都进行了单位化，即 $|T_{ii}|\le 1$ ,要使 $\sum \limits_{i=1}^m{T_{ii}\sigma_i}$ 最大，即 $T=I_m$ ,所以
$\begin {aligned} T&=U^TRV=I_m\\ R&=UV^T\\ t&=\frac{1}{N}{(B-RA)1} \end {aligned}$

4.ICP的完整步骤

给予两组点云集

源点云，旋转的， $P=\{p_1,...,p_{N_{p}}\},p_i \in R^3$
目标点云，固定的， $Q=\{q_1,...,q_{N_{q}}\},q_i\in R^3$

数据预处理
1. 对每一个点 $p_i$ 在 $Q$ 内寻找最近的邻居
2. 移除外点对，如 $p_i-q_i||$ 太大
$R,t=arg_{R,t}minE(R,t)=arg_{R,t}min\frac{1}{N}{\sum \limits_{t=1}^N{||q_i-Rp_i-t||^2}}$
1. 计算中心点 $\mu_p=\frac{1}{N}{\sum \limits_{i=1}^N{p_i}}$ , $\mu_q=\frac{1}{N}{\sum \limits_{i=1}^N{q_i}}$
2. $P'={p_i-\mu_p}=P(I_N-\frac{1}{N}{(1\times 1^T)})$ , $Q'={q_i-\mu_q}=Q(I_N-\frac{1}{N}{(1\times 1^T)})$
3. 计算 $Q'P'^T$ 的SVD分解，即 $Q'P'^T=U\Sigma V^T$
4. 计算 $R=UV^T,t=\frac{1}{N}{(B-RA)1}=\mu_q-R\mu_q$
校验
1. 评估迭代的收敛性
  1. $E (R, t)$ 比较小
  2. $\Delta R,\Delta t$ 比较小
2. 迭代没有终止
  1. $\leftarrow RP+t$
  2. 重复1-3步，直至迭代成功。

5.ICP的缺点

上述的ICP方法是基于Point-to-Point的方法，会由于平面之间的滑动导致匹配错误的问题，且迭代次数比较多，需要进行对Point-to-Point的ICP方法进行改进，进而提出了Point-to-Plane的方法。但实际工程中Point-to-Point的方法比较简单，故还是有很多人使用它的。但不可否认Point-to-Plane的方法更优。

附录

1证明
$标量形式\to f(R,t)=\frac{1}{N}{\sum \limits_{t=1}^N{||b_i-Ra_i-t||^2}}=||B-(RA+t1^T)||_F^2 \leftarrow 矩阵形式\\$
定义 $A, B ， R ， t$ 的矩阵形式如下
${\begin{bmatrix} {{b_{11}}}&{...}&{{b_{1n}}}\\ {...}&{...}&{...}\\ {{b_{m1}}}&{...}&{{b_{mn}}} \end{bmatrix}_{m \times n}},{b_i} =\begin{bmatrix} {{b_{1i}}}\\ {...}\\ {{b_{mi}}} \end{bmatrix}\\ A = {\begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}_{m \times n}},{a_i} =\begin{bmatrix} {{a_{1i}}}\\ {...}\\ {{a_{mi}}} \end{bmatrix}\\ R = {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}}\\ t = {\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}}$

那么
$B-RA-t1^T={\begin{bmatrix} {{b_{11}}}&{...}&{{b_{1n}}}\\ {...}&{...}&{...}\\ {{b_{m1}}}&{...}&{{b_{mn}}} \end{bmatrix}_{m \times n}} - {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}} \times {\begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}_{m \times n}} - {\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}} \times {\begin{bmatrix} 1&{...}&1 \end{bmatrix}_{1 \times N}}\\ =\begin{bmatrix} {{b_{11}} - ({R_{11}} \times {a_{11}} + ... + {R_{1m}} \times {a_{m1}}) - {t_1}}&{...}&{{b_{1n}} - ({R_{11}} \times {a_{1n}} + ... + {R_{1m}} \times {a_{mn}}) - {t_1}}\\ {...}&{...}&{...}\\ {{b_{m1}} - ({R_{m1}} \times {a_{11}} + ... + {R_{mm}} \times {a_{m1}}) - {t_m}}&{...}&{{b_{mn}} - ({R_{m1}} \times {a_{1n}} + ... + {R_{mm}} \times {a_{mn}}) - {t_m}} \end{bmatrix}$
因为对于矩阵 $A\in R^{m \times n}$ 的最大Frobenius范数 $A||_F^2$
$||A||_F^2=\sqrt{\sum \limits_{i=1}^m{\sum \limits_{j=1}^n}{|a_{ij}^2|}}=\sqrt{trace(A^*A)}=\sqrt{\sum \limits_{i=1}^{min\{m,n\}}{\sigma_i^2(A)}}$
其中 $A *$ 表示为 $A$ 的共轭转置矩阵，在实数域中 $A^*=A^T$ 。 $\sigma_i$ 表示为 $A$ 矩阵进行SVD分解的奇异值。

那么根据Frobenius范数的定义得
$右式=||B-RA-t1^T||_F^2=||b_{11}-(R_{11} \times a_{11}+...+R_{1m} \times a_{m1}-t_1)||^2+...+||b_{mn}-(R_{m1} \times a_{1n}+...+R_{mm} \times a_{mn}-t_m)||^2$
即 $B-RA-t1^T||_F^2$ 表示为矩阵中的每一个元素先进行平方后，再进行求和。

对于
$b_i-Ra_i-t=\begin{bmatrix} {{b_{1i}}}\\ {...}\\ {{b_{mi}}} \end{bmatrix}-{\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}_{m \times m}} \times \begin{bmatrix} {{a_{1i}}}\\ {...}\\ {{a_{mi}}} \end{bmatrix}-{\begin{bmatrix} {{t_1}}\\ {...}\\ {{t_m}} \end{bmatrix}_{m \times 1}}\\ =\begin{bmatrix} {{b_{1i}} - ({R_{11}} \times {a_{1i}} + ... + {R_{1m}} \times {a_{mi}}) - {t_1}}\\ {...}\\ {{b_{mi}} - ({R_{m1}} \times {a_{1i}} + ... + {R_{mm}} \times {a_{mi}}) - {t_m}} \end{bmatrix}$

所以
$左式=\sum \limits_{i=1}^N{||b_i-Ra_i-t||^2}=||\begin{bmatrix} {{b_{11}} - ({R_{11}} \times {a_{11}} + ... + {R_{1m}} \times {a_{m1}}) - {t_1}}\\ {...}\\ {{b_{m1}} - ({R_{m1}} \times {a_{11}} + ... + {R_{mm}} \times {a_{m1}}) - {t_m}} \end{bmatrix}||^2 +...+ ||\begin{bmatrix} {{b_{1n}} - ({R_{11}} \times {a_{1n}} + ... + {R_{1m}} \times {a_{mn}}) - {t_1}}\\ {...}\\ {{b_{mn}} - ({R_{m1}} \times {a_{1n}} + ... + {R_{mm}} \times {a_{mn}}) - {t_m}} \end{bmatrix}||^2$
化简得左式=右式。

2Frobenius范数的性质
$||A||_F^2=\sqrt{\sum \limits_{i=1}^m{\sum \limits_{j=1}^n}{|a_{ij}^2|}}=\sqrt{trace(A^*A)}=\sqrt{\sum \limits_{i=1}^{min\{m,n\}}{\sigma_i^2(A)}}$
Frobenius范数分解
$||A+B||_F^2=||A||_F^2+||B||_F^2+2\left\langle {A,B} \right\rangle_F$
其中
$\left\langle {A,B} \right\rangle_F=\sum \limits_{ij}{A_{ij}B_{ij}}=tr(A^TB)=tr(AB^T)$

3矩阵的一些求导性质
$\begin {aligned} \frac{\partial }{{\partial X}}{Tr(X)}&=I\\ \frac{\partial }{{\partial X}}{Tr(XA)}&=A^T\\ \frac{\partial }{{\partial X}}{Tr(AXB)}&=A^TB^T\\ \frac{\partial }{{\partial X}}{Tr(AX^TB)}&=BA\\ \frac{\partial }{{\partial X}}{Tr(X^TA)}&=A\\ \frac{\partial }{{\partial X}}{Tr(AX^T)}&=A\\ \frac{\partial }{{\partial X}}{Tr(A)\otimes X}&=Tr(A)I\\ \\ \frac{\partial }{{\partial X}}{\beta^T X}&=\beta\\ \frac{\partial }{{\partial X}}{X^T X} &=2X \\ \frac{\partial }{{\partial X}}{X^TAX} &=(A+A^T)X \end {aligned}$
上式公式的推导主要使用的是
$\frac{\partial }{{\partial X}}{Tr(AX^T)}=A \\ \frac{\partial }{{\partial X}}{X^T X} =2X$

4证明
$\begin {aligned} B-RA&=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix} \times \begin{bmatrix} {{a_{11}}}&{...}&{{a_{1n}}}\\ {...}&{...}&{...}\\ {{a_{m1}}}&{...}&{{a_{mn}}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}\begin{bmatrix} {{a_{11}}}\\ {...}\\ {{a_{m1}}} \end{bmatrix}} &{...} &{\begin{bmatrix} {{R_{11}}}&{...}&{{R_{1m}}}\\ {...}&{...}&{...}\\ {{R_{m1}}}&{...}&{{R_{mm}}} \end{bmatrix}\begin{bmatrix} {{a_{1n}}}\\ {...}\\ {{a_{mn}}} \end{bmatrix}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1}}&{...}&{{b_n}} \end{bmatrix} - \begin{bmatrix} {R{a_1}}&{...}&{R{a_n}} \end{bmatrix}\\ &=\begin{bmatrix} {{b_1} - R{a_1}}&{...}&{{b_n} - R{a_n}} \end{bmatrix} \end {aligned}$

5矩阵迹的性质

迹的定义
$tr(A)=\sum \limits_{i=1}^n{a_{ii}}=a_{11}+a_{22}+...+a_{nn}$
迹的线性性质
$tr(A+B)=tr(A)+tr(B)\\ tr(cA)=ctr(A)\\ tr(A)=tr(A^T)$
迹的高级性质
$tr(A^TB)=tr(AB^T)=tr(B^TA)=tr(BA^T)=\sum \limits_{i,j}{A_{ij}B_{ij}}\\ tr(ba^T)=a^Tb(其中ba^T为向量的外积，a^Tb为向量的内积)$
迹的循环性质
$t r (A B C D) = t r (B C D A) = t r (C D A B) = t r (D A B C)$