3D-3D：ICP

假设有一组配对好的3D点(例如对两幅RGB-D图像进行了匹配)：

P s = { p s 1 , ⋯   , p s n } , P t = { t p t 1 , ⋯   , p t n } \boldsymbol{P}_s=\{\boldsymbol{p}_{s_1},\cdots,\boldsymbol{p}_{s_n}\},\quad \boldsymbol{P}_t=\{{}^t\boldsymbol{p}_{t_1},\cdots,\boldsymbol{p}_{t_n}\} Ps​={ps1​​,⋯,psn​​},Pt​={tpt1​​,⋯,ptn​​}
现在想要找一个欧式变换$\mathbf{R},\mathbf{t}$，使得
$$\forall i,{\mathbf{p}}_{t_i}=\mathbf{R}{\mathbf{p}}_{s_i}+\mathbf{t}$$
由于噪声等的影响，一般上式无法全部同时成立，因此转化为最小二乘优化问题求解

SVD分解求解

首先定义第$i$对点的误差项：
$${\mathbf{e}}_i={\mathbf{p}}_{t_i}-\left(\mathbf{R}{\mathbf{p}}_{s_i}+\mathbf{t}\right)$$
然后，构建最小二乘问题，求使误差平方和达到极小的$\mathbf{R},\mathbf{t}$：
$$\underset{\mathbf{R},\mathbf{t}}{\min }\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{p}}_{t_i}-\left(\mathbf{R}{\mathbf{p}}_{s_i}+\mathbf{t}\right){\Vert }_2^2$$
定义两组点的质心：
$${\mathbf{p}}_s=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_{s_i}{\mathbf{p}}_t=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_{t_i}$$
对误差函数做如下处理：
$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{p}}_{t_i}-\left(\mathbf{R}{\mathbf{p}}_{s_i}+\mathbf{t}\right)\Vert & =\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_{t_i}-\mathbf{R}{\mathbf{p}}_{s_i}-\mathbf{t}+(\mathbf{R}{\mathbf{p}}_s-{\mathbf{p}}_t)-(\mathbf{R}{\mathbf{p}}_s-{\mathbf{p}}_t)\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n{\Vert \left[{\mathbf{p}}_{t_i}-{\mathbf{p}}_t-\mathbf{R}\left({\mathbf{p}}_{s_i}-{\mathbf{p}}_s\right)\right]+\left({\mathbf{p}}_t-\mathbf{R}{\mathbf{p}}_s-\mathbf{t}\right)\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n\left\{{\Vert {\mathbf{p}}_{t_i}-{\mathbf{p}}_t-\mathbf{R}\left({\mathbf{p}}_{s_i}-{\mathbf{p}}_s\right)\Vert }^2+{\Vert {\mathbf{p}}_t-\mathbf{R}{\mathbf{p}}_s-\mathbf{t}\Vert }^2+2{\left[{\mathbf{p}}_{t_i}-{\mathbf{p}}_t-\mathbf{R}\left({\mathbf{p}}_{s_i}-{\mathbf{p}}_s\right)\right]}^T\left({\mathbf{p}}_t-\mathbf{R}{\mathbf{p}}_s-\mathbf{t}\right)\right\}\end{array}$$
注意交叉项部分中$\left[{\mathbf{p}}_{t_i}-{\mathbf{p}}_t-\mathbf{R}\left({\mathbf{p}}_{s_i}-{\mathbf{p}}_s\right)\right]$在求和后为零，因此目标函数可以简化为：
$$\underset{\mathbf{R},\mathbf{t}}{\min }J=\frac{1}{2}\sum _{i=1}^n\left[{\Vert {\mathbf{p}}_{t_i}-{\mathbf{p}}_t-\mathbf{R}\left({\mathbf{p}}_{s_i}-{\mathbf{p}}_s\right)\Vert }^2+{\Vert {\mathbf{p}}_t-\mathbf{R}{\mathbf{p}}_s-\mathbf{t}\Vert }^2\right]$$
仔细观察左右两项，发现左边只与旋转矩阵$\mathbf{R}$有关，而右边既有$\mathbf{R}$又有$\mathbf{t}$，但只与质心相关，只要求得最优的${\mathbf{R}}^{\ast }$，令第二项为零就能得到$\mathbf{t}$。

1. 计算两组点的质心位置${\mathbf{p}}_s,{\mathbf{p}}_t$，然后计算每个点的去质心坐标：
   $${\mathbf{q}}_{s_i}={\mathbf{p}}_{s_i}-{\mathbf{p}}_s{\mathbf{q}}_{t_i}={\mathbf{p}}_{t_i}-{\mathbf{p}}_t$$
2. 根据以下优化问题计算旋转矩阵：
   $${\mathbf{R}}^{\ast }=\arg \underset{\mathbf{R}}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{q}}_{t_i}-\mathbf{R}{\mathbf{q}}_{s_i}\Vert }^2$$
3. 令第二项为零计算出$\mathbf{t}$：
   $${\mathbf{t}}^{\ast }={\mathbf{p}}_t-\mathbf{R}{\mathbf{p}}_s$$

展开关于$\mathbf{R}$的误差项，得
$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{q}}_{t_i}-{\mathbf{Rq}}_{s_i}\Vert }^2& =\frac{1}{2}\sum _{i=1}^n\left({\mathbf{q}}_{t_i}^T{\mathbf{q}}_{t_i}+{\mathbf{q}}_{s_i}^T{\mathbf{R}}^T\mathbf{R}{\mathbf{q}}_{s_i}-2{\mathbf{q}}_{t_i}^T\mathbf{R}{\mathbf{q}}_{s_i}\right)\\ \sum _{i=1}^n-{\mathbf{q}}_{t_i}^T\mathbf{R}{\mathbf{q}}_{s_i}& =\sum _{i=1}^n-\mathrm{tr}\left({\mathbf{Rq}}_{s_i}{\mathbf{q}}_{t_i}^T\right)=-\mathrm{tr}\left(\mathbf{R}\sum _{i=1}^n{\mathbf{q}}_{s_i}{\mathbf{q}}_{t_i}^T\right)=-\mathrm{tr}\left({\mathbf{RW}}^T\right)\end{array}$$
定义矩阵$\mathbf{W}={\sum }_{i=1}^n{\mathbf{q}}_{s_i}{\mathbf{q}}_{t_i}^T$，对$\mathbf{W}$进行SVD分解，得
$$\mathbf{W}={\mathbf{U}\mathbf{\Sigma }\mathbf{V}}^T$$
由最优性定理¹可得：
$$\begin{array}{rl}\underset{\mathbf{R}}{\max }\mathrm{tr}\left({\mathbf{RW}}^T\right)& =\underset{\mathbf{R}}{\max }\mathrm{tr}\left({\mathbf{RV}\mathbf{\Sigma }\mathbf{U}}^T\right)\\ & =\underset{\mathbf{R}}{\max }\mathrm{tr}\left[\underset{正交矩阵}{\underbrace{\left({\mathbf{RVU}}^T\right)}}\underset{正半定矩阵}{\underbrace{\left({\mathbf{U}\mathbf{\Sigma }\mathbf{U}}^T\right)}}\right]=\mathrm{tr}\left({\mathbf{U}\mathbf{\Sigma }\mathbf{U}}^T\right)\end{array}$$
只有当${\mathbf{RVU}}^T=\mathbf{I}$，即$\mathbf{R}={\mathbf{UV}}^T$时，括号内变为正半定矩阵，取得最大值。如果此时$\mathbf{R}$的行列式为负，则取$-\mathbf{R}$作为最优值。

另一种更简单的证明方法：
$$\begin{array}{rl}\underset{\mathbf{R}}{\max }\mathrm{tr}\left({\mathbf{RW}}^T\right)& =\underset{\mathbf{R}}{\max }\mathrm{tr}\left({\mathbf{RV}\mathbf{\Sigma }\mathbf{U}}^T\right)\mathrm{assume}\mathbf{S}=\mathbf{RV}\\ & =\underset{\mathbf{R}}{\max }\mathrm{tr}\left(\mathbf{S}\mathbf{\Sigma }{\mathbf{U}}^T\right)\end{array}$$
$$\begin{array}{rl}\mathrm{tr}({\mathbf{S}\mathbf{\Sigma }\mathbf{U}}^T)& =\mathrm{tr}(\left[\begin{array}{ccc}{\mathbf{s}}_1& {\mathbf{s}}_2& {\mathbf{s}}_3\end{array}\right]\left[\begin{array}{ccc}{\sigma }_1& & \\ & {\sigma }_2& \\ & & {\sigma }_3\end{array}\right]\left[\begin{array}{c}{\mathbf{u}}_1^T\\ {\mathbf{u}}_2^T\\ {\mathbf{u}}_3^T\end{array}\right])\\ & =\mathrm{tr}({\sigma }_1{\mathbf{s}}_1{\mathbf{u}}_1^T+{\sigma }_2{\mathbf{s}}_2{\mathbf{u}}_2^T+{\sigma }_3{\mathbf{s}}_3{\mathbf{u}}_3^T)\\ & ={\sigma }_1{\mathbf{u}}_1^T{\mathbf{s}}_1+{\sigma }_2{\mathbf{u}}_2^T{\mathbf{s}}_2+{\sigma }_3{\mathbf{u}}_3^T{\mathbf{s}}_3\end{array}$$
由于$\mathbf{S}$和$\mathbf{U}$ 都是$3\times 3$的正交矩阵，因此
$$\Vert {\mathbf{s}}_1\Vert =\Vert {\mathbf{s}}_2\Vert =\Vert {\mathbf{s}}_3\Vert =\Vert {\mathbf{u}}_1\Vert =\Vert {\mathbf{u}}_2\Vert =\Vert {\mathbf{u}}_3\Vert =1$$
进而
$${\sigma }_1{\mathbf{u}}_1^T{\mathbf{s}}_1+{\sigma }_2{\mathbf{u}}_2^T{\mathbf{s}}_2+{\sigma }_3{\mathbf{u}}_3^T{\mathbf{s}}_3⩽{\sigma }_1+{\sigma }_2+{\sigma }_3$$
当且仅当${\mathbf{s}}_1$与${\mathbf{u}}_1$ 同方向、${\mathbf{s}}_2$与${\mathbf{u}}_2$ 同方向、${\mathbf{s}}_3$与${\mathbf{u}}_3$ 同方向时等号成立，此时$\mathbf{S}=\mathbf{U}$ ，即$\mathbf{RV}=\mathbf{U}$ ,$\mathbf{R}={\mathbf{UV}}^T$

非线性优化求解

$$\underset{\mathbf{\xi }}{\min }\frac{1}{2}\sum _{i=1}^n\Vert ({\mathbf{p}}_{t_i}-\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{p}}_{s_i}{\Vert }_2^2$$
单个误差项关于位姿的求导参考²，使用李代数扰动模型：
$$\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=-{\left(\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{p}}_{s_i}\right)}^{\odot }$$

---

1. 定理：一个正交矩阵$\mathbf{B}$乘以一个正半定矩阵${\mathbf{AA}}^T$的迹小于等于正半定矩阵${\mathbf{AA}}^T$的迹
   $$\begin{gathered} \mathrm{tr}\left({\mathbf{AA}}^T\right)⩾\mathrm{tr}\left({\mathbf{BAA}}^T\right)\forall \mathbf{B}\in {\mathbb{U}}^{3\times 3} \\ \mathrm{tr}\left({\mathbf{BAA}}^T\right)=\mathrm{tr}\left({\mathbf{A}}^T\mathbf{BA}\right)=\sum _i{\mathbf{a}}_i^T{\mathbf{Ba}}_i⩽\sum _i\sqrt{\left({\mathbf{a}}_i^T{\mathbf{a}}_i\right)\left({\mathbf{a}}_i^T{\mathbf{B}}^T{\mathbf{Ba}}_i\right)} \\ =\sum _i{\mathbf{a}}_i^T{\mathbf{a}}_i=\mathrm{tr}\left({\mathbf{A}}^T\mathbf{A}\right)=\mathrm{tr}\left({\mathbf{AA}}^T\right) \end{gathered}$$ ↩︎

2. 李群李代数求导 ↩︎