当我们知道空间点在两个坐标系下的位置时，可以通过ICP来求解其相对位姿。ICP最初常用在激光SLAM中，因为激光可以获得3D位置，而不像视觉还需要经过相机的投影变换，但是从激光雷达获得的三维点云中，我们很难对两帧点云进行匹配，通常是寻找距离最近的点作为匹配点，所以称为迭代最近法（ICP）。当我们已知相机的位姿后，可以把相机坐标系下的特征点$P_i^c$转换到世界坐标系下，这与世界坐标系下对应的特征点$P_i^w$存在误差，即：
$$e_i=P_i^w-(R_{wc}{P}_i^c+t_{wc})$$
考虑所有特征点的误差，我们定义总误差为：
$$J(R,t)=\frac{1}{2}\sum _{i=1}^n\Vert e_i{\Vert }_2^2$$
我们的目标是寻找一个最优的$R$、$t$，使得总误差$J$最小，即：
$$T^{\ast }=\underset{R.t}{\mathrm{argmin}}J(R,t)$$
这是一个非线性最小二乘问题，通常可以用非线性优化的方法进行求解，但对于ICP来说，我们还可以使用SVD分解的方式求出解析解。

SVD方法

首先我们先定义相机坐标系下和世界坐标系下所有3D特征点的质心分别为（注意质心是没有下标的）：
$$P^c=\frac{1}{n}\sum _{i=1}^n{P}_i^c$$ $$P^w=\frac{1}{n}\sum _{i=1}^n{P}_i^w$$
考虑第$i$个误差项：
$$\begin{array}{rl}{e}_i& =P_i^w-(R_{wc}{P}_i^c+t_{wc})\\ & =P_i^w-P^w+P^w-R_{wc}(P_i^c-P^c+P^c)-t_{wc}\\ & =P_i^w-P^w-R_{wc}(P_i^c-P^c)+P^w-(R_{wc}{P}^c+t_{wc})\\ & =\underset{e_{ia}}{\underbrace{{P^{\prime }}_i^w-R_{wc}{P^{\prime }}_i^c}}+\underset{e_{ib}}{\underbrace{P^w-(R_{wc}{P}^c+t_{wc})}}\end{array}$$
其中${P^{\prime }}_i^w=P_i^w-P^w$，${P^{\prime }}_i^c=P_i^c-P^c$，分别表示特征点在世界和相机坐标系下的去质心坐标。由于：
$$\sum _{i=1}^n{e}_{ia}=\sum _{i=1}^n({P^{\prime }}_i^w-R_{wc}{P^{\prime }}_i^c)=0$$
那么总误差可以化简为：
$$J(R,t)=\frac{1}{2}\sum _{i=1}^n\Vert e_i{\Vert }_2^2=\frac{1}{2}\sum _{i=1}^n\Vert e_{ia}{\Vert }_2^2+\frac{1}{2}\sum _{i=1}^n\Vert e_{ib}{\Vert }_2^2=J_a(R)+J_b(R,t)$$
由于$J_a$只与$R$有关，与$t$无关，我们可以先通过优化$J_a$求得$R$，再带入$J_b(R,t)=0$，求得$t$。$J_a$可进一步展开为：
$$J_a(R)=\frac{1}{2}\sum _{i=1}^n\Vert {P^{\prime }}_i^w-R_{wc}{P^{\prime }}_i^c{\Vert }_2^2=\frac{1}{2}\sum _{i=1}^n({{P^{\prime }}_i^w}^T{P^{\prime }}_i^w+{{P^{\prime }}_i^c}^T{R}_{wc}^T{R}_{wc}{P^{\prime }}_i^c-2{{P^{\prime }}_i^w}^T{R}_{wc}{P^{\prime }}_i^c)$$
由于前两项与$R$无关，所以优化目标可以简化为：
$${J^{\prime }}_a(R)=-\sum _{i=1}^n{{P^{\prime }}_i^w}^T{R}_{wc}{P^{\prime }}_i^c=-\sum _{i=1}^{n}tr(R_{wc}{P^{\prime }}_i^c{{P^{\prime }}_i^w}^T)=-tr(R_{wc}\sum _{i=1}^n{P^{\prime }}_i^c{{P^{\prime }}_i^w}^T)$$
令$W={\sum }_{i=1}^n{P^{\prime }}_i^c{{P^{\prime }}_i^w}^T$，对$W$进行SVD分解，即$W=U\Sigma V^T$，$\Sigma$对应的奇异值从大到小排列。 根据最优性证明，当$W$满秩时，最优的$R=U{V}^T$，如果$R$行列式小于0，取负作为最优值。

非线性优化方法

由于ICP存在无穷多解或者唯一解，如果我们使用迭代的方法获取局部最优解，即为全局最优解，因此对于ICP问题，我们可以任意选定初始值并进行迭代。误差的一阶导数即雅克比矩阵为：
$$\frac{\partial e_i}{\partial R}=-(R_{wc}{P}_i^c{)}^{\wedge }=(-P_i^w{)}^{\wedge }，\frac{\partial e_i}{\partial t}=I$$