点云配准 - ICP证明

知识预备

• 问题描述
给定两个点云 X = { x 1 , . . . , x i , . . . , x N } ⊂ R 3 X = \lbrace x_1, ..., x_i, ..., x_N\rbrace \subset R^3 X={x1​,...,xi​,...,xN​}⊂R3和 Y = { y 1 , . . . , y i , . . . , y M } ⊂ R 3 Y = \lbrace y_1, ..., y_i, ..., y_M\rbrace \subset R^3 Y={y1​,...,yi​,...,yM​}⊂R3, 刚性配准(rigid registration)的任务是找到一个旋转矩阵$R_{XY}\in SO(3)$和平移矩阵$t_{XY}\in R^3$.

• 点云配准的应用: 机器人, 无人驾驶, 医疗影像, 计算化学等
• 旋转矩阵与群:
  • 旋转矩阵: 行列式为1的正交矩阵.
    S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } SO(n) = \lbrace R \in \mathbb R^{n \times n } | RR^T = I, \text{det}(R) = 1 \rbrace SO(n)={R∈Rn×n∣RRT=I,det(R)=1}, 对于乘法,属于特殊正交群(Special Orthogonal Group).
    • 群: 一种集合加上一种运算的代数结构. 定义如下, 集合记作$A$, 运算记作, 那么群记作$G=(A,\cdot )$. 群要求这个运算满足以下几个条件:
    • 封闭性: $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
    • 结合律: $\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
    • 幺元: $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
    • 逆: $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$
      其中 S E ( 3 ) = { [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \lbrace \left[ \begin{matrix} R & t \\ 0^T & 1 \end{matrix} \right] \in \mathbb{R}^{4 \times 4} | R \in SO(3), t \in \mathbb{R}^3 \rbrace SE(3)={[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
      ,关于乘法,也属于群.
      • 李群: 具有连续性质的群
      • 整数群这样离散的群没有连续性质, 不是李群.
      • SO(n)和SE(n)在实数空间上是连续的, 属于李群.

ICP算法

• 迭代进行如下操作:

  • $m(x_i,Y)=\underset{j}{\text{arg min}}||R_{XY}{x}_i+t_{XY}-y_j||$
  • $E(R_{XY},t_{XY},m)=\frac{1}{N}{\sum }_{i=1}^N||R_{XY}{x}_i+t_{XY}-y_{m(x_i)}|{|}^2$
    • 当映射关系$m$确定时, $R_{XY}=V{U}^T,t_{XY}=-R_{XY}\overline{x}+\overline{y}$, 参数说明和证明过程如下。
    • 其中 $\overline{x}=\frac{1}{N}{\sum }_{i=1}^n{x}_i,\overline{y}=\frac{1}{N}{\sum }_{i=1}^n{y}_i$, 记$v_i=y_i-\overline{y},u_i=x_i-\overline{x}$, $H={\sum }_{i=1}^N(u_i{v}_i^T)$, 对进行SVD分解$H=U\Sigma V^T$, 上述$U$和$V$是SVD分解得到.

• SVD 求解ICP算法及其证明[Least-squares fitting of two 3-D point sets]:

  • 第一步, 化简
    记 $\overline{x}=\frac{1}{N}{\sum }_{i=1}^n{x}_i,\overline{y}=\frac{1}{N}{\sum }_{i=1}^n{y}_i$, 当$m(x_i)=y_i$时,

    $$\begin{array}{rl}E(R_{XY},t_{XY})& =\frac{1}{N}\sum _{i=1}^N||R_{XY}{x}_i+t_{XY}-y_i|{|}^2\\ & =\frac{1}{N}\sum _{i=1}^N||R_{XY}{x}_i+t_{XY}-y_i-\overline{y}+\overline{y}-R_{XY}\overline{x}+R_{XY}\overline{x}|{|}^2\\ & =\frac{1}{N}\sum _{i=1}^N||(\overline{y}-y_i+R_{XY}(x_i-\overline{x}))+(t_{XY}+R_{XY}\overline{x}-\overline{y})|{|}^2\\ & =\frac{1}{N}\sum _{i=1}^N[||\overline{y}-y_i+R_{XY}(x_i-\overline{x})|{|}^2+||t_{XY}+R_{XY}\overline{x}-\overline{y}|{|}^2+\\ & 2(\overline{y}-y_i+R_{XY}(x_i-\overline{x}){)}^T(t_{XY}+R_{XY}\overline{x}-\overline{y})]\end{array}$$

    其中第三项:

    $$\begin{array}{rl}& \frac{1}{N}\sum _{i=1}^{N}2(\overline{y}-y_i+R_{XY}(x_i-\overline{x}){)}^T(t_{XY}+R_{XY}\overline{x}-\overline{y})\\ & =\frac{2}{N}[\sum _{i=1}^N(\overline{y}-y_i+R_{XY}(x_i-\overline{x}))](t_{XY}+R_{XY}\overline{x}-\overline{y})\\ & =\frac{2}{N}[n\overline{y}-\sum _{i=1}^N{y}_i+R_{XY}(\sum _{i=1}^N{x}_i-n\overline{x})](t_{XY}+R_{XY}\overline{X}-\overline{y})\\ & =\frac{1}{N}\cdot 0\cdot (t_{XY}+R_{XY}\overline{X}-\overline{y})\\ & =0\end{array}$$

    因此, 原优化函数可以化简成

    $$\begin{array}{rl}E(R_{XY},t_{XY})& =\frac{1}{N}\sum _{i=1}^N[||\overline{y}-y_i+R_{XY}(x_i-\overline{x})|{|}^2+||t_{XY}+R_{XY}\overline{x}-\overline{y}|{|}^2]\\ & =\frac{1}{N}\sum _{i=1}^N[||y_i-\overline{y}-R_{XY}(x_i-\overline{x})|{|}^2+||t_{XY}+R_{XY}\overline{x}-\overline{y}|{|}^2]\end{array}$$

  • 第二步, 分析
    注意到第一项只包含$R_{XY}$, 因此只需求解$R_{XY}$使得${\sum }_{i=1}^N||y_i-\overline{y}-R_{XY}(x_i-\overline{x})|{|}^2$最小即可, $t_{XY}$可通过$t_{XY}=\overline{y}-R_{XY}\overline{x}$求得.

    考虑去均值坐标, 记$v_i=y_i-\overline{y},u_i=x_i-\overline{x},$
    $$\begin{array}{rl}\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N||y_i-\overline{y}-R_{XY}(x_i-\overline{x})|{|}^2& =\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N||v_i-R_{XY}{u}_i|{|}^2\\ & =\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N(v_i^T{v}_i+u_i^T{R}_{XY}^T{R}_{XY}{u}_i-2v_i^T{R}_{XY}{u}_i)\\ & =\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N(v_i^T{v}_i+u_i^T{u}_i-2v_i^T{R}_{XY}{u}_i)\\ & =\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N(-v_i^T{R}_{XY}{u}_i)\\ & =\underset{R_{XY}}{\mathrm{argmin}}\sum _{i=1}^N(-\text{tr}(R_{XY}{u}_i{v}_i^T))\\ & =\underset{R_{XY}}{\mathrm{argmin}}-\text{tr}(R_{XY}\sum _{i=1}^N(u_i{v}_i^T))\end{array}$$

    记 $H={\sum }_{i=1}^N(u_i{v}_i^T)$, 对$H$进行SVD分解$H=U\Sigma V^T$， 其中$U$, $V$是正交阵

  引理(Lemma): 对于任意正定矩阵$A{A}^T$与正交矩阵$B$, 有 $\text{tr}(A{A}^T)\ge \text{tr}(BA{A}^T)$.

  借助引理:

  $$\begin{array}{rl}\text{tr}(R_{XY}H)& =\text{tr}(R_{XY}U\Sigma V^T)\\ & =\text{tr}(R_{XY}U{\Sigma }^{\frac{1}{2}}{\Sigma }^{\frac{1}{2}}{V}^T)\\ & =\text{tr}(R_{XY}U{V}^{T}V{\Sigma }^{\frac{1}{2}}{\Sigma }^{\frac{1}{2}}{V}^T)\\ & =\text{tr}(R_{XY}U{V}^{T}V{\Sigma }^{\frac{1}{2}}(V{\Sigma }^{\frac{1}{2}}{)}^T)\\ & <=tr(V{\Sigma }^{\frac{1}{2}}(V{\Sigma }^{\frac{1}{2}}{)}^T)[\text{Derivated from Lemma}]\end{array}$$

  取等条件 $R_{XY}=V{U}^T$

• 缺点: 非凸, 局部最优; 对于大点云, 耗时.