作為備用知識，memo

學(xué)過矩陣理論或者線性代數(shù)的肯定知道正交矩陣（orthogonal matrix）是一個非常好的矩陣，為什么這么說？原因有一下幾點：

正交矩陣每一列都是單位矩陣，并且兩兩正交。最簡單的正交矩陣就是單位陣。

正交矩陣的逆（inverse）等于正交矩陣的轉(zhuǎn)置（transpose）。同時可以推論出正交矩陣的行列式的值肯定為正負1的。

正交矩陣滿足很多矩陣性質(zhì)，比如可以相似于對角矩陣等等。

以上可以看出正交矩陣是非常特殊的矩陣，而本文題目中的旋轉(zhuǎn)矩陣就是一種正交矩陣！它完美的詮釋了正交矩陣的所有特點。

先說一下什么是旋轉(zhuǎn)矩陣？如圖1所示，我們假設(shè)最開始空間的坐標系XA，YA，ZA就是笛卡爾坐標系，這樣我們得到空間A的矩陣VA={XA，YA，ZA}T，其實也可以看做是單位陣E。進過旋轉(zhuǎn)后，空間A的三個坐標系變成了圖1中紅色的三個坐標系XB，YB，ZB，得到空間B的矩陣VB={XB，YB，ZB}T。我們將兩個空間聯(lián)系起來可以得到VB=R?VA，這里R就是我們所說的旋轉(zhuǎn)矩陣。

圖1

由于XA={1,0,0}T，YA={0,1,0}T，ZA={0,0,1}T，結(jié)合圖2可以看出，旋轉(zhuǎn)矩陣R就是由XB，YB，ZB三個向量組成的。講到這里，大家應(yīng)該會發(fā)現(xiàn)旋轉(zhuǎn)矩陣R滿足第一個條件，因為單位向量無論怎么旋轉(zhuǎn)長度肯定不會變而且向量之間的正交性質(zhì)也不會變。那么旋轉(zhuǎn)矩陣就是正交陣！不過這還不能說明問題，下面我更進一步利用數(shù)學(xué)公式進行證明。

圖2

進一步討論之前，我們先說兩點數(shù)學(xué)知識。（1）點乘（dot product）的幾何意義：如圖3，我們從點乘的公式可以得到α?β相當(dāng)與β的模乘上α在β上投影的模，所以當(dāng)|β|=1時，α?β就是指α在β上投影的模。這一點在下面的內(nèi)容中非常重要。（2）旋轉(zhuǎn)矩陣逆的幾何意思：這個比較抽象，不過也好理解。旋轉(zhuǎn)矩陣相當(dāng)于把一個向量（空間）旋轉(zhuǎn)成新的向量（空間），那么逆可以理解為由新的向量（空間）轉(zhuǎn)回原來的向量（空間）。

圖3

接下來就是重點了，我們結(jié)合圖4進行分析。上面已經(jīng)說明了，旋轉(zhuǎn)矩陣R就是由XB，YB，ZB三個向量組成的。我們來看看XB，YB，ZB究竟是什么？由于圖中所有的向量均是單位向量，所以XB與XA點乘的結(jié)果可以看成XB在XA上的投影的模，也就是XB在空間A中x軸的分量??！圖中中間的位置列出了XB向量中的三個分量分別為XB在XA上的投影的模、XB在YA上的投影的模和XB在ZA上的投影的模。這從幾何角度很好理解。以此類推，可以得出的旋轉(zhuǎn)矩陣R的表達形式。我們根據(jù)圖4可以驚喜的發(fā)現(xiàn)，矩陣R的第一行就是XA在XB，YB，ZB上的投影的模，也就是XAT。

圖4

這個發(fā)現(xiàn)有什么用呢？圖5做出解釋。根據(jù)上面公式可以推出A到B的旋轉(zhuǎn)矩陣等于B到A的旋轉(zhuǎn)矩陣的轉(zhuǎn)置。根據(jù)我們上一段所說的A到B的旋轉(zhuǎn)矩陣的逆就是等于B到A的旋轉(zhuǎn)矩陣，因此很容易推出R-1等于RT！這滿足正交矩陣的第二個條件，又一次證明了旋轉(zhuǎn)矩陣就是正交陣。在平時的工作中，我也測試過所有的旋轉(zhuǎn)矩陣的行列式的值都是為1的，所以旋轉(zhuǎn)矩陣滿足正交陣的一切性質(zhì)，可以說是很完美的矩陣。

圖5

現(xiàn)在以三個歐拉角中的RotX為例（其余兩個歐拉角以此類推），驗證一下以上說的結(jié)論。

首先結(jié)合圖5的公式，計算出RotX的旋轉(zhuǎn)矩陣Rrotx。

由于X軸是垂直于YoZ平面的，所以XA和YB，ZB的點乘結(jié)果為0，同時XB和YA，ZA的點乘結(jié)果也為0。

由于XA，XB都是單位向量，所以XA和XB的點乘結(jié)果為1。

由于繞x軸旋轉(zhuǎn)，所以我們觀察YB和ZB分別在YA和ZA上的投影情況，如圖6，我已經(jīng)將坐標標注了。

圖6

這樣就完成旋轉(zhuǎn)矩陣Rrotx，我們接下來驗證一下。

我們計算每一行每一列的模，都為1；并且任意兩個列向量或者任意兩個行向量都是正交的。所以滿足上文列出的第一個性質(zhì)。

我們計算Rrotx的行列式，很簡單可以算出為1。這時我們計算一下該矩陣的逆和轉(zhuǎn)置，這里我不寫出來了是相等的。所以滿足上文列出的第三個性質(zhì)。

第三個性質(zhì)要牽扯到更多的數(shù)學(xué)知識，在這里就不驗證了。

總結(jié)一下：旋轉(zhuǎn)矩陣是一個完美的矩陣——正交矩陣。它的行列式為1，且每個列向量都是單位向量且相互正交，它的逆等于它的轉(zhuǎn)置。、

—————————————————————————————————

三維空間的旋轉(zhuǎn)(3D Rotation)是一個很神奇的東東：如果對某個剛體在三維空間進行任意次的旋轉(zhuǎn)，只要旋轉(zhuǎn)中心保持不變，無論多少次的旋轉(zhuǎn)都可以用繞三維空間中某一個軸的一次旋轉(zhuǎn)來表示。表示三維空間的旋轉(zhuǎn)有多種互相等價的方式，常見的有旋轉(zhuǎn)矩陣、DCM、旋轉(zhuǎn)向量、四元數(shù)、歐拉角等。本篇文章主要梳理一下這些表示方式及相互轉(zhuǎn)換的方法。

1. 歐拉角(Euler Angle)

最直觀的表示方式是繞剛體自身的X、Y、Z三個軸分別進行旋轉(zhuǎn)某個角度，這就是所謂的歐拉角(Euler Angle)表示方式。

需要注意的是，歐拉角的表示方式里，yaw、pitch、roll的順序?qū)πD(zhuǎn)的結(jié)果是有影響的。給定一組歐拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的順序旋轉(zhuǎn)和按照yaw-roll-pitch的順序旋轉(zhuǎn)，最終剛體的朝向是不同的！換言之，若剛體需要按照兩種不同的旋轉(zhuǎn)順序旋轉(zhuǎn)到相同的朝向，所需要的歐拉角角度值則是不同的！

另外需要注意的是，在歐拉角的表示方式里，三個旋轉(zhuǎn)軸一般是隨著剛體在運動，即wikipedia中所謂的intrinsic rotation，見下圖動畫所示(圖來自wikipedia)。相對應(yīng)的另一種表示方式是，三個旋轉(zhuǎn)軸是固定的，不隨剛體旋轉(zhuǎn)而旋轉(zhuǎn)，即extrinsic rotation，這種表示方式在計算機視覺中不是很常用。

歐拉角的表示方式比較直觀，但是有幾個缺點：

(1) 歐拉角的表示方式不唯一。給定某個起始朝向和目標朝向，即使給定yaw、pitch、roll的順序，也可以通過不同的yaw/pitch/roll的角度組合來表示所需的旋轉(zhuǎn)。比如，同樣的yaw-pitch-roll順序，(0,90,0)和(90,90,90)會將剛體轉(zhuǎn)到相同的位置。這其實主要是由于萬向鎖(Gimbal Lock)引起的，關(guān)于萬向鎖的解釋，有條件的同學(xué)看看Youtube的視頻或許會比較直觀。

(2) 歐拉角的插值比較難。

(3) 計算旋轉(zhuǎn)變換時，一般需要轉(zhuǎn)換成旋轉(zhuǎn)矩陣，這時候需要計算很多sin, cos，計算量較大。

2. 旋轉(zhuǎn)矩陣(Rotation Matrix)和方向余弦矩陣(Direction Cosine Matrix)

在計算坐標變換時，旋轉(zhuǎn)更方便的表示形式是旋轉(zhuǎn)矩陣(Rotation Matrix)。三維空間的旋轉(zhuǎn)矩陣可以表示成3x3的矩陣，將歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣的計算方式如下，假設(shè)歐拉角yaw、pitch、roll的角度為alpha, beta, gamma，則旋轉(zhuǎn)矩陣可以計算如下：

其中，

這里也可以看出，如果yaw、pitch、roll的順序有改變，矩陣相乘的順序需要作出相應(yīng)改變，所得的旋轉(zhuǎn)矩陣結(jié)果也會有所改變。

需要注意的是，旋轉(zhuǎn)矩陣的雖然有9個元素，但是只有3個自由度，所以不是任何矩陣都可以作為旋轉(zhuǎn)矩陣，旋轉(zhuǎn)矩陣需要是正交矩陣(即逆矩陣等于轉(zhuǎn)置矩陣)。

此外，旋轉(zhuǎn)矩陣的另一個名字叫方向余弦矩陣(Direction Cosine Matrix)，簡稱DCM，在陀螺力學(xué)領(lǐng)域較為常用。DCM的名字來歷其實是用歐拉角之外的另一種用3個角度值表示三維旋轉(zhuǎn)的方式，假設(shè)剛體在起始朝向時三個坐標軸的向量為I,J,K，而剛體在目標朝向時的三個坐標軸的向量為i,j,k，則該旋轉(zhuǎn)可以通過三個坐標軸分別與原始坐標軸的夾角表示，如下圖所示：

DCM可以通過三個夾角的余弦計算如下：

這就是DCM名稱的由來。其實可以驗證，DCM其實就是旋轉(zhuǎn)矩陣，所以，下文不再區(qū)分DCM和旋轉(zhuǎn)矩陣的稱呼。

在Matlab中(R2006a以后的版本中，需安裝Aerospace Toolbox)，可以方便地用angle2dcm和dcm2angle來轉(zhuǎn)換歐拉角和旋轉(zhuǎn)矩陣。下面的Matlab代碼可以驗證，兩個不同的歐拉角方式可以轉(zhuǎn)換到相同的旋轉(zhuǎn)矩陣：

% Matlab code by MulinB, Aerospace Toolbox is needed

%?Gimbal?Lock?experiments

yaw1?=???0;

pitch1?=?90;

roll1?=??0;

yaw2?=???90;

pitch2?=?90;

roll2?=??90;

R1?=?angle2dcm(yaw1/180*pi,pitch1/180*pi,roll1/180*pi);

R2?=?angle2dcm(yaw2/180*pi,pitch2/180*pi,roll2/180*pi);

disp(R1);disp(R2);

3. 四元數(shù)(Quaternion)、旋轉(zhuǎn)向量(Rotation Vector)、軸-角表示(Axis-Angle)

旋轉(zhuǎn)的一個神奇之處就在于，三維空間的任意旋轉(zhuǎn)，都可以用繞三維空間的某個軸旋轉(zhuǎn)過某個角度來表示，即所謂的Axis-Angle表示方法。這種表示方法里，Axis可用一個三維向量(x,y,z)來表示，theta可以用一個角度值來表示，直觀來講，一個四維向量(theta,x,y,z)就可以表示出三維空間任意的旋轉(zhuǎn)。注意，這里的三維向量(x,y,z)只是用來表示axis的方向朝向，因此更緊湊的表示方式是用一個單位向量來表示方向axis，而用該三維向量的長度來表示角度值theta。這樣以來，可以用一個三維向量(theta*x, theta*y, theta*z)就可以表示出三維空間任意的旋轉(zhuǎn)，前提是其中(x,y,z)是單位向量。這就是旋轉(zhuǎn)向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是這種表示方法來表示旋轉(zhuǎn)(見OpenCV相機標定部分的rvec)。

Axis-Angle的表示方法還可以推導(dǎo)出另一種很常用的三維旋轉(zhuǎn)表示方法，叫四元數(shù)(Quaternion)，這里有一篇非常通俗易懂介紹四元數(shù)的文章。同上，假設(shè)(x,y,z)是axis方向的單位向量，theta是繞axis轉(zhuǎn)過的角度，那么四元數(shù)可以表示為[cos(theta/2), x*sin(theta/2), y*sin(theta/2), z*sin(theta/2)]。注意，這里可以推導(dǎo)出，用于表示旋轉(zhuǎn)的四元數(shù)向量也必須是單位向量。四元數(shù)的神奇之處在于，對于三維坐標的旋轉(zhuǎn)，可以通過四元數(shù)乘法直接操作，與上述旋轉(zhuǎn)矩陣操作可以等價，但是表示方式更加緊湊，計算量也可以小一些。首先，四元數(shù)的乘法是如下規(guī)定的：

由此定義，四元數(shù)的逆也可以求出。作為旋轉(zhuǎn)四元數(shù)，由于其單位向量的特性，四元數(shù)的逆其實等于四元數(shù)的共軛，也就是如果四元數(shù)q=[a,b,c,d]，由于a^2+b^2+c^2+d^2=1，那么q的逆和共軛都是q'=[a,-b,-c,-d]。需要注意的是，四元數(shù)的乘法是不可交換的。通過四元數(shù)計算旋轉(zhuǎn)的方式為(將三維空間一個點v_I旋轉(zhuǎn)到v_B，四元數(shù)是q)：

在Matlab里，可以用quatmultiply計算四元數(shù)乘法，用quatinv來計算四元數(shù)的逆，用quatconj來計算四元數(shù)的共軛。四元數(shù)的旋轉(zhuǎn)和旋轉(zhuǎn)矩陣的旋轉(zhuǎn)可以由以下matlab代碼驗證：

%?Matlab?code?by?MulinB,?Aerospace?Toolbox?is?needed

pt?=?[10,20,30];?%?point?coordinate

yaw?=???45;

pitch?=?30;

roll?=??60;

q?=?angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi);

R?=?angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi);

pt1?=?R*pt';

pt2?=?quatmultiply(quatconj(q),?quatmultiply([0,pt],q));?%?NOTE?the?order

disp(pt1');disp(pt2(2:4));

從上述代碼里也可以看到四元數(shù)和歐拉角和dcm的轉(zhuǎn)換，在matlab里可以很方便的用quat, dcm, angle之間的轉(zhuǎn)換來任意互轉(zhuǎn)。另外，從四元數(shù)計算axis和angle，可以用以下代碼計算：%?Matlab?code?by?MulinB,?Compute?the?axis?and?angle?from?a?quaternion

function?[axis,?theta]?=?quat2axisangle(q)

theta?=?acos(q(1))?*?2;

axis?=?q(2:4)/sin(theta/2);

從OpenCV的rotation vector和quaternion的互轉(zhuǎn)可以用以下代碼：

%?Matlab?code?by?MulinB,?Convert?a?quaternion?to?a?rotation?vector

function?rvec?=?quat2rvec(q)

theta?=?acos(q(1))?*?2;

axis?=?q(2:4)/sin(theta/2);

axis?=?axis?/?norm(axis);

rvec?=?axis*theta;

%?Matlab?code?by?MulinB,?Convert?a?rotation?vector?to?a?quaternion

function?q?=?rvec2quat(rvec)

theta?=?norm(rvec);

axis?=?rvec/theta;

sht?=?sin(theta/2);

q?=?[cos(theta/2),?axis*sht];

參考

http://blog.csdn.net/mulinb/article/details/51227597

http://www.cnblogs.com/caster99/p/4703033.html