Background

ICP（Iterative Closest Point）问题，迭代最近点。已知一组三维点在两个坐标系中的坐标表示，求这两个坐标系之间的变换关系，称为ICP问题。

最开始想到这个问题，是想进行手眼标定，有一台机械臂以及一个深度相机，如何确定相机坐标系和机械臂坐标系之间的变换关系？后来想使用接口将机械臂末端移动至某个位姿，在深度相机图像中标出该点位置（通过专门的标注工具），这样得到了一个三维点在两个坐标系下的表示，这实际构建了一个方程组。设变换矩阵为$T$，点在两个坐标系下的表示为$p,p^{\prime }$，则方程组可表示为$p=T\times p^{\prime }$。

一维情形只需要一个点，就可以基本确定两个坐标系的关系；二维情形需要两个点，只有一个点的话，两个坐标系可以绕该点进行旋转；进而推之，三维情形至少需要三个点。但是，采样过程是存在误差的，因此需要增加数据点控制误差。

ICP问题也常常在SLAM和无人驾驶的研究中出现，也称为3D点云之间的匹配问题，传感器外参的标定问题。

SVD求解ICP问题

有两组点 P = { p 1 , p 2 , . . . , p n } P=\{p_1,p_2,...,p_n\} P={p1​,p2​,...,pn​}和 P ′ = { p 1 ′ , p 2 ′ , . . . , p n ′ } P'=\{p_1',p_2',...,p_n'\} P′={p1′​,p2′​,...,pn′​}，需要找到到一组参数 { R , t } \{R,t\} {R,t}表示这两组点之间“最有可能的变换关系”，可以构建最小二乘问题如下

$$\begin{gathered} mi{n}_{R,t}J=\frac{1}{2}\sum _{i=1}^n||p_i-(R{p}_i^{\prime }+t)|{|}^2 \\ s.t.R^{T}R=I \end{gathered}$$

Solution

首先，去质心坐标；
$p=\frac{1}{n}{\sum }_{i=1}^n{p}_i,p^{\prime }=\frac{1}{n}{\sum }_{i=1}^n{p}_i^{\prime }$
$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$

误差函数化简如下
$J=\frac{1}{2}{\sum }_{i=1}^n||p_i-(R{p}_i^{\prime }+t)|{|}^2$
$=\frac{1}{2}{\sum }_{i=1}^n||p_i-(R{p}_i^{\prime }+t)-p+R{p}^{\prime }+p-R{p}^{\prime }|{|}^2$
$=\frac{1}{2}{\sum }_{i=1}^n||(q_i-R{q}_i^{\prime })+(p-R{p}^{\prime }-t)|{|}^2$
$=\frac{1}{2}{\sum }_{i=1}^n(||q_i-R{q}_i^{\prime }|{|}^2+||p-R{p}^{\prime }-t|{|}^2+2(q_i-R{q}_i^{\prime })(p-R{p}^{\prime }-t))$

$=\frac{1}{2}{\sum }_{i=1}^n(||q_i-R{q}_i^{\prime }|{|}^2+||p-R{p}^{\prime }-t|{|}^2)+(p-R{p}^{\prime }-t){\sum }_{i=1}^n(q_i-R{q}_i^{\prime })$

根据定义可得最后一项为0.

${\sum }_{i=1}^n(q_i-R{q}_i^{\prime })={\sum }_{i=1}^n{q}_i-R({\sum }_{i=1}^n{q}_i^{\prime })={\sum }_{i=1}^n(p_i-p)-R({\sum }_{i=1}^n(p_i^{\prime }-p^{\prime }))=0$

因此我们有

$J=\frac{1}{2}{\sum }_{i=1}^n(||q_i-R{q}_i^{\prime }|{|}^2+||p-R{p}^{\prime }-t|{|}^2)$

令$t=p-R{p}^{\prime }$可以使第二项为0，同时减少参数$t$，得到

$R^{\ast }=ar{g}_{R}min\frac{1}{2}{\sum }_{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2$

利用约束条件$R^{T}R=I$，继续化简：

$R^{\ast }=ar{g}_{R}min\frac{1}{2}{\sum }_{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2$

$=ar{g}_{R}min\frac{1}{2}{\sum }_{i=1}^n(q_i^T{q}_i+q_i^{\prime T}{R}^{T}R{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$

$=ar{g}_{R}min\frac{1}{2}{\sum }_{i=1}^n(q_i^T{q}_i+q_i^{\prime T}{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$

$=ar{g}_{R}min\frac{1}{2}{\sum }_{i=1}^n(q_i^T{q}_i+q_i^{\prime T}{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$

$=ar{g}_{R}max{\sum }_{i=1}^n{q}_i^{T}R{q}_i^{\prime }$

利用$a^{T}b=tr(b{a}^T)$继续变形，得

${\sum }_{i=1}^n{q}_i^T(R{q}_i^{\prime })={\sum }_{i=1}^{n}tr(R{q}_i^{\prime }{q}_i^T)=tr(R{\sum }_{i=1}^n{q}_i^{\prime }{q}_i^T)$

定义$W={\sum }_{i=1}^n{q}_i{q}_i^{\prime T}$，至此ICP问题转化为
$$\begin{gathered} maxtr(R{W}^T) \\ s.t.R^{T}R=I \end{gathered}$$

Method A

对矩阵$W$进行奇异值分解（SVD）得到$W=U\Sigma V^T$，根据SVD的定义，$U$和$V$均是$3\times 3$的正交矩阵，根据约束，$R$矩阵也是。
$$tr(R{W}^T)=tr(RV\Sigma U^T)=tr(S\Sigma U^T)$$
其中$S=UV$，$S{S}^T=UV(UV{)}^T=UV{V}^T{U}^T=I$也是正交矩阵（两个正交矩阵相乘结果仍是正交矩阵）。

将$S$和$V$展开写成3个列向量的组合的形式：

$tr(S\Sigma U^T)=tr({\sigma }_1{s}_1{u}_1^T+{\sigma }_2{s}_2{u}_2^T+{\sigma }_3{s}_3{u}_3^T)$

$=s_1{u}_1^T{\sigma }_1+s_2{u}_2^T{\sigma }_2+s_3{u}_3^T{\sigma }_3$

由于$S$和$U$均是正交矩阵，有

$||s_1||=||s_2||=||s_3||=||u_1||=||u_2||=||u_3||=1$

因此，满足不等式

$s_1{u}_1^T{\sigma }_1+s_2{u}_2^T{\sigma }_2+s_3{u}_3^T{\sigma }_3\le {\sigma }_1+{\sigma }_2+{\sigma }_3$

当$\forall s_i,u_i$同向时，等式成立。此时，$S=U$，即$RV=U$，$R=U{V}^T$

因此，我们得到$R$的最优解就是$W$矩阵SVD得到两个正交矩阵$U$和$V^T$的乘积。

Method B

这个问题形式和PCA十分相近，PCA最终转化得到问题是
$$\begin{gathered} ma{x}_u{u}^{T}Su \\ s.t.u^{T}u=1 \end{gathered}$$
这个可以通过拉格朗日乘子法求解，对上面的问题套用。

设拉格朗日函数为$\mathcal{L}(R,\lambda )=tr(R{\sum }_{i=1}^n{q}_i^{\prime }{q}_i^T)-\lambda (R^{T}R-I)$，

对$R$求导得$\nabla \mathcal{L}(R,\lambda )={\sum }_{i=1}^n{q}_i{q}_i^{\prime T}-2\lambda R$，令$\nabla =0$，得到

（矩阵求导${\nabla }_{A}tr(AB)=B^T,{\nabla }_A(A^{T}A)=2A$）

$R=\frac{1}{2\lambda }{\sum }_{i=1}^n{q}_i{q}_i^{\prime T}=\frac{1}{2\lambda }W=\frac{1}{2\lambda }U\Sigma V^T$

代入$R^{T}R=I$得到

$\frac{1}{4{\lambda }^2}U\Sigma V^T(U\Sigma V^T{)}^T=U\frac{{\Sigma }^2}{4{\lambda }^2}{U}^T=I$

利用$U$为正定矩阵的性质得$\frac{{\Sigma }^2}{4{\lambda }^2}=I$，$\Sigma =2\lambda I$，

因此，$R=\frac{1}{2\lambda }U\Sigma V^T=\frac{1}{2\lambda }U(2\lambda I)V^T=U{V}^T$

通过拉格朗日乘子法可以得到相同的结果。

Reference

用SVD求解ICP问题的完整证明：https://zhuanlan.zhihu.com/p/108858766

SVD求解ICP问题：https://zhuanlan.zhihu.com/p/188668384