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

1.问题描述

给定两个在d维空间中对应的点集合 P={p1,p2,…,pn} P = { p 1 , p 2 , … , p n } 和 Q={q1,q2,…,qn} Q = { q 1 , q 2 , … , q n } ,为了计算出它们之间的刚体变换，即 R R 和 t t ，可以将其建模为如下的数学形式：

(R,t)=argmin∑i=1nwi||(Rpi+t)−qi||2(1) (1) ( R , t ) = a r g m i n ∑ i = 1 n w i | | ( R p i + t ) − q i | | 2

wi w i 表示每个点对之间的权重。

2. 计算转移矩阵

首先，对公式(1）求导，可以得到：

0=∂F∂t=∑i=1n2wi(Rpi+t−qi)=2t(∑i=1nwi)+2R(∑i=1nwipi)−2∑i=1nwiqi(2) (2) 0 = ∂ F ∂ t = ∑ i = 1 n 2 w i ( R p i + t − q i ) = 2 t ( ∑ i = 1 n w i ) + 2 R ( ∑ i = 1 n w i p i ) − 2 ∑ i = 1 n w i q i

现在，引入点集合P的中心点 p̂  p ^ 和点集合Q的中心点 q̂  q ^ ，它们分别为：

p̂ =∑ni=1wipi∑ni=1wiq̂ =∑ni=1wiqi∑ni=1wi(3) (3) p ^ = ∑ i = 1 n w i p i ∑ i = 1 n w i q ^ = ∑ i = 1 n w i q i ∑ i = 1 n w i

公式(2)两边同时除以， ∑ni=1wi ∑ i = 1 n w i 则得到：

0∑ni=1wi=2t(∑ni=1wi)∑ni=1wi+2R(∑ni=1wipi)∑ni=1wi−2∑ni=1wiqi∑ni=1wi0=2t+2Rp̂ −2q̂ q̂ −Rp̂ =t(4) (4) 0 ∑ i = 1 n w i = 2 t ( ∑ i = 1 n w i ) ∑ i = 1 n w i + 2 R ( ∑ i = 1 n w i p i ) ∑ i = 1 n w i − 2 ∑ i = 1 n w i q i ∑ i = 1 n w i 0 = 2 t + 2 R p ^ − 2 q ^ q ^ − R p ^ = t

将等式 t=hatq−Rp̂  t = h a t q − R p ^ 替换到公式(1)可以得到：

∑i=1nwi||(Rpi+t)−qi||2=∑i=1nwi||Rpi+q̂ −Rp̂ −qi||2=∑i=1nwi||R(pi−p̂ )−(qi−q̂ )||2(5) (5) ∑ i = 1 n w i | | ( R p i + t ) − q i | | 2 = ∑ i = 1 n w i | | R p i + q ^ − R p ^ − q i | | 2 = ∑ i = 1 n w i | | R ( p i − p ^ ) − ( q i − q ^ ) | | 2

公式(5)看出，我们可以利用集合 X X 和集合 Y Y 表示 pi−p̂  p i − p ^ 和 qi−q̂  q i − q ^ ，用 xi x i 和 yi y i 分别表示新数据集合中的点。

xi:=pi−p̂ yi:=qi−q̂ (6) (6) x i := p i − p ^ y i := q i − q ^

这时所以公式(1)可以等价于为：

R=argmin∑i=1nwi||Rxi−yi||2(7) (7) R = a r g m i n ∑ i = 1 n w i | | R x i − y i | | 2

3. 计算旋转矩阵

首先，扩展公式（7）：

∑i=1n||Rxi−yi||2=(Rxi−yi)T(Rxi−yi)=(xTiRT−yTi)(Rxi−yi)=xTiRTRxi−yTiRxi−xTiRTyi+yTiyi=RTR=IxTixi−yTiRxi−xTiRTyi+yTiyi(8) (8) ∑ 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 = 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

在公式（8)中，需要注意的是： xTiRTyi x i T R T y i 是一个标量，因为在集合中的每个点 xi x i 是 1×d 1 × d 维的矢量，旋转矩阵 R R 是一个 d×d d × d 维度的矩阵， yi y i 是一个 d×1 d × 1 的矢量。

[]1×d[]d×d[]d×1=[]1×1 [ ] 1 × d [ ] d × d [ ] d × 1 = [ ] 1 × 1

对任意的标量a，它满足 aT=a a T = a ，且在公式(8)中：

xTiRTyi=(xTiRTyi)T=yTiRxi x i T R T y i = ( x i T R T y i ) T = y i T R x i

所以公式(8)可以变成：

||Rxi−yi||2=xTixi−2yTiRxi+yTiyi(9) (9) | | R x i − y i | | 2 = x i T x i − 2 y i T R x i + y i T y i

现在重新对公式(9)进行扩展，我们可以看出：

argmin∑i=1nwi||Rxi−yi||2=argmin∑i=1nwi(xTixi−2yTiRxi+yTiyi)=argmin(∑i=1nwixTixi−2∑i=1nwiyTiRxi+∑i=1nwiyTiyi) a r g m i n ∑ i = 1 n w i | | R x i − y i | | 2 = a r g m i n ∑ i = 1 n w i ( x i T x i − 2 y i T R x i + y i T y i ) = a r g m i n ( ∑ i = 1 n w i x i T x i − 2 ∑ i = 1 n w i y i T R x i + ∑ i = 1 n w i y i T y i )

因为 ∑ni=1wixTixi ∑ i = 1 n w i x i T x i 和 ∑ni=1wiyTiyi ∑ i = 1 n w i y i T y i 不依赖于旋转矩阵 R R ，所以

argmin∑i=1nwi||Rxi−yi||2=argmin(−2∑i=1nwiyTiRxi)=argmax(∑i=1nwiyTiRxi) a r g m i n ∑ i = 1 n w i | | R x i − y i | | 2 = a r g m i n ( − 2 ∑ i = 1 n w i y i T R x i ) = a r g m a x ( ∑ i = 1 n w i y i T R x i )

另外，

∑i=1nwiyTiRxi=⎡⎣⎢⎢⎢⎢w1w2⋱wn⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢yT1yT2⋮yTn⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢R⎤⎦⎥⎥[x1x2⋯xn]=⎡⎣⎢⎢⎢⎢w1yT1w2yT2⋮wnyn⎤⎦⎥⎥⎥⎥[Rx1Rx2⋯Rxn]=⎡⎣⎢⎢⎢⎢w1yT1Rx1w2yT2Rx2⋱wnyTnRxn⎤⎦⎥⎥⎥⎥ ∑ i = 1 n w i y i T R x i = [ w 1 w 2 ⋱ w n ] [ y 1 T y 2 T ⋮ y n T ] [ R ] [ x 1 x 2 ⋯ x n ] = [ w 1 y 1 T w 2 y 2 T ⋮ w n y n ] [ R x 1 R x 2 ⋯ 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=1nwiyTiRxi=tr(WYTRX) ∑ i = 1 n w i y i T R x i = t r ( W Y T R X )

其中， W=diag(w1,...,wn) W = d i a g ( w 1 , . . . , w n ) ， Y=[y1y2⋯yn]T Y = [ y 1 y 2 ⋯ y n ] T ， X=[x1x2⋯xn] X = [ x 1 x 2 ⋯ x n ]
且矩阵的迹满足某种特性，

tr(AB)=tr(BA) t r ( A B ) = t r ( B A )

则，

∑i=1nwiyTiRxi=tr(WYTRX)=tr((WYT)(RX))=tr((RX)(WYT))=tr(RXWYT) ∑ i = 1 n w i y i T R x i = t r ( W Y T R X ) = t r ( ( W Y T ) ( R X ) ) = t r ( ( R X ) ( W Y T ) ) = t r ( R X W Y T )

令 S=XWYT S = X W Y T

∑i=1nwiyTiRxi=tr(RXWYT)=S=XWYTtr(RS)=SVDtr(RUΣVT)=tr((ΣVT)(RU))=tr(ΣVTRU) ∑ i = 1 n w i y i T R x i = t r ( R X W Y T ) = S = X W Y T t r ( R S ) = S V D t r ( R U Σ V T ) = t r ( ( Σ V T ) ( R U ) ) = t r ( Σ V T R U )

需要注意的是： V V ， R R ， U U 是正交矩阵，所以 M=VTRU M = V T R U 同样也是正交矩阵。这也意味这在矩阵M中，每一列的向量 mj m j ， mTjmj=1 m j T m j = 1 ，因此，

tr(ΣM)=⎛⎝⎜⎜⎜⎜σ1σ2⋱σd⎞⎠⎟⎟⎟⎟⎛⎝⎜⎜⎜⎜m11m21⋮md1m12m22⋮md2⋯⋯⋱⋯m1dm2d⋮mdd⎞⎠⎟⎟⎟⎟=∑i=1dσimii≤∑i=1dσi t r ( Σ M ) = ( σ 1 σ 2 ⋱ σ d ) ( m 11 m 12 ⋯ m 1 d m 21 m 22 ⋯ m 2 d ⋮ ⋮ ⋱ ⋮ m d 1 m d 2 ⋯ m d d ) = ∑ i = 1 d σ i m i i ≤ ∑ i = 1 d σ i

为了求得最大化的R， argmax(∑ni=1wiyTiRxi) a r g m a x ( ∑ i = 1 n w i y i T R x i ) ，则

I=M=VTRUV=RUR=VUT I = M = V T R U V = R U R = V U T

计算出R之后，转移矩阵 t t 为：

t=q̂ −Rp̂  t = q ^ − R p ^

总结一下：

给定两个在d维空间中对应的点集合 P={p1,p2,…,pn} P = { p 1 , p 2 , … , p n } 和 Q={q1,q2,…,qn} Q = { q 1 , q 2 , … , q n } ,为了计算出它们之间的刚体变换，即 R R 和 t t ，其过程如下：
1. 构建上述问题的模型为：

(R,t)=argmin∑i=1nwi||(Rpi+t)−qi||2 ( R , t ) = a r g m i n ∑ i = 1 n w i | | ( R p i + t ) − q i | | 2

2.对两个点集合进行去中心化，得到新的点集合 X X 和 Y Y ,表示为：

p̂ =∑ni=1wipi∑ni=1wiq̂ =∑ni=1wiqi∑ni=1wixi:=pi−p̂ yi:=qi−q̂  p ^ = ∑ i = 1 n w i p i ∑ i = 1 n w i q ^ = ∑ i = 1 n w i q i ∑ i = 1 n w i x i := p i − p ^ y i := q i − q ^

此时，转移矩阵

t=q̂ −Rp̂  t = q ^ − R p ^

3. 步骤一中的问题转化为：

R=argmin∑i=1nwi||Rxi−yi||2=argmax∑i=1nwiyTiRxi=argmax tr(WYTRX)=argmax tr(RXWYT)=argmax tr(RXWYT)=SVDargmax tr(RUΣVT)=argmax tr(ΣVTRU) R = a r g m i n ∑ i = 1 n w i | | R x i − y i | | 2 = a r g m a x ∑ i = 1 n w i y i T R x i = a r g m a x   t r ( W Y T R X ) = a r g m a x   t r ( R X W Y T ) = a r g m a x   t r ( R X W Y T ) = S V D a r g m a x   t r ( R U Σ V T ) = a r g m a x   t r ( Σ V T R U )

4. 为了使得 tr(ΣVTRU) t r ( Σ V T R U ) 达到最大值，

I=VTRU I = V T R U

逐步化简：

V=RUR=VUT V = R U R = V U T

所以， t t 可以根据公式 t=q̂ −Rq̂  t = q ^ − R q ^ 计算出来。
至此，就计算出两个点集合之间的选装矩阵 R R 和转移矩阵 t t 。另外，针对本章的推导，我写了一小段python代码验证了一下，有兴趣的可以看一下。 计算两个对应点集之间的旋转矩阵R和转移矩阵T

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