- 向量的矩陣形式
有兩個向量：
\[\overrightarrow {\rm{a}} = ({a_1},{a_2},{a_3})\]
\[\overrightarrow {\rm{b}} = ({b_1},{b_2},{b_3})\]
叉乘的結果表示一個向量，這個向量向量垂直於a,b向量構成的平面。
\[\overrightarrow a \times \overrightarrow {\rm{b}} = \left\| {\begin{array}{*{20}{c}}
{{e_1}}&{{e_2}}&{{e_3}}\\
{{a_1}}&{{a_2}}&{{a_3}}\\
{{b_1}}&{{b_2}}&{{b_3}}
\end{array}} \right\| = \left[ {\begin{array}{*{20}{c}}
{{a_2}{b_3} - {a_3}{b_2}}\\
{{a_3}{b_1} - {a_1}{b_3}}\\
{{a_1}{b_2} - {a_2}{b_1}}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
0&{ - {a_3}}&{{a_2}}\\
{{a_3}}&0&{ - {a_1}}\\
{ - {a_2}}&{{a_1}}&0
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{{b_1}}\\
{{b_2}}\\
{{b_3}}
\end{array}} \right] = {a^ \wedge }b\]
將向量a對應的矩陣表示出來，為一個反對稱矩陣，每一個向量都對應着一個反對稱矩陣。這就引出向量的矩陣形式。
\[{a^ \wedge } = \left[ {\begin{array}{*{20}{c}}
0&{ - {a_3}}&{{a_2}}\\
{{a_3}}&0&{ - {a_1}}\\
{ - {a_2}}&{{a_1}}&0
\end{array}} \right]\]
- 坐標變換的易混點
在齊次變換中
\[{{\rm{p}}_1} = {T_{12}} \bullet {p_2}\]
\[{{\rm{p}}_2} = {T_{23}} \bullet {p_3}\]
T12表示，把坐標系{2}的向量變換到坐標系{1}中，T23同理，如果把坐標系{3}下的向量變換到坐標系{1}中為：
\[{{\rm{p}}_1} = {T_{12}} \bullet {T_{23}} \bullet {p_3}\]
- 旋轉向量和歐拉角:
SO(3)的旋轉矩陣有9個量，但是只有3個自由度，同理SE(3)有16個量，但是也只有6個自由度。在實際的旋轉中，任意的旋轉都可用一個旋轉軸和一個旋轉角來表示，我們使用一個向量，方向與旋轉軸一致，長度等於旋轉角，這樣只需要一個三維向量即可描述旋轉。對於SE(3),用一個旋轉向量和一個平移向量即可表達，恰好自由度為6.如果用旋轉向量來描述R：旋轉軸為一個單位長度的向量n,角度為$\theta $，那么$\theta {\rm{n}}$可以表示這個旋轉。旋轉矩陣R和旋轉向量$\theta {\rm{n}}$的轉換過程為羅德里格斯變換：
\[R = \cos \theta I + (1 - \cos \theta )n{n^T} + \sin \theta {n^ \wedge }\]
此處末尾的${{\rm{n}}^ \wedge }$ 如上面所示，代表矩陣表示的向量。那么反過來通過旋轉矩陣獲取轉角 $\theta $;
$$\theta {\rm{ = arccos}}\frac{{tr(R) - 1}}{2}$$
tr(R)為矩陣R的跡。對於轉軸n,Rn=n;表示為轉軸繞自身轉動不生改變，從數學來說n是矩陣R特征值為1時對應的特征向量。從現在來看旋轉軸和旋轉角來表示的旋轉是緊湊的，沒有冗余性，但是歐拉角RPY的空間中，當一個旋轉達到$\underline {\rm{ + }} 90^\circ $是就出現了奇異性。相當於地球的經緯度中當緯度為$\underline {\rm{ + }} 90^\circ $時，經度無意義。
那么如何解決冗余性和奇異性呢，於是又提出了四元數，既不冗余有沒有奇異性。
- 四元數
$q = {q_0} + {q_1}i + {q_2}j + {q_3}k$其中i,j,k 為四元數的三個虛部，關系為：
\[\left\{ {\begin{array}{*{20}{c}}
{{i^2}{\rm{ = }}{j^2}{\rm{ = }}{k^2}{\rm{ = }}\; - 1}\\
{ij = k,ji = - k}\\
{jk = i,kj = - i}\\
{ki = j,ik = - j}
\end{array}} \right.\]
i,j,k的關系就像三維坐標系一樣，用單位四元數可以表示空間中的任意一個旋轉，與復數有一些不同，在復數中，乘以i代表旋轉90°，但在四元數中，乘以i對應着旋轉180°，保證$ij = k$，對應着繞i旋轉180°，在繞j旋轉180°等於繞k旋轉180°。${i^2} = - 1$意味着繞i軸旋轉360°得到一個相反的東西。四元數之間的運算滿足上面四個關系。
- 用四元數表示旋轉
空間的一個三維點$p = [x,y,z] \in {R^3}$，由一個單位四元數q指定的旋轉，三維點$p$經過旋轉之后變為$p'$，在原始的矩陣描述為$p' = Rp$，在四元數中描述時:$p = {[0,x,y,z]^T} = {[0,u]^T}$,相當於把四元數的三個虛部與空間中的三個軸對應，旋轉后的$p'$可以表示為：$p' = qp{q^{ - 1}}$（不用糾結為什么左乘一個q右乘一個q的逆，運算規則就是這樣），結果仍然為四元數。把$p'$的虛部取出得到旋轉之后的坐標。
- 說千道萬落到實踐是關鍵：
1 Matrix3d R_mat = Matrix3d::Identity();//初始化為單位陣 2 AngleAxisd R_vec(M_PI/4,Vector3d(0,0,1));//用旋轉角和旋轉軸表示的旋轉向量 3 cout.precision(3);//輸出精確到三位小數 4 R_mat = R_vec.toRotationMatrix();//旋轉向量轉化為旋轉矩陣 5 cout << "旋轉矩陣1：\n"<<R_vec.matrix() << endl //旋轉向量轉化為旋轉矩陣 6 << "旋轉矩陣1：\n" << R_mat << endl; 7 8 Vector3d v(1, 0, 0); 9 Vector3d v_r = R_vec * v;//用旋轉軸和角表示的坐標變換 10 cout << "旋轉向量2：\n" << v_r.transpose() << endl;//transpose()為轉置 11 v_r = R_mat * v;//用旋轉矩陣表示的坐標變換 12 cout << "旋轉向量2：\n" << v_r.transpose() << endl; 13 14 Vector3d eul_ang = R_mat.eulerAngles(2, 1, 0);//ZYX順序，RPY 15 cout << "歐拉角rpy為：\n" << eul_ang << endl;//繞Z旋轉π/4 16 17 Isometry3d T = Isometry3d::Identity();//歐氏變換，4x4矩陣 18 T.rotate(R_vec);//按照R_vec旋轉向量來旋轉 19 T.pretranslate(Vector3d(1, 2, 3));//平移向量為（1.2.3） 20 cout << "變換矩陣為：\n" <<T.matrix() << endl; 21 22 Vector3d v_transform = T * v;//就是通常的R*v+t,旋轉和平移都包括進去了對於坐標第四個數默認補1 23 cout << "變換后的坐標為：\n" << v_transform << endl; 24 25 Quaterniond q = Quaterniond(R_vec);//用四元數表示旋轉向量 26 cout << "四元數表示的旋轉向量為：\n" << q.coeffs().transpose() << endl;//(i,j,k,w),前三個為虛部，最后一個為實部 27 q = Quaterniond(R_mat);//用四元數表示旋轉矩陣 28 cout << "四元數表示的旋轉矩陣為：\n" << q.coeffs().transpose() << endl;//(i,j,k,w),前三個為虛部，最后一個為實部 29 30 v_r = q * v;//用四元數表示的旋轉變換，數學中是q*v*q.inverse() 31 cout << "四元數旋轉向量為：\n" << v_r.transpose() << endl;
