利用SVD求得两个对应点集合的旋转矩阵R和转移矩阵t的数学推导

1.问题描述

给定两个在d维空间中对应的点集合 P = { p 1 , p 2 , … , p n } P = \{ p_1,p_2 ,\dots , p_n\} P={p1​,p2​,…,pn​}和 Q = { q 1 , q 2 , … , q n } Q = \{ q_1 ,q_2, \dots , q_n \} Q={q1​,q2​,…,qn​},为了计算出它们之间的刚体变换，即 $R$ 和$t$，可以将其建模为如下的数学形式：
$$(R,t)=argmin\sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2\text{\hspace{1em}(1)}$$
$w_i$ 表示每个点对之间的权重。

2. 计算转移矩阵

首先，对公式(1）求导，可以得到：
$$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)}$$
现在，引入点集合P的中心点$\hat{p}$和点集合Q的中心点$\hat{q}$，它们分别为：
$$\begin{gathered} \hat{p}=\frac{{\sum }_{i=1}^n{w}_i{p}_i}{{\sum }_{i=1}^n{w}_i} \\ \hat{q}=\frac{{\sum }_{i=1}^n{w}_i{q}_i}{{\sum }_{i=1}^n{w}_i}\text{\hspace{1em}(3)} \end{gathered}$$
公式(2)两边同时除以，${\sum }_{i=1}^n{w}_i$则得到：
$$\begin{gathered} \frac{0}{{\sum }_{i=1}^n{w}_i}=\frac{2t({\sum }_{i=1}^n{w}_i)}{{\sum }_{i=1}^n{w}_i}+\frac{2R({\sum }_{i=1}^n{w}_i{p}_i)}{{\sum }_{i=1}^n{w}_i}-\frac{2{\sum }_{i=1}^n{w}_i{q}_i}{{\sum }_{i=1}^n{w}_i} \\ 0=2t+2R\hat{p}-2\hat{q} \\ \hat{q}-R\hat{p}=t\text{\hspace{1em}(4)} \end{gathered}$$

将等式$t=\hat{q}-R\hat{p}$替换到公式(1)可以得到：
$$\begin{gathered} \sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2 \\ =\sum _{i=1}^n{w}_i||R{p}_i+\hat{q}-R\hat{p}-q_i|{|}^2 \\ =\sum _{i=1}^n{w}_i||R(p_i-\hat{p})-(q_i-\hat{q})|{|}^2\text{\hspace{1em}(5)} \end{gathered}$$
公式(5)看出，我们可以利用集合$X$和集合$Y$表示$p_i-\hat{p}$和$q_i-\hat{q}$，用$x_i$和$y_i$分别表示新数据集合中的点。
$$\begin{gathered} x_i:=p_i-\hat{p} \\ {y}_i:=q_i-\hat{q}\text{\hspace{1em}(6)} \end{gathered}$$
这时所以公式(1)可以等价于为：
$$R=argmin\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2\text{\hspace{1em}(7)}$$

3. 计算旋转矩阵

首先，扩展公式（7）：
$$\begin{gathered} \sum _{i=1}^n||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 \\ \stackrel{R^{T}R=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}$$
在公式（8)中，需要注意的是：$x_i^T{R}^T{y}_i$是一个标量，因为在集合中的每个点$x_i$是$1\times d$维的矢量，旋转矩阵$R$是一个$d\times d$维度的矩阵，$y_i$ 是一个$d\times 1$的矢量。
$${\left[\right]}_{1\times d}{\left[\right]}_{d\times d}{\left[\right]}_{d\times 1}={\left[\right]}_{1\times 1}$$
对任意的标量a，它满足$a^T=a$，且在公式(8)中：
$$x_i^T{R}^T{y}_i=(x_i^T{R}^T{y}_i{)}^T=y_i^{T}R{x}_i$$
所以公式(8)可以变成：
$$||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}(9)}$$
现在重新对公式(9)进行扩展，我们可以看出：
$$\begin{gathered} argmin\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2=argmin\sum _{i=1}^n{w}_i(x_i^T{x}_i-2y_i^{T}R{x}_i+y_i^T{y}_i) \\ =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) \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$，所以
$$argmin\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2=argmin(-2\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i)=argmax(\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i)$$

另外，
$$\begin{gathered} \sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i=\left[\begin{array}{cccc}{w}_1& & & \\ & w_2& & \\ & & \ddots & \\ & & & w_n\end{array}\right]\left[\begin{array}{c}{y}_1^T\\ y_2^T\\ ⋮\\ y_n^T\end{array}\right]\left[\begin{array}{cccc}& & & \\ & R& & \\ & & & \end{array}\right]\left[\begin{array}{cccc}{x}_1& x_2& \cdots & x_n\end{array}\right] \\ =\left[\begin{array}{c}{w}_1{y}_1^T\\ w_2{y}_2^T\\ ⋮\\ w_n{y}_n\end{array}\right]\left[\begin{array}{cccc}R{x}_1& R{x}_2& \cdots & R{x}_n\end{array}\right] \\ =\left[\begin{array}{cccc}{w}_1{y}_1^{T}R{x}_1& & & \\ & w_2{y}_2^{T}R{x}_2& & \\ & & \ddots & \\ & & & w_n{y}_n^{T}R{x}_n\end{array}\right] \end{gathered}$$
所以，我们可以得到
$$\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i=tr(W{Y}^{T}RX)$$
其中，$W=diag(w_1,...,w_n)$ ， $Y={\left[\begin{array}{cccc}{y}_1& y_2& \cdots & y_n\end{array}\right]}^T$ ， $X=\left[\begin{array}{cccc}{x}_1& x_2& \cdots & x_n\end{array}\right]$
且矩阵的迹满足某种特性，
$$tr(AB)=tr(BA)$$
则，
$$\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i=tr(W{Y}^{T}RX)=tr((W{Y}^T)(RX))=tr((RX)(W{Y}^T))=tr(RXW{Y}^T)$$
令$S=XW{Y}^T$
$$\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i=tr(RXW{Y}^T)\stackrel{S=XW{Y}^T}{=}tr(RS)\stackrel{SVD}{=}tr(RU\Sigma V^T)=tr((\Sigma V^T)(RU))=tr(\Sigma V^{T}RU)$$

需要注意的是：$V$， $R$，$U$是正交矩阵，所以$M=V^{T}RU$同样也是正交矩阵。这也意味这在矩阵M中，每一列的向量$m_j$，$m_j^T{m}_j=1$，因此，
$$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}\\ ⋮& ⋮& \ddots & ⋮\\ 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$$
为了求得最大化的R，$argmax({\sum }_{i=1}^n{w}_i{y}_i^{T}R{x}_i)$，则
$$\begin{gathered} I=M=V^{T}RU \\ V=RU \\ R=V{U}^T \end{gathered}$$

计算出R之后，转移矩阵$t$为：
$$t=\hat{q}-R\hat{p}$$

总结一下：

给定两个在d维空间中对应的点集合 P = { p 1 , p 2 , … , p n } P = \{ p_1,p_2 ,\dots , p_n\} P={p1​,p2​,…,pn​}和 Q = { q 1 , q 2 , … , q n } Q = \{ q_1 ,q_2, \dots , q_n \} Q={q1​,q2​,…,qn​},为了计算出它们之间的刚体变换，即 $R$ 和$t$，其过程如下：

1. 构建上述问题的模型为：
   $$(R,t)=argmin\sum _{i=1}^n{w}_i||(R{p}_i+t)-q_i|{|}^2$$
   2.对两个点集合进行去中心化，得到新的点集合$X$和$Y$,表示为：
   $$\begin{gathered} \hat{p}=\frac{{\sum }_{i=1}^n{w}_i{p}_i}{{\sum }_{i=1}^n{w}_i} \\ \hat{q}=\frac{{\sum }_{i=1}^n{w}_i{q}_i}{{\sum }_{i=1}^n{w}_i} \\ {x}_i:=p_i-\hat{p} \\ {y}_i:=q_i-\hat{q} \end{gathered}$$
   此时，转移矩阵
   $$t=\hat{q}-R\hat{p}$$
2. 步骤一中的问题转化为：
   $$\begin{gathered} R=\text{argmin}\sum _{i=1}^n{w}_i||R{x}_i-y_i|{|}^2 \\ =\text{argmax}\sum _{i=1}^n{w}_i{y}_i^{T}R{x}_i \\ =\text{argmax}tr(W{Y}^{T}RX) \\ =\text{argmax}tr(RXW{Y}^T) \\ =\text{argmax}tr(RXW{Y}^T) \\ \stackrel{SVD}{=}\text{argmax}tr(RU\Sigma V^T)=\text{argmax}tr(\Sigma V^{T}RU) \end{gathered}$$
3. 为了使得$tr(\Sigma V^{T}RU)$达到最大值，
   $$I=V^{T}RU$$
   逐步化简：
   $$\begin{gathered} V=RU \\ R=V{U}^T \end{gathered}$$
   所以，$t$可以根据公式$t=\hat{q}-R\hat{q}$ 计算出来。
   至此，就计算出两个点集合之间的选装矩阵$R$和转移矩阵$t$。另外，针对本章的推导，我写了一小段python代码验证了一下，有兴趣的可以看一下。计算两个对应点集之间的旋转矩阵R和转移矩阵T

【参考文献】
【1】Least-Squares Rigid Motion Using SVD