ICP算法和优化问题详细公式推导

1. 介绍

ICP(Iterative Closest Point)：求一组平移和旋转使得两个点云之间重合度尽可能高。

2. 算法流程

找最近邻关联点，求解$R,tR,tR,tR,t$，如此反复直到重合程度足够高。

3. 数学描述

X = { x 1 , x 2 , . . . , x N x } X=\left\{ {x_1,x_2,...,x_{Nx} }\right\} X={x1​,x2​,...,xNx​}

Y = { y 1 , y 2 , . . . , y N y } Y=\left\{ {y_1,y_2,...,y_{Ny} } \right\} Y={y1​,y2​,...,yNy​}

由于是变换前后对应点云，则$Nx=Ny$

$\min E(R,t)=\min {\sum }_{i=1}^{Nx}{\Vert y_i-\left(R{x}_i+t\right)\Vert }^2$

求解使其最小的$R,t$

4. 解法

4.1 SVD直接解法

$$\begin{array}{rl}\min E(R,t)& =\min {\sum }_{i=1}^{Nx}{\Vert y_i-\left(R{x}_i+t\right)\Vert }^2\\ & =\min {\sum }_{i=1}^{Nx}{\Vert y_i-R{x}_i-t-{\mu }_y+R{\mu }_x+{\mu }_y-R{\mu }_x\Vert }^2\end{array}$$

其中

$${\mu }_x=\sum {i=1}^{Nx}{x}_i,{\mu }_y={\sum }_{i=1}^{Ny}{y}_i$$
则
$$\begin{array}{rl}\min E(R,t)& =\min {\sum }_{i=1}^{Nx}{\Vert y_i-R{x}_i-t-{\mu }_y+R{\mu }_x+{\mu }_y-R{\mu }_x\Vert }^2\\ & =\min {\sum }_{i=1}^{Nx}{\Vert y_i-{\mu }_y-R(x_i-{\mu }_x)+{\mu }_y-R{\mu }_x-t\Vert }^2\\ & =\min {\sum }_{i=1}^{Nx}({\Vert y_i-{\mu }_y-R(x_i-{\mu }_x)\Vert }^2+{\Vert {\mu }_y-R{\mu }_x-t\Vert }^2\\ & +2[y_i-{\mu }_y-R(x_i-{\mu }_x){]}^T({\mu }_y-R{\mu }_x-t))\end{array}$$

其中${\sum }_{i=1}^{Nx}{x}_i-Nx{\mu }_x=0,{\sum }_{i=1}^{Ny}{y}_i-Ny{\mu }_y=0$

$$\min E(R,t)=\min {\sum }_{i=1}^{Nx}({\Vert y_i-{\mu }_y-R(x_i-{\mu }_x)\Vert }^2+{\Vert {\mu }_y-R{\mu }_x-t\Vert }^2）$$

这样第一项中只含有变量$R$，第二项含有$R,t$，求整体的最小值可以使第一项的值最小，将得到的$R$，带入第二项中，再使第二项为零求出$t$的值。

$$\begin{array}{rl}\min {\sum }_{i=1}^{Nx}& {\Vert y_i-{\mu }_y-R(x_i-{\mu }_x)\Vert }^2=\min {\sum }_{i=1}^{Nx}{\Vert y_i^{\prime }-R{x}_i^{\prime }\Vert }^2\\ & =\min {\sum }_{i=1}^{Nx}(y_i^{\prime }-R{x}_i^{\prime }{)}^T(y_i^{\prime }-R{x}_i^{\prime })\\ & =\min {\sum }_{i=1}^{Nx}{\Vert y_i^{\prime }\Vert }^2+x_i^{\prime T}{R}^{T}R{x}_i^{\prime }-x_i^{\prime T}{R}^T{y}_i^{\prime }-y_i^{\prime T}R{x}_i^{\prime }\\ & =\min {\sum }_{i=1}^{Nx}{\Vert y_i^{\prime }\Vert }^2+x_i^{\prime T}{R}^{T}R{x}_i^{\prime }-2x_i^{\prime T}{R}^T{y}_i^{\prime }\end{array}$$

求解原函数最小值等同于求解：$\min {\sum }_{i=1}^{Nx}-x_i^{\prime T}{R}^T{y}_i^{\prime }$

等同于求解：$\max {\sum }_{i=1}^{Nx}{x}_i^{\prime T}{R}^T{y}_i^{\prime }$

$$\max {\sum }_{i=1}^{Nx}{x}_i^{\prime T}{R}^T{y}_i^{\prime }=\max Trace\left({\sum }_{i=1}^{Nx}R{x}_i^{\prime }{y}_i^{\prime T}\right)=\max Trace(RH)$$

由定理可知对于正定矩阵$A{A}^T$和任意正交矩阵$B$，有：$Tr(A{A}^T)\ge Tr(BA{A}^T)$

则对矩阵$H$进行SVD分解：

$$H=U\Sigma V^T$$

取$R=V{U}^T$，则由定理得到此时的$R$使得原式最大

再对应求解：$t={\mu }_y-R{\mu }_x$

4.2 迭代求解

将ICP构建成一个优化问题

$$F(x)={\sum }_{i=1}^n{\Vert x_i-T{y}_i\Vert }_2^2$$

其中$T$为位姿变换矩阵

$$T=\left[\begin{array}{cc}R& \mathrm{t}\\ 0& 1\end{array}\right]$$

求解优化问题需要构建残差约束并计算雅可比矩阵，这里的残差为$x_i-T{y}_i$（可以理解为为变换后的点云与目标待配准点云之间的差异），计算雅可比矩阵可以使用李代数的方法对目标函数求导。

5.（非线性优化问题详细公式推导）

问题描述：

对于一个目标函数$F(X)={\Vert f(X)\Vert }^2$，其中$F:{\mathbb{R}}^n\to {\mathbb{R}}^m,X\in {\mathbb{R}}^n$ ，需要求解$X$使得目标函数$F(X)$ 取值最小。如果$f(X)$为线性函数，则可以简化为最小二乘问题，可以求得最优解，但$f(X)$为非线性函数则无法直接求解，需要迭代求解。

$$F(X+\Delta X)={\Vert f(X+\Delta X)\Vert }^2$$

迭代求$\Delta X$使得$F(X+\Delta X)$减小，直到其足够小并接近最小值时停止。

那么$\Delta X$的取值该如何确定？

1. 高斯法

对$F(X+\Delta X)$进行Taylor Expansion得到：

$$F(X+\Delta X)=F(X)+J_F\Delta X+\frac{1}{2}\Delta X^T{H}_F\Delta X+O(\Delta X)$$

只取到二阶项，其中$J$为$F$的雅可比矩阵，$H$为海塞矩阵，$O(\Delta X)$为忽略的高阶项。

雅可比矩阵定义:

有矩阵函数$F(X)$，其中$F:{\mathbb{R}}^n\to {\mathbb{R}}^m,X\in {\mathbb{R}}^n$

$$J_F=\frac{\partial F(X)}{\partial X^T}$$

使等式对$\Delta X$求导为$0$得到：

$$\frac{\partial F(X+\Delta X)}{\partial \Delta X}=J_F+H_F\Delta X=0$$

由此得到$\Delta X$的取值。

2. 高斯-牛顿法

$$F(X+\Delta X)={\Vert f(X+\Delta X)\Vert }^2$$

对$f(X+\Delta X)$进行Taylor Expansion得到：

$$f(X+\Delta X)=f(X)+J_f\Delta X+O(\Delta X)$$

只取到一阶项。

$$\begin{array}{rl}F(X+\Delta X)& ={\Vert f(X+\Delta X)\Vert }^2\\ & =(f(X)+J_f\Delta X{)}^T(f(X)+J_f\Delta X)\\ & ={\Vert f(X)\Vert }^2+\Delta X^T{J}_f^T{J}_f\Delta X+\Delta X^T{J}_f^{T}f(X)+f(X{)}^T{J}_f\Delta X\\ & ={\Vert f(X)\Vert }^2+\Delta X^T{J}_f^T{J}_f\Delta X+2f(X{)}^T{J}_f\Delta X\end{array}$$

使等式对$\Delta X$求导等于$0$得到：

$$\frac{\partial F(X+\Delta X)}{\partial \Delta X}=2J_f^T{J}_f\Delta X+2f(X{)}^T{J}_f=0$$

可以求得

$$J_f^T{J}_f\Delta X=-f(X{)}^T{J}_f$$