ICP（一）原理详解

---

一、ICP-SVD推导流程

1. 流程图

2. 有两个点集

其中$X,Y$是两个原始点云的子集，选取的是两个点集中能相互关联的点，即$N_x=N_y$

3. 目标
$$minE(R,t)=min\frac{1}{N_y}\sum _{i=1}^{N_y}{||x_i-R{y}_i-t||}^2$$
取平方可以避免相减后结果为负，如果直接相加可能会导致正负抵消

4. 推导

其中$u_x$和$u_y$分别是点集$X$和$Y$的质心，即

$$\because \sum _{i=1}^{N_y}(x_i-u_x)=0$$
令$E_1(R,t)=\frac{1}{N_y}{\sum }_{i=1}^{N_y}{||x_i-u_x-R(y_i-u_y)||}^2$（只与旋转部分有关）
$E_2(R,t)={||u_x-R{u}_y-t||}^2$（用于求平移部分）
则$E(R,t)=E_1+E_2$
那么对于任意的$R$，均可以找到一个$t$，使得$u_x-R{u}_y-t=0$，即$E_2=0$；
因此可以先根据$E_1(R,t)$求旋转，在根据$E_2(R,t)$求平移
$所以接下来求解E_1$

问题转化为找到合适的$R$，使得$Trace(RH)$达到最大值
a. 求解旋转部分
定理：若有正定矩阵$A{A}^T$则对于任意正交矩阵$B$有：$$Trace(A{A}^T)\ge Trace(BA{A}^T)$$
意义：若能找到$R$，把$Trace(RH)$转换成$Trace(A{A}^T)$的形式，则该$R$就是我们要找的旋转矩阵
方法：对$H$进行SVD分解
$$\begin{gathered} H=U\Sigma V^T \\ R=V{U}^T \\ RH=V{U}^{T}U\Sigma V^T=V\Sigma V^T=V{\Sigma }^{\frac{1}{2}}{\Sigma }^{\frac{1}{2}}{V}^T=V{\Sigma }^{\frac{1}{2}}{\left(V{\Sigma }^{\frac{1}{2}}\right)}^T \end{gathered}$$
b.求解平移部分
旋转$R$确定后根据$$E_2(R,t)=\Vert u_x-R{u}_y-t{\Vert }^2$$
直接得到平移量$$t=u_x-R{u}_y$$

二、ICP-基于优化推导流程

ICP求解问题，可以重新表示为
$$\underset{T}{\min }=\frac{1}{2}\sum _{i=1}^n{\Vert (x_i-T{y}_i)\Vert }_2^2$$

其中$T$为位姿变换矩阵$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
在李代数模式下，又可以重新表示为$$\underset{\xi }{\min }=\frac{1}{2}\sum _{i=1}^n||(x_i-\exp ({\xi }^{\wedge })y_i)|{|}_2^2$$

其中$\xi$为$T$对应的李代数$$J=\frac{\partial e}{\partial \delta \xi }=-{\left(\exp \left({\xi }^{\wedge }\right)y_i\right)}^{\odot }$$
随后，便可根据优化的固定步骤求解位姿。