- 三維旋轉矩陣構成特殊正交群，SO(3),變換矩陣構成了特殊歐式群SE(3).
$${\rm{SO(3) = \{ R}} \in {{\rm{R}}^{3 \times 3}}\left| {R{R^T} = I,\det (R) = I} \right.{\rm{\} }}$$
\[{\rm{SE(3) = }}\left\{ {{\rm{T = }}\left[ {\begin{array}{*{20}{c}}
R&t\\
0&1
\end{array}} \right] \in {{\rm{R}}^{4 \times 4}}\left| {R \in SO(3),t \in {R^3}} \right.} \right\}\]
- 從上面看群是一種集合加上一種運算的代數結構。什么集合，何種運算不知道。把集合記作A，運算記作$ \bullet $，群記為$G = (A, \bullet )$,群應滿足以下四個特性（封結幺逆）：
- 封閉性：$\forall {a_1},{a_2} \in A,\;\;\;\;\;{a_1} \bullet {a_2} \in A$
- 結合律：
\[\forall {a_1},{a_2},{a_3} \in A,\;\;\;\;\;({a_1} \bullet {a_2}) \bullet {a_3} = {a_1} \bullet ({a_2} \bullet {a_3})\]
- 幺元：$\exists {a_0} \in A,s.t\;\forall a \in A,{a_0} \bullet a = a \bullet {a_0} = a$
- 逆：$\forall a \in A,\;\;\;\exists {a^{{\rm{ - }}1}} \in A,s.t\;a \bullet {a^{_{{\rm{ - }}1}}} = a$
- 李群是指具有連續光滑性質的群,像一般的整數離散群不具有連續性，SO(3), SE(3)在實數空間上是連續的，表示一個位置變換到另一個位置，路徑是連續的。但是群的概念只定義了乘法，沒有加法。
-
李代數的引出：
李代數由一個集合V、一個數域F和一個二元運算[ ，]（李括號）組成。稱（V,F，[ , ]）為一個李代數，記為g.
在實際應用中我們估計相機的位置和姿態時，由於干擾，不可能沒有誤差，那么產生誤差需要矯正，我們會用補償量$\Delta R,\Delta T$去補償誤差。然而群的定義對加法沒有封閉性，比如兩個旋轉或變換矩陣相加后不再表示一個旋轉或變換矩陣，失去了原有的意義，於是李代數的提出為此問題提供了解決思路，把SE(3)空間的T映射為李代數se(3),它由向量組成，我們在中學數學就知道，向量相加還表示為一個向量（例如平行四邊形原則），李代數對加法具有封閉性（加完之后還是向量）。這樣可以通過李代數求導間接對變換矩陣T進行求導。在優化相機位姿時，每次迭代更新一個位姿的增量δ，使目標函數最小，那么這個δ就是通過對T微分得到的。它們對於某時刻的R(t)(李群空間)，存在一個三維向量$\phi {\rm{ = (}}{\phi _1}{\rm{,}}{\phi _2}{\rm{,}}{\phi _3}{\rm{)}}$的李代數空間來描述R在t時刻的導數。
\[\mathop R\limits^ \bullet (t) = \phi {(t)^ \wedge }R(t) = \left[ {\begin{array}{*{20}{c}}
0&{ - {\phi _3}}&{{\phi _2}}\\
{{\phi _3}}&0&{ - {\phi _1}}\\
{ - {\phi _2}}&{{\phi _1}}&0
\end{array}} \right]R(t)\]
- 指數映射：向量$\phi {\rm{ = (}}{\phi _1}{\rm{,}}{\phi _2}{\rm{,}}{\phi _3}{\rm{)}}$反映了R的導數性質，它們之間的微分關系可以描述為：李代數so(3)對應李群SO(3) 在原點附近的正切空間。換句話說，李代數so(3)是三維向量$\phi {\rm{ = (}}{\phi _1}{\rm{,}}{\phi _2}{\rm{,}}{\phi _3}{\rm{)}}$的集合，每個$\phi (i)$的反對稱矩陣都$\phi (i)$^可以表示SO(3)上旋轉矩陣$R(i)$的導數,R和$\phi $呢又呈現一個指數映射關系：$R(t) = \exp (\phi _0^ \wedge t)$,$\exp ({\phi ^ \wedge }) = \exp (\theta {{\rm{a}}^ \wedge }) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}} {(\theta {{\rm{a}}^ \wedge })^n}$經過泰勒展開后變為$\exp (\theta {{\rm{a}}^ \wedge }) = \cos \theta I + (1 - \cos \theta )a{a^T} + \sin \theta {a^ \wedge }$又回到了羅德里格斯。對於三維旋轉和變換的關系圖如下;
- 李代數求導與擾動模型：
以SO(3)為例，當李代數的so(3)做加法時，在李群SO(3)是否對應為兩個矩陣的乘積，$\exp (\phi _1^ \wedge )\exp (\phi _2^ \wedge ) = \exp ({({\phi _1} + {\phi _2})^ \wedge })$？$\ln (\exp (A)\exp (B)) = A{\rm{ + }}B$？答案是對矩陣運算都不成立，BCH公式給出了近似解：
\[\ln {(\exp (A\phi _1^ \wedge )\exp (\phi _2^ \wedge ))^ \vee } \approx \left\{ {\begin{array}{*{20}{c}}
{{J_l}{{({\phi _2})}^{ - 1}}{\phi _1} + {\phi _2},{\phi _1}\min }\\
{{J_r}{{({\phi _1})}^{ - 1}}{\phi _2} + {\phi _1},{\phi _2}\min }
\end{array}} \right.\]
以下標為l的左乘模型來說，意思是，我們對一個旋轉矩陣R2 (李代數${\phi _2}$)左乘一個微小旋轉矩陣R1 (李代數${\phi _1}$ )，看成是李代數 ${\phi _2}$加上一項
\[{J_l}{({\phi _2})^{ - 1}}{\phi _1}\]
右乘模型同理。式中有一個近似的雅可比Jl,再通俗的形象一下就是，假定對一個旋轉R(李代數$\phi $)產生的漂移進行矯正，左乘一個$\Delta R$ (李代數$\Delta \phi $)，在李群上的結果為$\Delta R \bullet R$
用BCH近似為
\[{J_l}{(\phi )^{ - 1}}\Delta \phi {\rm{ + }}\phi \]
合起來就是$\exp (\Delta {\phi ^ \wedge })\exp ({\phi ^ \wedge }) = \exp ({(\phi + {J_l}(\phi )\Delta \phi )^ \wedge })$，然而上面剛剛說到在李代數上加法是封閉的，可以進行加法運算，一個
\[\phi {\rm{ + }}\Delta \phi \]
可以近似為李群上帶左右雅可比的乘法:$\exp ({(\phi + \Delta \phi )^ \wedge }) = \exp ({({J_l}\Delta \phi )^ \wedge })\exp ({\phi ^ \wedge }) = \exp ({\phi ^ \wedge })\exp ({({J_r}\Delta \phi )^ \wedge })$
- 求導運算1
空間中有一向量p,然后我們對其進行了R的旋轉變換變為Rp,求旋轉之后Rp對旋轉R的導數，$\frac{{\partial (Rp)}}{{\partial R}}$ ，但是沒有加法，無法按照導數定義計算，因此轉為李代數計算。
$$\frac{{\partial (Rp)}}{{\partial R}}{\rm{ = }}\frac{{\partial (\exp ({\phi ^ \wedge })p)}}{{\partial \phi }} = - {(Rp)^ \wedge }{J_l}$$
結果中有一個雅可比，在應用中不太好處理，又出現了擾動模型來替代，思路是對R進行一次擾動$\Delta R$(左乘形式），看結果相對擾動的變化率。 $\Delta R$對應的李代數為 $\varphi $，$\frac{{\partial (Rp)}}{{\partial \varphi }}{\rm{ = }} - {(Rp)^ \wedge }$,這樣就沒有了雅可比，對實際應用更加合理。
- 實現才是硬道理：
1 #include<iostream> 2 #include<cmath> 3 #include<Eigen/Core> 4 #include<Eigen/Geometry> 5 #include"sophus/se3.h" 6 #include"sophus/so3.h" 7 8 using namespace std; 9 using namespace Eigen; 10 11 int main(int argc,char **argv) 12 { 13 // 沿Z軸轉90度的旋轉矩陣用旋轉向量表示 14 Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix(); 15 16 Sophus::SO3 SO3_R(R); // Sophus::SO(3)可以直接從旋轉矩陣構造 17 Sophus::SO3 SO3_v( 0, 0, M_PI/2 ); // 亦可從旋轉向量構造 18 Eigen::Quaterniond q(R); // 或者四元數 19 Sophus::SO3 SO3_q( q ); 20 // 上述表達方式都是等價的 21 22 cout<<"SO(3)由矩陣構造:\n "<<SO3_R.matrix()<<endl; 23 cout<<"SO(3)由向量構造: \n"<<SO3_v.matrix()<<endl; 24 cout<<"SO(3)由四元數構造:\n"<<SO3_q.matrix()<<endl; 25 26 // 使用對數映射獲得它的李代數 27 Eigen::Vector3d so3 = SO3_R.log(); 28 cout<<"SO(3)的李代數so(3) =SO3_R.log()為： "<<so3.transpose()<<endl; 29 // hat 為向量到反對稱矩陣 30 cout<<"so(3)李代數向量對應的反對稱矩陣： \n"<<Sophus::SO3::hat(so3)<<endl; 31 // 相對的，vee為反對稱矩陣到向量 32 cout<<"so(3)反對稱矩陣對應的李代數向量： \n "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; 33 // transpose純粹是為了輸出美觀一些 34 35 // 增量擾動模型的更新 36 Eigen::Vector3d update_so3(1e-4, 0, 0); //假設更新量為這么多 37 Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R; 38 cout<<"更新后的SO(3) =δ×R為 \n"<<SO3_updated.matrix()<<endl; 39 40 cout<<"***********SE(3)的操作*************"<<endl; 41 // 對SE(3)操作大同小異 42 Eigen::Vector3d t(1,0,0); // 沿X軸平移1 43 Sophus::SE3 SE3_Rt(R, t); // 從R,t構造SE(3) 44 Sophus::SE3 SE3_qt(q,t); // 從q,t構造SE(3) 45 cout<<"SE3由矩陣旋轉+平移構造（ R,t)= "<<endl<<SE3_Rt.matrix()<<endl; 46 cout<<"SE3由四元數q,t= "<<endl<<SE3_qt.matrix()<<endl; 47 // 李代數se(3) 是一個六維向量，方便起見先typedef一下 48 typedef Eigen::Matrix<double,6,1> Vector6d; 49 Vector6d se3 = SE3_Rt.log(); 50 cout<<"SE(3)對應的李代數se(3) =\n "<<se3.transpose()<<endl; 51 // 觀察輸出，會發現在Sophus中，se(3)的平移在前，旋轉在后. 52 // 同樣的，有hat和vee兩個算符 53 cout<<"se(3)對應的反對稱矩陣 = "<<endl<<Sophus::SE3::hat(se3)<<endl; 54 cout<<"反對稱矩陣對應的李代數向量 = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl; 55 56 // se(3)的擾動 57 Vector6d update_se3; //更新量 58 update_se3.setZero();//六維更新量 59 update_se3(0,0) = 1e-4d;//改變其中一個量來產生觀測 60 Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt; 61 cout<<"更新后的SE(3) = "<<endl<<SE3_updated.matrix()<<endl; 62 63 return 0; 64 }
