雷达系列论文翻译（二）：Least-Squares Rigid Motion Using SVD

Least-Squares Rigid Motion Using SVD

摘要

本注释总结了计算对齐两组对应点的最优的刚体变换的步骤。

问题陈述

令 P = { p 1 , p 2 , . . . , p n } \mathcal{P} = \{p_1,p_2,...,p_n\} P={p1​,p2​,...,pn​}和 Q = { q 1 , q 2 , . . . , q n } \mathcal{Q}=\{q_1,q_2,...,q_n\} Q={q1​,q2​,...,qn​}是$R^d$空间内的两组对应的点。我们希望找到一个刚性的变换，在最小二乘的意义上最优地对齐两个点集，也就是说，我们寻求一个旋转$R$和一个平移向量$t$来满足
$$(R,t)=\underset{R\in SO(d),t\in R^d}{\mathrm{argmin}}\sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2\text{\hspace{1em}(1)}$$
其中$w_i>0$是每个点对的权重。
在下文中，我们详细介绍了$R$和$t$的推导过程；对最终结果感兴趣的读者可以跳过证明，直接进入第4节。

计算平移

假设$R$被固定并定义$F(t)={\sum }_{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2$。我们可以通过取$F(t)$相对于$t$的导数并寻找它的根来找到最优平移:
$$\begin{gathered} 0=\frac{\partial F}{\partial t}=\sum _{i=1}^{n}2w_i(R{p}_i+t-q_i) \\ =2t(\sum _{i=1}^n{w}_i)+2R(\sum _{i=1}^n{w}_i{p}_i)-2\sum _{i=1}^n{w}_i{q}_i\text{\hspace{1em}(2)} \end{gathered}$$
定义
$$\bar{p}=\frac{{\sum }_{i=1}^n{w}_i{p}_i}{{\sum }_{i=1}^n{w}_i},\bar{q}=\frac{{\sum }_{i=1}^n{w}_i{q}_i}{{\sum }_{i=1}^n{w}_i}\text{\hspace{1em}(3)}$$
通过重新排列公式(2)的项，我们得到
$$t=\bar{q}-R\bar{p}\text{\hspace{1em}(4)}$$
换句话说，最佳平移$t$将被变换的点集$p$的加权质心映射到点集$\mathcal{Q}$的加权质心。让我们将最优的$t$插入我们的目标函数:
$$\begin{gathered} \sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2=\sum _{i=1}^n{w}_i||R{p}_i+\bar{q}-R\bar{p}-q_i|{|}^2 \\ =\sum {i=1}^n{w}_i||R(p_i-\bar{p})-(q_i-\bar{q})|{|}^2\text{\hspace{1em}(5)} \end{gathered}$$

因此，我们可以集中精力计算旋转$R$。通过重构这个问题，使得平移为零:
$$x_i=p_i-\bar{p},y_i=q_i-\bar{q}\text{\hspace{1em}(6)}$$
所以我们寻找最佳旋转$R$，使其满足
$$R=\underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2\text{\hspace{1em}(7)}$$

计算旋转

让我们简化公式(7)中试图最小化的表达式:
$$\begin{gathered} ||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-y_i^{T}R{x}_i-x_i^T{R}^T{y}_i+y_i^T{y}_i \\ =x_i^T{x}_i-y_i^{T}R{x}_i-x_i^T{R}^T{y}_i+y_i^T{y}_i\text{\hspace{1em}(8)} \end{gathered}$$
我们通过旋转矩阵的正交性（$R{R}^T=R^{T}R=I$）来完成最后一步。

请注意，$x_i^T{R}^T{y}_i$是一个标量：$x_i^T$维度为$1\times d$，$R^T$维度为$d\times d$并且$y_i$的维度为$d\times 1$。对于任何标量$a$，我们通常都有$a=a^T$，因此
$$x_i^T{R}^T{y}_i=(x_i^T{R}^T{y}_i{)}^T=y_i^{T}R{x}_i\text{\hspace{1em}(9)}$$
因此我们有
$$||R{x}_i-y_i|{|}^2=x_i^T{x}_i-2y_i^{T}R{x}_i+y_i^T{y}_i\text{\hspace{1em}(10)}$$
让我们看看要最小化的代价函数，并使用上面的表达式进行替换:
$$\begin{gathered} \underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2=\underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i(x_i^T{x}_i-2y_i^{T}R{x}_i+y_i^T{y}_i) \\ =\underset{R\in SO(d)}{\mathrm{argmin}}(\sum _{i=1}^n{w}_i{x}_i^T{x}_i-2\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i+\sum _{i=1}^n{w}_i{y}_i^T{y}_i) \\ =\underset{R\in SO(d)}{\mathrm{argmin}}(-2\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i)\text{\hspace{1em}(11)} \end{gathered}$$
最后一步(移除${\sum }_{i=1}^n{w}_i{x}_i^T{x}_i$ 和${\sum }_{i=1}^n{w}_i{y}_i^T{y}_i$)成立，因为这些表达式根本不依赖于$R$，所以移除它们不会影响最小值。最小化表达式乘以标量也是如此，所以我们有
$$\underset{R\in SO(d)}{\mathrm{argmin}}(-2\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i)=\underset{R\in SO(d)}{\mathrm{argmax}}\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i\text{\hspace{1em}(12)}$$
我们定义
$$\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i=tr(W{Y}^{T}RX)\text{\hspace{1em}(13)}$$

其中$W=diag(w_1,w_2,...,w_n)$是带加权对角元素$w_i$的$n\times n$对角矩阵；$Y$是一个以$y_i$为列的$d\times n$矩阵，$X$是一个以$x_i$为列的$d\times n$矩阵。我们提醒读者，方阵的迹是对角线上元素的和：$tr(A)={\sum }_{i=1}^n{a}_{ii}$。参见图1来了解代数操作的说明。

因此，我们正在寻找一个最大化$tr(W{Y}^{T}RX)$的旋转$R$。矩阵迹具有性质
$$tr(AB)=tr(BA)\text{\hspace{1em}(14)}$$
对于任何矩阵维度允许的矩阵$A$和$B$。因此
$$tr(W{Y}^{T}RX)=tr((W{Y}^T)(RX))=tr(RXW{Y}^T)\text{\hspace{1em}(15)}$$
让我们定义$d\times d$的协方差矩阵$S=XW{Y}^T$。对$S$进行SVD分解：
$$S=U\Sigma V^T\text{\hspace{1em}(16)}$$
现在将分解替换到我们试图最大化的代价函数中：
$$tr(RXW{Y}^T)=tr(RS)=tr(RU\Sigma V^T)=tr(\Sigma V^{T}RU)\text{\hspace{1em}(17)}$$
最后一步是使用迹的属性(14)实现的。注意$V$,$R$和$U$都是正交矩阵，所以$M=V^{T}RU$也是正交矩阵。这意味着$M$的列是正交向量，特别是，对于每个$M$的列$m_j$，$m_j^T{m}_j=1$。因此$M$的所有元素$m_{ij}\le 1$：
$$1=m_j^T{m}_j=\sum _{i=1}^d{m}_{ij}^2\to m_{ij}^2\le 1\to |m_{ij}|\le 1\text{\hspace{1em}(18)}$$
那么$tr(\Sigma M)$的最大可能值是多少？请记住，$\Sigma$是一个对角矩阵，具有非负元素${\sigma }_1,{\sigma }_2,...,{\sigma }_d\ge 0$。因此：
$$tr(\Sigma M)=\left(\begin{array}{cccc}{\sigma }_1& & & \\ & {\sigma }_2& & \\ & & \ddots & \\ & & & {\sigma }_d\end{array}\right)\left(\begin{array}{cccc}{m}_{11}& m_{12}& \cdots & m_{1d}\\ m_{21}& m_{22}& \cdots & m_{2d}\\ ⋮& ⋮& ⋮& ⋮\\ m_{d1}& m_{d2}& \cdots & m_{dd}\end{array}\right)=\sum _{i=1}^d{\sigma }_i{m}_{ii}\le \sum _{i=1}^d{\sigma }_i\text{\hspace{1em}(19)}$$
因此，如果$m_{ii}=1$，则迹最大化。因为$M$是正交矩阵，这意味着$M$必须是单位矩阵！
$$I=M=V^{T}RU\to V=RU\to R=V{U}^T\text{\hspace{1em}(20)}$$

旋转反射

我们刚才描述的过程找到了最优的正交矩阵，除了旋转之外，它还可能包含反射。想象一下，点集$\mathcal{P}$是点集合$\mathcal{Q}$的完美反射。然后我们会发现反射完美地对齐了两个点集，并在公式(7)中产生零代价，在这种情况下它是全局最小值。然而，如果我们严格要求它是一个旋转，可能就不会有一个完美对齐点集的旋转。

检查是$R=V{U}^T$否是旋转很简单:如果$det(V{U}^T)=-1$，则它包含一个反射，否则$det(V{U}^T)=+1$。假设$det(V{U}^T)=1$。限制$R$为旋转等同于限制$M$为反射。我们现在想找到一个反射来最大化:
$$tr(\Sigma M)={\sigma }_1{m}_{11}+{\sigma }_2{m}_{22}+...+{\sigma }_d{m}_{dd}=f(m_{11},m_{22},...,m_{dd})\text{\hspace{1em}(21)}$$
请注意，$f$只取决于$M$的对角线，而不是它的其他元素。我们现在考虑$m_{ii}$作为变量$(m_{11}，...，m_{dd})$。这是$n$阶反射矩阵的所有对角线元素的集合。令人惊讶的是，它的结构非常简单。事实上，A.Horn[1]的一个结果表明，$n$阶旋转矩阵的所有对角线的集合等于点集$(\pm 1，...，\pm 1)$的凸包，偶数个坐标为$-1$。因为任何反射矩阵都可以通过反转旋转矩阵一行的符号来构造，反之亦然，所以我们优化的集合是具有非偶数个$-1$的点集$(\pm 1，...，\pm 1)$的凸包。

由于我们的定义域是凸多面体，所以线性函数$f$在其顶点处有极值。对角线$(1，1，...，1)$不在域中，因为它有偶数个$-1$(即零)，因此下一个最佳选择是$(1，1，...,1,-1)$：
$$tr(\Sigma M)={\sigma }_1+{\sigma }_2+...+{\sigma }_{d-1}-{\sigma }_d\text{\hspace{1em}(22)}$$
这个值是在我们的定义域的一个顶点上得到的，并且比形式$(\pm 1，...，\pm 1)$除了$(1，1，...，1)$的组合更大，因为${\sigma }_d$是最小的奇异值。

总而言之，我们得出这样一个事实，如果$det(V{U}^T)=-1$，我们需要
$$M=V^{T}RU=\left(\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & -1\end{array}\right)\to R=V\left(\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & -1\end{array}\right)U^T\text{\hspace{1em}(23)}$$
我们可以写出一个包含两种情况的通用公式，$det(V{U}^T)=1$和$det(V{U}^T)=1$：
$$R=V\left(\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & det(V{U}^T)\end{array}\right)U^T\text{\hspace{1em}(24)}$$

刚体运动计算-概述

让我们总结一下计算最有平移$t$和旋转$R$的步骤
$$\sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2$$

1. 计算两个点集的加权质心
   $$\bar{p}=\frac{{\sum }_{i=1}^n{w}_i{p}_i}{{\sum }_{i=1}^n{w}_i},\bar{q}=\frac{{\sum }_{i=1}^n{w}_i{q}_i}{{\sum }_{i=1}^n{w}_i}$$
2. 计算去中心的坐标
   $$x_i=p_i-\bar{p},y_i=q_i-\bar{q}$$
3. 计算$d\times d$的协方差矩阵
   $$S=XW{Y}^T$$
   其中$W=diag(w_1,w_2,...,w_n)$是带加权对角元素$w_i$的$n\times n$对角矩阵；$Y$是一个以$y_i$为列的$d\times n$矩阵，$X$是一个以$x_i$为列的$d\times n$矩阵。
4. 计算奇异值分解$S=U\Sigma V^T$，旋转矩阵可以由以下公式求出
   $$R=V\left(\begin{array}{ccccc}1& & & & \\ & 1& & & \\ & & \ddots & & \\ & & & 1& \\ & & & & det(V{U}^T)\end{array}\right)U^T$$
5. 计算最优的平移
   $$t=\bar{q}-R\bar{p}$$