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

Least-Squares Rigid Motion Using SVD

摘要

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

问题陈述

令P={p1,p2,...,pn}\mathcal{P} = {p_1,p_2,...,p_n}和Q={q1,q2,...,qn}\mathcal{Q}={q_1,q_2,...,q_n}是RdR^d空间内的两组对应的点。我们希望找到一个刚性的变换，在最小二乘的意义上最优地对齐两个点集，也就是说，我们寻求一个旋转RR和一个平移向量tt来满足
(R,t)=arg min⁡R∈SO(d),t∈Rd∑i=1nwi∣∣(Rpi+t)−qi∣∣2(1) (R,t) = \argmin_{R \in SO(d),t \in R^d}\sum_{i=1}{n}{w_i ||(Rp_i+t) - q_i||^2} \tag{1}
其中wi>0w_i > 0是每个点对的权重。
在下文中，我们详细介绍了RR和tt的推导过程；对最终结果感兴趣的读者可以跳过证明，直接进入第4节。

计算平移

假设RR被固定并定义F(t)=∑i=1nwi∣∣(Rpi+t)−qi∣∣2F(t) = \sum_{i=1}^{n}{w_i || (Rp_i+t) - q_i ||^2}。我们可以通过取F(t)F(t)相对于tt的导数并寻找它的根来找到最优平移:
0=∂F∂t=∑i=1n2wi(Rpi+t−qi)=2t(∑i=1nwi)+2R(∑i=1nwipi)−2∑i=1nwiqi(2) 0 = \frac{\partial F}{\partial t} = \sum_{i=1}^{n}{2 w_i (Rp_i + t -q_i)} \ = 2t(\sum_{i=1}^{n}{w_i}) + 2R (\sum_{i=1}^{n}{w_ip_i}) - 2\sum_{i=1}^{n}{w_iq_i} \tag{2}
定义
pˉ=∑i=1nwipi∑i=1nwi,qˉ=∑i=1nwiqi∑i=1nwi(3) \bar{p} = \frac{ \sum_{i=1}^{n}{w_ip_i} }{\sum_{i=1}^{n}{w_i}},\bar{q} = \frac{ \sum_{i=1}^{n}{w_iq_i} }{\sum_{i=1}^{n}{w_i}} \tag{3}
通过重新排列公式(2)的项，我们得到
t=qˉ−Rpˉ(4) t = \bar{q} - R \bar{p} \tag{4}
换句话说，最佳平移tt将被变换的点集p\mathcal{p}的加权质心映射到点集Q\mathcal{Q}的加权质心。让我们将最优的tt插入我们的目标函数:
∑i=1nwi∣∣(Rpi+t)−qi∣∣2=∑i=1nwi∣∣Rpi+qˉ−Rpˉ−qi∣∣2=∑i=1nwi∣∣R(pi−pˉ)−(qi−qˉ)∣∣2(5) \sum_{i=1}^{n}{w_i || (Rp_i + t) - q_i ||^2} = \sum_{i=1}^{n}{w_i || Rp_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} \tag{5}

因此，我们可以集中精力计算旋转RR。通过重构这个问题，使得平移为零:
xi=pi−pˉ,yi=qi−qˉ(6) x_i = p_i - \bar{p} , y_i = q_i - \bar{q} \tag{6}
所以我们寻找最佳旋转RR，使其满足
R=arg min⁡R∈SO(d)∑i=1nwi∣∣Rxi−yi∣∣2(7) R = \argmin_{R \in SO(d)} \sum_{i=1}^{n}{w_i || Rx_i - y_i ||^2} \tag{7}

计算旋转

让我们简化公式(7)中试图最小化的表达式:
∣∣Rxi−yi∣∣2=(Rxi−yi)T(Rxi−yi)=(xiTRT−yiT)(Rxi−yi)=xiTRTRxi−yiTRxi−xiTRTyi+yiTyi=xiTxi−yiTRxi−xiTRTyi+yiTyi(8) ||Rx_i - y_i||^2 = (Rx_i - y_i)^T(Rx_i - y_i) = (x_i^TRT - y_i^T)(Rx_i - y_i) \ = x_i^TRTRx_i - y_i^TRx_i - x_i^TRTy_i + y_i^Ty_i \ = x_i^Tx_i - y_i^TRx_i - x_i^TRTy_i + y_i^Ty_i \tag{8}
我们通过旋转矩阵的正交性（RRT=RTR=IRR^T = R^TR = I）来完成最后一步。

请注意，xiTRTyix_i^TRTy_i是一个标量：xiTx_i^T维度为1×d1 \times d，RTR^T维度为d×dd \times d并且yiy_i的维度为d×1d \times 1。对于任何标量aa，我们通常都有a=aTa=a^T，因此
xiTRTyi=(xiTRTyi)T=yiTRxi(9) x_i^TRTy_i = (x_i^TRTy_i)^T = y_i^TRx_i \tag{9}
因此我们有
∣∣Rxi−yi∣∣2=xiTxi−2yiTRxi+yiTyi(10) || Rx_i - y_i ||^2 = x_i^Tx_i - 2y_i^TRx_i + y_i^Ty_i \tag{10}
让我们看看要最小化的代价函数，并使用上面的表达式进行替换:
arg min⁡R∈SO(d)∑i=1nwi∣∣Rxi−yi∣∣2=arg min⁡R∈SO(d)∑i=1nwi(xiTxi−2yiTRxi+yiTyi)=arg min⁡R∈SO(d)(∑i=1nwixiTxi−2∑i=1nwiyiTRxi+∑i=1nwiyiTyi)=arg min⁡R∈SO(d)(−2∑i=1nwiyiTRxi)(11) \argmin_{R \in SO(d)} \sum_{i=1}^{n}{w_i || Rx_i - y_i ||^2} = \argmin_{R \in SO(d)} \sum_{i=1}^{n}{w_i (x_i^Tx_i - 2y_i^TRx_i + y_i^Ty_i)} \ = \argmin_{R \in SO(d)} (\sum_{i=1}^{n}{w_ix_iTx_i - 2\sum_{i=1}^{n}{w_i y_i^T R x_i}} + \sum_{i=1}^{n}{w_iy_iTy_i}) \ = \argmin_{R \in SO(d)}(-2 \sum_{i=1}^{n}{w_i y_i^T R x_i}) \tag{11}
最后一步(移除∑i=1nwixiTxi\sum_{i=1}^{n}{w_ix_iTx_i} 和∑i=1nwiyiTyi\sum_{i=1}^{n}{w_iy_iTy_i})成立，因为这些表达式根本不依赖于RR，所以移除它们不会影响最小值。最小化表达式乘以标量也是如此，所以我们有
arg min⁡R∈SO(d)(−2∑i=1nwiyiTRxi)=arg max⁡R∈SO(d)∑i=1nwiyiTRxi(12) \argmin_{R \in SO(d)}(-2 \sum_{i=1}^{n}{w_i y_i^T R x_i}) = \argmax_{R \in SO(d)} \sum_{i=1}^{n}{w_i y_i^T R x_i} \tag{12}
我们定义
∑i=1nwiyiTRxi=tr(WYTRX)(13) \sum_{i=1}^{n}{w_iy_iTRx_i} = tr(WY^TRX) \tag{13}

其中W=diag(w1,w2,...,wn)W = diag(w_1,w_2,...,w_n)是带加权对角元素wiw_i的n×nn \times n对角矩阵；YY是一个以yiy_i为列的d×nd \times n矩阵，XX是一个以xix_i为列的d×nd \times n矩阵。我们提醒读者，方阵的迹是对角线上元素的和：tr(A)=∑i=1naiitr(A) = \sum_{i=1}^{n}{a_{ii}}。参见图1来了解代数操作的说明。

因此，我们正在寻找一个最大化tr(WYTRX)tr(WY^TRX)的旋转RR。矩阵迹具有性质
tr(AB)=tr(BA)(14) tr(AB) = tr(BA) \tag{14}
对于任何矩阵维度允许的矩阵AA和BB。因此
tr(WYTRX)=tr((WYT)(RX))=tr(RXWYT)(15) tr(WY^TRX) = tr((WY^T)(RX) ) = tr(RXWY^T) \tag{15}
让我们定义d×dd\times d的协方差矩阵S=XWYTS= XWY^T。对SS进行SVD分解：
S=UΣVT(16) S = U \Sigma V^T \tag{16}
现在将分解替换到我们试图最大化的代价函数中：
tr(RXWYT)=tr(RS)=tr(RUΣVT)=tr(ΣVTRU)(17) tr(RXWY^T) = tr(RS) = tr(RU\Sigma V^T) = tr(\Sigma V^T RU) \tag{17}
最后一步是使用迹的属性(14)实现的。注意VV,RR和UU都是正交矩阵，所以M=VTRUM = V^{TRU也是正交矩阵。这意味着MM的列是正交向量，特别是，对于每个MM的列mjm_j，mjTmj=1m_j}Tm_j = 1。因此MM的所有元素mij≤1m_{ij} \leq 1：
1=mjTmj=∑i=1dmij2→mij2≤1→∣mij∣≤1(18) 1 = m_j^Tm_j = \sum_{i=1}^{d}{m_{ij}2} \to m_{ij}^2 \leq 1 \to |m_{ij}| \leq 1 \tag{18}
那么tr(ΣM)tr(\Sigma M)的最大可能值是多少？请记住，Σ\Sigma是一个对角矩阵，具有非负元素σ1,σ2,...,σd≥0\sigma_1 , \sigma_2 ,..., \sigma_d \geq 0。因此：
tr(ΣM)=(σ1σ2⋱σd)(m11m12⋯m1dm21m22⋯m2d⋮⋮⋮⋮md1md2⋯mdd)=∑i=1dσimii≤∑i=1dσi(19) tr(\Sigma M) = = \sum_{i=1}^{d}{\sigma_i m_{ii}} \leq \sum_{i=1}^{d}{\sigma_i} \tag{19}
因此，如果mii=1m_{ii} = 1，则迹最大化。因为MM是正交矩阵，这意味着MM必须是单位矩阵！
I=M=VTRU→V=RU→R=VUT(20) I = M = V^T RU \to V = RU \to R = VU^T \tag{20}

旋转反射

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

检查是R=VUTR = VU^{T否是旋转很简单:如果det(VUT)=−1det(VU}T)= -1，则它包含一个反射，否则det(VUT)=+1det(VU^T)= +1。假设det(VUT)=1det(VU^T)= 1。限制RR为旋转等同于限制MM为反射。我们现在想找到一个反射来最大化:
tr(ΣM)=σ1m11+σ2m22+...+σdmdd=f(m11,m22,...,mdd)(21) tr(\Sigma M) = \sigma_1m_{11} + \sigma_2 m_{22} + ... + \sigma_d m_{dd} = f(m_{11},m_{22},...,m_{dd}) \tag{21}
请注意，ff只取决于MM的对角线，而不是它的其他元素。我们现在考虑miim_{ii}作为变量(m11，...，mdd)(m_{11}，...，m_{dd})。这是nn阶反射矩阵的所有对角线元素的集合。令人惊讶的是，它的结构非常简单。事实上，A.Horn[1]的一个结果表明，nn阶旋转矩阵的所有对角线的集合等于点集(±1，...，±1)(\pm1，...，\pm1)的凸包，偶数个坐标为−1-1。因为任何反射矩阵都可以通过反转旋转矩阵一行的符号来构造，反之亦然，所以我们优化的集合是具有非偶数个−1-1的点集(±1，...，±1)(\pm1，...，\pm1)的凸包。

由于我们的定义域是凸多面体，所以线性函数ff在其顶点处有极值。对角线(1，1，...，1)(1，1，...，1)不在域中，因为它有偶数个−1-1(即零)，因此下一个最佳选择是(1，1，...,1,−1)(1，1，...,1,−1)：
tr(ΣM)=σ1+σ2+...+σd−1−σd(22) tr(\Sigma M) = \sigma_1 + \sigma_2 + ... + \sigma_{d-1} - \sigma_d \tag{22}
这个值是在我们的定义域的一个顶点上得到的，并且比形式(±1，...，±1)(\pm1，...，\pm1)除了(1，1，...，1)(1，1，...，1)的组合更大，因为σd\sigma_d是最小的奇异值。

总而言之，我们得出这样一个事实，如果det(VUT)=−1det(VU^T)= -1，我们需要
M=VTRU=(11⋱1−1)→R=V(11⋱1−1)UT(23) M = V^TRU = \to R = VU^T \tag{23}
我们可以写出一个包含两种情况的通用公式，det(VUT)=1det(VU^T) = 1和det(VUT)=1det(VU^T)= 1：

R = V\begin{pmatrix} 1 & & & & \\ & 1 & & & \\ & & \ddots & & \\ & & & 1 & \\ & & & & det(VU^T) \end{pmatrix}U^T \tag{24}

刚体运动计算-概述

让我们总结一下计算最有平移tt和旋转RR的步骤
∑i=1nwi∣∣(Rpi+t)−qi∣∣2 \sum_{i=1}^{n}{w_i ||(Rp_i+t) - q_i||^2}

1. 计算两个点集的加权质心
   pˉ=∑i=1nwipi∑i=1nwi,qˉ=∑i=1nwiqi∑i=1nwi \bar{p} = \frac{ \sum_{i=1}^{n}{w_ip_i} }{\sum_{i=1}^{n}{w_i}},\bar{q} = \frac{ \sum_{i=1}^{n}{w_iq_i} }{\sum_{i=1}^{n}{w_i}}

2. 计算去中心的坐标
   xi=pi−pˉ,yi=qi−qˉ x_i = p_i - \bar{p} , y_i = q_i - \bar{q}

3. 计算d×dd \times d的协方差矩阵
   S=XWYT S= XWY^T
   其中W=diag(w1,w2,...,wn)W = diag(w_1,w_2,...,w_n)是带加权对角元素wiw_i的n×nn \times n对角矩阵；YY是一个以yiy_i为列的d×nd \times n矩阵，XX是一个以xix_i为列的d×nd \times n矩阵。

4. 计算奇异值分解S=UΣVTS = U\Sigma V^T，旋转矩阵可以由以下公式求出
   R=V(11⋱1det(VUT))UT R = VU^T

5. 计算最优的平移
   t=qˉ−Rpˉ t = \bar{q} - R \bar{p}