3D-3D:ICP中SVD用法详解

我学习《视觉SLAM十四讲》中遇到了SVD法求解ICP问题，比较困惑，如何从最小误差中构造SVD分解，如何得到R呐？，在此做出详解。读完这个你就悟了！

1. 问题描述

在ICP算法中，我们有两个点云：

• 源点云 P = { p i } \mathbf{P} = \{\mathbf{p}_i\} P={pi​}，其中 ${\mathbf{p}}_i$ 是第$i$个点的坐标。
• 目标点云 Q = { q i } \mathbf{Q} = \{\mathbf{q}_i\} Q={qi​}，其中${\mathbf{q}}_i$ 是第$i$个点的坐标。

目标是找到一个旋转矩阵 ( R ) 和一个平移向量 ( T )，使得通过这个变换，源点云 ( \mathbf{P} ) 中的点与目标点云$\mathbf{Q}$中的点对齐，即：

${\min }_{R,T}{\sum }_{i=1}^N\Vert {\mathbf{q}}_i-(R{\mathbf{p}}_i+T){\Vert }^2$

2. 去除质心

首先，为了简化问题，我们将两个点云的质心对齐。质心去除的步骤如下：

• 计算源点云的质心${\mathbf{p}}_{\text{centroid}}$和目标点云的质心 ${\mathbf{q}}_{\text{centroid}}$：
  ${\mathbf{p}}_{\text{centroid}}=\frac{1}{N}{\sum }_{i=1}^N{\mathbf{p}}_i$
  ${\mathbf{q}}_{\text{centroid}}=\frac{1}{N}{\sum }_{i=1}^N{\mathbf{q}}_i$
• 将两个点云分别移动到质心为原点的坐标系：
  ${\mathbf{p}}_i^{\prime }={\mathbf{p}}_i-{\mathbf{p}}_{\text{centroid}}$
  ${\mathbf{q}}_i^{\prime }={\mathbf{q}}_i-{\mathbf{q}}_{\text{centroid}}$

此时优化问题变为：

${\min }_R{\sum }_{i=1}^N\Vert {\mathbf{q}}_i^{\prime }-R{\mathbf{p}}_i^{\prime }{\Vert }^2$

3. 误差函数展开

为了最小化误差函数，我们将其展开：

$E(R)={\sum }_{i=1}^N\Vert {\mathbf{q}}_i^{\prime }-R{\mathbf{p}}_i^{\prime }{\Vert }^2$

展开误差函数：
$E(R)={\sum }_{i=1}^N\left[({\mathbf{q}}_i^{\prime }-R{\mathbf{p}}_i^{\prime }{)}^T({\mathbf{q}}_i^{\prime }-R{\mathbf{p}}_i^{\prime })\right]$

进一步展开得到：

$E(R)={\sum }_{i=1}^N\left[{\mathbf{q}}_i^{\prime T}{\mathbf{q}}_i^{\prime }-2{\mathbf{q}}_i^{\prime T}R{\mathbf{p}}_i^{\prime }+{\mathbf{p}}_i^{\prime T}{R}^{T}R{\mathbf{p}}_i^{\prime }\right]$

由于 $R^{T}R=I$（因为 $R$是正交矩阵），所以第三项为${\mathbf{p}}_i^{\prime T}{\mathbf{p}}_i^{\prime }v$。因为我们只关心与 ( R ) 相关的项，所以可以忽略常数项，简化为：

$E(R)=-2{\sum }_{i=1}^N{\mathbf{q}}_i^{\prime T}R{\mathbf{p}}_i^{\prime }$

4. 构建协方差矩阵$H$

最小化误差 ( E® ) 等价于最大化：

${\sum }_{i=1}^N{\mathbf{q}}_i^{\prime T}R{\mathbf{p}}_i^{\prime }$

我们可以将线性项 ${\sum }_{i=1}^N{\mathbf{q}}_i^{\prime T}R{\mathbf{p}}_i^{\prime }$写成矩阵形式：

${\sum }_{i=1}^N{\mathbf{q}}_i^{\prime T}R{\mathbf{p}}_i^{\prime }=\text{trace}\left(R{\sum }_{i=1}^N{\mathbf{p}}_i^{\prime }{\mathbf{q}}_i^{\prime T}\right)$

这里用到了矩阵迹的性质： $\text{trace}(\mathbf{AB})=\text{trace}(\mathbf{BA})$，所以可以重新排列为：

$\text{trace}\left(R{\sum }_{i=1}^N{\mathbf{p}}_i^{\prime }{\mathbf{q}}_i^{\prime T}\right)$

因此，我们可以构建协方差矩阵$H$：

$H={\sum }_{i=1}^N{\mathbf{p}}_i^{\prime }{\mathbf{q}}_i^{\prime T}$

于是，最大化线性项的过程等价于最大化$\text{trace}(RH)$。

几何上，协方差矩阵 ( H ) 捕捉了去质心后的源点云和目标点云的点对之间的分布关系。通过最大化 ( \text{trace}(R H) )，我们实际上是在寻找一个旋转矩阵 ( R )，使得源点云在目标点云坐标系下的投影尽可能地接近。

5. 最优化问题中的 SVD 解法

通过 SVD 分解，我们知道:

$\text{trace}(RH)=\text{trace}(RU\Sigma V^T)$

利用矩阵迹的性质（即$\text{trace}(ABC)=\text{trace}(CAB)=\text{trace}(BCA)$），我们可以将上式改写为：

$\text{trace}(RH)=\text{trace}(RU\Sigma V^T)=\text{trace}(V^{T}RU\Sigma )$

注意到 $\text{trace}(V^{T}RU\Sigma )$ 的最大化主要依赖于矩阵$V^{T}RU$的结构。因为$V^{T}RU$ 是一个正交矩阵，而对于给定的对角矩阵$\Sigma$，当 $V^{T}RU$ 是一个对角矩阵时，迹达到最大值。这个对角矩阵应该是单位矩阵$I$（因为这时各项奇异值直接相加），因此我们可以设置：

$R=V{U}^T$
（视觉SLAM十四讲里面给的是$R=U{V}^T$,我严重怀疑这本书印错了！！！）

6. 结论：用 SVD 解出 $R$

通过上面的推导过程，得出的旋转矩阵 $R$是 $V{U}^T$。这就是为什么在得到$H$之后，通过 SVD 分解$H$ 得到的两个正交矩阵$U$和 $V$，可以通过计算 $R=V{U}^T$得到旋转矩阵的原因。

5. 几何意义

从几何角度来看，矩阵$H$捕捉了两个点集之间的协方差关系。SVD 分解将这个关系解构成旋转和尺度变换（通过奇异值）。通过$R=V{U}^T$的组合，实际上是在寻找最优的旋转矩阵$R$，使得旋转后的点集与原点集的协方差最大程度匹配。

总结

• 最大化$\text{trace}(RH)$：目标是找到一个旋转矩阵 ( R )，使得目标函数最大化。
• SVD 的应用：通过对$H$进行 SVD 分解，获得两个正交矩阵 $U$和$V$，从而构建出最优的旋转矩阵$R=V{U}^T$。
• 几何意义：SVD 解构了点集的协方差关系，通过旋转矩阵最大化了这些点集之间的对齐程度。

拓展：

为什么$V^{T}RU$是正交阵？为什么正交阵和对角阵相乘时候，当正交阵是单位阵时候，正交阵和对角阵乘积的迹最大？
在进行矩阵优化和分解时，尤其是在奇异值分解 (SVD) 和旋转矩阵的上下文中，了解 ( V^T R U ) 是正交矩阵的原因非常重要。以下是详细的解释。

证明1

首先我们来证明 ( V^T R U ) 是正交矩阵。

步骤 1: 计算$(V^{T}RU{)}^T(V^{T}RU)$

$(V^{T}RU{)}^T(V^{T}RU)$

首先计算$(V^{T}RU{)}^T$：

$(V^{T}RU{)}^T=U^T{R}^{T}V$

接下来计算乘积：

$(V^{T}RU{)}^T(V^{T}RU)=(U^T{R}^{T}V)(V^{T}RU)$

利用矩阵乘法的结合律：

$(U^T{R}^{T}V)(V^{T}RU)=U^T(R^T(V{V}^T)R)U$

因为$V$ 是正交矩阵，所以$V{V}^T=I$，因此：

$U^T(R^{T}IR)U=U^T(R^{T}R)U$

由于$R$ 是旋转矩阵，它是正交的，所以 ( R^T R = I )：

$U^{T}IU=I$

所以：

$(V^{T}RU{)}^T(V^{T}RU)=I$

这说明 $V^{T}RU$ 是正交矩阵。

证明2

为了使用公式证明为什么当$A$) 是正交矩阵，$B$ 是对角矩阵时，$\text{tr}(AB)$ 的最大值在 $A$ 为单位矩阵时达到，我们可以通过迹的性质和正交矩阵的特性来推导。

1. 定义和性质

设$A$是一个$n\times n$ 的正交矩阵，$B$ 是一个$n\times n$ 的对角矩阵。我们记$B=\text{diag}(b_1,b_2,\dots ,b_n)$。

正交矩阵$A$满足$A^{T}A=I$，即 ( A ) 的列向量是单位正交的。

2. 迹的定义

矩阵$AB$的迹为：

$\text{tr}(AB)={\sum }_{i=1}^n(AB{)}_{ii}$

其中 $(AB{)}_{ii}$表示矩阵$AB$的第$i$ 个对角元素。

3. 矩阵乘积的迹

迹的一个重要性质是：

$\text{tr}(AB)=\text{tr}(BA)$

因此我们可以等价地考虑$\text{tr}(BA)$，其中 $B$ 是对角矩阵：

$\text{tr}(BA)={\sum }_{i=1}^n(BA{)}_{ii}$

由于$B$ 是对角矩阵，其对角元素为$b_1,b_2,\dots ,b_n$，所以：

$(BA{)}_{ii}=b_i\cdot A_{ii}$

因此：

$\text{tr}(AB)=\text{tr}(BA)={\sum }_{i=1}^n{b}_i\cdot A_{ii}$

4. 最大化$\text{tr}(AB)$

要最大化 $\text{tr}(AB)$，我们需要最大化上式中的每一项，即 $b_i\cdot A_{ii}$。

由于 $A$ 是正交矩阵，满足${\sum }_{i=1}^n{A}_{ij}^2=1$（即每一列的平方和为 1），因此 $A_{ii}$ 的取值范围为$[-1,1]$。

特别地，当$A$是单位矩阵时，$A_{ii}=1$ 对于所有$i$ 成立，因此此时：

$\text{tr}(AB)={\sum }_{i=1}^n{b}_i$

这是可能得到的最大值，因为如果$A$不是单位矩阵，那么$A_{ii}$会小于 1，从而使得每个$b_i\cdot A_{ii}$ 的贡献变小，总和 $\text{tr}(AB)$ 也会减小。

5. 证明总结

1. 迹的计算：$\text{tr}(AB)={\sum }_{i=1}^n{b}_i\cdot A_{ii}$。
2. 优化目标：我们希望最大化 $\text{tr}(AB)$。
3. 单位矩阵的作用：当 $A$是单位矩阵时，$A_{ii}=1$，因此 $\text{tr}(AB)={\sum }_{i=1}^n{b}_i$，达到最大值。
4. 非单位矩阵的影响：如果 $A$不是单位矩阵，则 $A_{ii}$的绝对值小于 1，从而导致 $\text{tr}(AB)$减小。

因此，我们通过公式证明了当 $A$是单位矩阵时，$\text{tr}(AB)$ 达到最大值。