📝笔记：SLAM常见问题(四)：求解ICP，利用SVD分解得到旋转矩阵

今天讲一篇关于利用SVD方法求解ICP问题的文献《Least-Squares Rigid Motion Using SVD》，这篇文章非常精彩地推导出将 $3 D$ 点对齐问题的解析解，同时总结了求解该问题的统一范式。

问题描述

已知 $P = {p_{1}, p_{2}, \dots, p_{n}}$ 以及 $Q = {q_{1}, q_{2}, \dots, q_{n}}$ 是空间中（文中说的更加普适， $p_{i}, q_{i} \in R^{d}$ ，可以表示 $d$ 维空间）的匹配点集，我们试图找到这样的旋转矩阵 $R$ 和平移向量 $t$ 最小化如下对齐误差（即ICP问题的形式）：

(R, t) = R \in S O (d), t \in R^{d} argmin i = 1 \sum n w_{i} ∣ (R p_{i} + t) - q_{i} ∣^{2} (1)

接下来文章分别推导了平移向量 $t$ 以及旋转矩阵 $R$ 的解析解。

计算平移量

此时假定旋转矩阵 $R$ 是固定的，令

F (t) = i = 1 \sum n w_{i} ∣ (R p_{i} + t) - q_{i} ∣^{2}

我们可以通过 $F$ 对 $t$ 求导的方式得到平移量的最优解，如下：

0 = \frac{\partial F}{\partial t} = i = 1 \sum n 2 w_{i} (R p_{i} + t - q_{i}) = 2 t (i = 1 \sum n w_{i}) + 2 R (i = 1 \sum n w_{i} p_{i}) - 2 i = 1 \sum n w_{i} q_{i} (2)

令：

\overline{p} = \frac{\sum _{i = 1}^{n} w _{i} p _{i}}{\sum _{i = 1}^{n} w _{i}}, \overline{q} = \frac{\sum _{i = 1}^{n} w _{i} q _{i}}{\sum _{i = 1}^{n} w _{i}} (3)

于是我们得到 $t$ 的解：

t = \overline{q} - R \overline{p} (4)

从上式看出最优的平移量 $t$ 将 $P$ 点集的加权中心映射到了 $Q$ 点集的中心。接下来将上式带入优化方程，得：

i = 1 \sum n w_{i} ∣ (R p_{i} + t) - q_{i} ∣^{2} = i = 1 \sum n w_{i} ∣ R p_{i} + \overline{q} - R \overline{p} - q_{i} ∣^{2} = i = 1 \sum n w_{i} ∣ R (p_{i} - \overline{p}) - (q_{i} - \overline{q}) ∣^{2} (5)

由此我们将原问题转换成了无平移量的优化问题，令：

x_{i} := p_{i} - \overline{p}, y_{i} := q_{i} - \overline{q} (6)

我们把问题简写成如下形式：

R = R \in S O (d) argmin i = 1 \sum n w_{i} ∣ R x_{i} - y_{i} ∣^{2} (7)

计算旋转量

简化上式：

∣ R x_{i} - y_{i} ∣^{2} = (R x_{i} - y_{i})^{T} (R x_{i} - y_{i}) = (x_{i}^{T} R^{T} - y_{i}^{T}) (R x_{i} - y_{i}) = x_{i}^{T} R^{T} R x_{i} - x_{i}^{T} R^{T} y_{i} - y_{i}^{T} R x_{i} + y_{i}^{T} y_{i} (8)

又因为旋转矩阵的正交性： $R^{T} R = I$ ；另外 $x_{i}^{T} R^{T} y_{i}$ 是标量： $x_{i}$ 维度为 $1 \times d$ ， $R^{T}$ 维度为 $d \times d$ ， $y_{i}$ 维度为 $d \times 1$ 。于是有下式：

x_{i}^{T} R^{T} y_{i} = (x_{i}^{T} R^{T} y_{i})^{T} = y_{i}^{T} R x_{i} (9)

得：

∣ R x_{i} - y_{i} ∣^{2} = x_{i}^{T} x_{i} - 2 y_{i}^{T} R x_{i} + y_{i}^{T} y_{i} (10)

将整理好的上式带入简化后的 $R$ 优化问题，得：

= = R \in S O (d) argmin i = 1 \sum n w_{i} ∣ R x_{i} - y_{i} ∣^{2} = R \in S O (d) argmin i = 1 \sum n w_{i} (x_{i}^{⊤} x_{i} - 2 y_{i}^{⊤} R x_{i} + y_{i}^{⊤} y_{i}) R \in S O (d) argmin (i = 1 \sum n w_{i} x_{i}^{⊤} x_{i} - 2 i = 1 \sum n w_{i} y_{i}^{⊤} R x_{i} + i = 1 \sum n w_{i} y_{i}^{⊤} y_{i}) argmin_{R \in S O (d)} (- 2 i = 1 \sum n w_{i} y_{i}^{⊤} R x_{i}) (11)

接下来将要利用到如下关于迹的技巧:

w_{1} w_{1} ⋱ w_{n} — — — — y_{1}^{T} y_{2}^{T} ⋮ y_{n}^{T} — — — — R ∣ x_{1} ∣ ∣ x_{2} ∣ ∣ \dots ∣ ∣ x_{n} ∣ = — — — — w_{1} y_{1}^{T} w_{2} y_{2}^{T} ⋮ w_{n} y_{n}^{T} — — — — ∣ R x_{1} ∣ ∣ R x_{2} ∣ ∣ \dots ∣ ∣ R x_{n} ∣ = w_{1} y_{1}^{T} R x_{1} * w_{2} y_{2}^{T} R x_{2} ⋱ * w_{n} y_{n}^{T} R x_{n}

上式就是对

i = 1 \sum n w_{i} y_{i}^{⊤} R x_{i} = tr (W Y^{T} R X)

的完美解释。

利用上式，式 $(11)$ 可以整理得：

R \in S O (d) argmin (- 2 i = 1 \sum n w_{i} y_{i}^{⊤} R x_{i}) = R \in S O (d) argmax (i = 1 \sum n w_{i} y_{i}^{⊤} R x_{i}) = R \in S O (d) argmax tr (W Y^{T} R X) (12)

这里说明一下维度： $W = diag (w_{1}, w_{2}, ..., w_{n})$ 维度为 $n \times n$ ， $Y^{T}$ 维度为 $n \times d$ ， $R$ 维度为 $d \times d$ ， $X$ 维度为 $d \times n$ 。

接下来回顾一下迹的性质： $tr (A B) = tr (B A)$ ，因此有下式：

tr (W Y^{T} R X) = tr ((W Y^{T}) (R X)) = tr (R X W Y^{T}) (13)

令 $d \times d$ 的”covariance”矩阵 $S = X W Y^{T}$ ，求 $S$ 的SVD分解：

S = U Σ V^{T} (14)

于是式 $(13)$ 变为：

tr (W Y^{T} R X) = tr (R S) = tr (R U Σ V^{T}) = tr (Σ V^{T} R U) (15)

由于 $V, U, R$ 均为正交矩阵，因此 $M = V^{T} R U$ 也是正交阵，也就是说 $M$ 的列向量 $m_{j}$ 是互相正交的单位向量，即：

m_{j}^{T} m_{j} = 1

于是：

1 = m_{j}^{⊤} m_{j} = i = 1 \sum d m_{ij}^{2} \Rightarrow m_{ij}^{2} \leq 1 \Rightarrow ∣ m_{ij} ∣ \leq 1 (16)

由于SVD分解的性质可知 $σ$ 的元素均为非负数： $σ_{1}, σ_{2}, ..., σ_{d} \geq 0$ ，于是式 $(17)$ 变为如下形式：

tr (Σ M) = σ_{1} σ_{2} ⋱ σ_{d} m_{11} m_{21} ⋮ m_{d 1} m_{12} m_{22} ⋮ m_{d 2} \dots \dots ⋮ \dots m_{1 d} m_{2 d} ⋮ m_{dd} = i = 1 \sum d σ_{i} m_{ii} \leq i = 1 \sum d σ_{i} (17)

可见，当迹最大时 $m_{ii} = 1$ ，又由于 $M$ 是正交阵，这使得 $M$ 为单位阵！

I = M = V^{T} R U \Rightarrow R = V U^{T} (18)

看到没，R的解析解竟然如此简单，并且与SVD分解产生了联系，让人感觉到了数学的美妙。不过到这里还没完，后面作者进行了一步方向矫正，大意是这样的：利用公式 $(18)$ 得到的矩阵并不一定是一个旋转矩阵，也可能为反射矩阵，此时可以通过验证 $V U^{T}$ 的行列式来判断到底是旋转（行列式 = 1）还是反射（行列式 = -1）。但我们要求的是旋转矩阵，这时需要对公式 $(18)$ 进行一步处理。

假设 $det (V U^{T}) = - 1$ ，则限制 $R$ 为旋转就意味着 $M = V^{T} R U$ 为反射矩阵，于是我们试图找到一个反射矩阵 $M$ 最大化下式：

tr (Σ M) = σ_{1} m_{11} + σ_{2} m_{22} + ... + σ_{d} m_{dd} := f (m_{11}, m_{22}, ..., m_{dd}) (19)

即 $f$ 是以 $m_{11}, m_{22}, ..., m_{dd}$ 为变量的线性函数，由于 $m_{ii} \in [- 1, 1]$ ，其极大值肯定在其定义域的边界处。于是当 $\forall i, m_{ii} = 1$ 时， $f$ 取得极大值，但是此时的 $R$ 为反射矩阵，所以并不能这样取值。然后我们看第二个极大值点 $(1, 1, ..., - 1)$ ，有：

f = tr (Σ M) = σ_{1} + σ_{2} + ... + σ_{d - 1} - σ_{d} (20)

这个值大于任何其它的自变量取值 $(\pm 1, \pm 1, ..., \pm 1)$ 的组合（除了 $(1, 1, ..., 1)$ ），因为奇异值是经过排序的， $σ_{d}$ 是最小的一个奇异值。

综上，为了将解转换为旋转矩阵要进行如下处理：

R = V 1 ⋱ 1 det (V U^{⊤}) U^{⊤} (21)

可以总结的套路

为了得到ICP问题的最优解，我们可以采取如下套路：

step1. 计算两组匹配点的加权中心：

\overline{p} = \frac{\sum _{i = 1}^{n} w _{i} p _{i}}{\sum _{i = 1}^{n} w _{i}}, \overline{q} = \frac{\sum _{i = 1}^{n} w _{i} q _{i}}{\sum _{i = 1}^{n} w _{i}}

step2. 得到去中心化的点集：

x_{i} := p_{i} - \overline{p}, y_{i} := q_{i} - \overline{q}, i = 1, 2... n

step3. 计算 $d \times d$ 的covariance矩阵：

S = X W Y^{T}

其中， $X, Y$ 为 $d \times n$ 的矩阵， $x_{i}, y_{i}$ 分别是它们的列元素，另外 $W = diag (w_{1}, w_{2}, ..., w_{n})$ 。

step4. 对 $S$ 进行SVD分解 $S = U Σ V^{T}$ ，得到旋转矩阵：

R = V 1 ⋱ 1 det (V U^{⊤}) U^{⊤}

step5. 计算平移量：

t = \overline{q} - R \overline{p}