前言
羅德里格公式闡述了旋轉矩陣 R R R和旋轉軸 n n n及旋轉角度 θ \theta θ之間的關係: R ( n , θ ) = I + sin ( θ ) n × + ( 1 − cos ( θ ) ) n × 2 R(n, \theta) = I + \sin(\theta)n^{\times} + (1-\cos(\theta)){n^{\times}}^2 R(n,θ)=I+sin(θ)n×+(1−cos(θ))n×2,其中 n × n^{\times} n×是 n n n的反對稱矩陣。
本篇文章的目的是想要由 R R R得到 n n n和 θ \theta θ。
注:本篇參考A property of orthogonal matrices。
旋轉軸
首先知道使用旋轉矩陣 R R R對它的旋轉軸 n n n做旋轉並不會有任何改變,所以有 R n = n Rn = n Rn=n,對照特徵值公式 A x = λ x Ax = \lambda x Ax=λx可知, n n n即 R R R的特徵向量,且特徵值 λ \lambda λ為1。
旋轉角度
接着來求 R R R的trace(跡),試圖把 n n n從公式中消去,就可以得到 R R R與 θ \theta θ的關係。
參考羅德里格斯公式的證明,有 n × 2 = n n T − I {n^\times}^2 = nn^T - I n×2=nnT−I
參考向量的反對稱矩陣,有 t r a c e ( n × ) = 0 trace({n^\times}) = 0 trace(n×)=0
參考向量的反對稱矩陣,有 t r a c e ( n n T ) = 1 trace(nn^T) = 1 trace(nnT)=1
所以:
t r a c e ( R ) = t r a c e ( I ) + t r a c e ( sin ( θ ) n × ) + t r a c e ( ( 1 − cos ( θ ) ) n × 2 ) = t r a c e ( I ) + sin ( θ ) t r a c e ( n × ) + ( 1 − cos ( θ ) ) t r a c e ( n × 2 ) = t r a c e ( I ) + sin ( θ ) t r a c e ( n × ) + ( 1 − cos ( θ ) ) t r a c e ( n n T − I ) = t r a c e ( I ) + sin ( θ ) t r a c e ( n × ) + ( 1 − cos ( θ ) ) ( t r a c e ( n n T ) − t r a c e ( I ) ) = ( 1 + 1 + 1 ) + sin ( θ ) ⋅ 0 + ( 1 − cos ( θ ) ) ( 1 − ( 1 + 1 + 1 ) ) = 3 + 0 − 2 + 2 cos ( θ ) = 1 + 2 cos ( θ ) \begin{aligned}trace(R) &= trace(I) + trace(\sin(\theta)n^{\times}) + trace((1-\cos(\theta)){n^{\times}}^2) \\&= trace(I) + \sin(\theta)trace(n^{\times}) + (1-\cos(\theta))trace({n^{\times}}^2) \\&= trace(I) + \sin(\theta)trace(n^{\times}) + (1-\cos(\theta))trace(nn^T - I) \\&= trace(I) + \sin(\theta)trace(n^{\times}) + (1-\cos(\theta))(trace(nn^T) - trace(I)) \\&= (1+1+1) + \sin(\theta) \cdot 0+(1-\cos(\theta))(1-(1+1+1)) \\&= 3+0-2+ 2\cos(\theta) \\&= 1+2\cos(\theta)\end{aligned} trace(R)=trace(I)+trace(sin(θ)n×)+trace((1−cos(θ))n×2)=trace(I)+sin(θ)trace(n×)+(1−cos(θ))trace(n×2)=trace(I)+sin(θ)trace(n×)+(1−cos(θ))trace(nnT−I)=trace(I)+sin(θ)trace(n×)+(1−cos(θ))(trace(nnT)−trace(I))=(1+1+1)+sin(θ)⋅0+(1−cos(θ))(1−(1+1+1))=3+0−2+2cos(θ)=1+2cos(θ)
故:
cos ( θ ) = t r a c e ( R ) − 1 2 → θ = arccos ( t r a c e ( R ) − 1 2 ) \cos(\theta) = \frac{trace(R)-1}{2} \rightarrow \theta = \arccos(\frac{trace(R)-1}{2}) cos(θ)=2trace(R)−1→θ=arccos(2trace(R)−1)