視覺SLAM（三）李群與李代數 后續作業

第三章作業

作業：曾是少年

二 群的性質

課上我們講解了什么是群。請根據群定義，求解以下問題：

1. \(\{Z, +\}\) 是否為群？若是，驗證其滿足群定義；若不是，說明理由。

答：{Z,+}是群；

對於\(\{Z,+\}\)，設 \(a_1\in Z\) , \(a_2 \in Z\) , \(a_e \in Z\)。

1. 對於\(\forall a_1\in Z\) , \(a_2 \in Z\), 有\(a_1+a_2\in Z\), 因此滿足封閉性。
2. 對於\(\forall a_1\in Z,a_2\in Z,a_3\in Z\), \((a_1+a_2)+a_3 = a_1+(a_2+a_3)\)，因此滿足結合律。
3. Z中存在\(0\in Z\),對於\(\forall a \in Z\),有\(a+0=a\),因此慢足幺元
4. 對於\(\forall a \in Z\), 存在 \(-a\in Z\),使得 \(a+(-a) = 0\),因此滿足逆。

\(\{Z,+\}\)滿足以上四條性質，因此是群

2. \(\{N, +\}\) 是否為群？若是，驗證其滿足群定義；若不是，說明理由。

其中\(Z\)為整數集，\(N\)為自然數集

答： \(\{N,+\}\) 不是群；

對於\(\forall a\in N\)，且\(a\neq0\),\(-a\notin N\),不滿足逆的性質要求。因此不是群。

三 驗證向量叉乘的李代數性質

我們說向量和叉乘運算構成了李代數，現在請你驗證它。書中對李代數的定義為：李代數由⼀個集合，V，⼀個數域 F 和⼀個⼆元運算 [, ] 組成。如果它們滿足以下集幾條性質，稱$ (V, F, [, ])$ 為⼀個李代數，記作\(g\)。

1. 封閉性 \(∀X, Y ∈ V, [X, Y ] ∈ V\).

2. 雙線性 \(∀X, Y , Z ∈ V, a, b ∈ F\), 有：

   \[[aX + bY , Z] = a[X, Z] + b[Y , Z], [Z, aX + bY ] = a[Z, X] + b[Z, Y ]. \]

3. 自反性 \(∀X ∈ V, [X, X] = 0\).

4. 雅可比等價 \(∀X, Y , Z ∈ V, [X, [Y , Z]] + [Y , [Z, X]] + [Z, [X, Y ]] = 0\).

其中二元運算被稱為李括號。

現取集合\(V=R^3\)，數域\(F=R\)，李括號為:

\[[a,b]=a\times b \]

請驗證\(g=(R^3,R,\times)\)構成李代數。

驗證：

1. 封閉性

對於\(\forall X,Y \in R^3\)，\(X \times Y\)依然是一個向量，即\(X\times Y \in R^3\),因此滿足封閉性條件。

2. 雙線性

對於\(\forall X,Y,Z \in R^3\)，\(a,b\in R\)，向量叉乘運算滿足分配律和線性性，因此有：

\((aX+bY)\times Z = aX\times Z+bY\times Z = a(X\times Z)+b(Y\times Z)\)

\(Z\times(aX+bY) = aZ\times X+bZ\times Y=a(Z\times X)+b(Z\times Y)\)

因此滿足雙線性

3. 自反性

對於 \(\forall X \in R^3\),\(|X\times X| = |X||X|sin0=0\)，因此\(X\times X = 0\),滿足自反性。

4. 雅可比等價

把三適量叉乘展開成點乘，向量的叉乘運算滿足一下性質：

對於\(\forall X,Y,Z \in R^3\)，

• \(（X×Y）×Z=(XZ)Y-(YZ)X\)
• \(X\times(Y\times Z) = (XZ)Y-(XY)Z\)

因此

\[X\times(Y\times Z)+Y\times(Z\times X)+Z\times(X\times Y) \\ = X(YZ)-Z(XY) + (YX)Z-(YZ)X+(ZY)X-(ZX)Y \\ = 0 \]

因此向量的叉乘運算滿足雅可比恆等式。

綜上所述，\(g=(R^3,R,\times)\)構成李代數

四 推導 SE(3) 的指數映射

課上給出了 SO(3) 的指數映射推導，但對於 SE(3)，僅介紹了結論，沒有給出詳細推導。請你完成SE(3) 指數映射部分，有關左雅可比的詳細推導。

設\(\xi = [\rho,\phi]^T\in se(3)\)，它的指數映射為：

令\(\phi = \theta a\),那么：

這也正是課件里提到的左雅可比。

答：令\(\phi = \theta a\)

\[\begin{aligned} \sum^{\infty}_{n=0}\frac{1}{(n+1)!}(\phi^{\wedge})^n & = \sum^{\infty}_{n=0}\frac{1}{(n+1)!}(\theta a^{\wedge})^n\\ &= I+\frac{1}{2!}(\theta a^{\wedge})+\frac{1}{3!}(\theta a^{\wedge})^2+\frac{1}{4!}(\theta a^{\wedge})^3+\frac{1}{5!}(\theta a^{\wedge})^4+... \\ & = I+\frac{1}{2!}\theta {a^{\wedge}}+\frac{1}{3!}\theta^2 {a^{\wedge}}^2+\frac{1}{4!}{\theta}^3 {a^{\wedge}}^3+\frac{1}{5!}\theta^4 {a^{\wedge}}^4+...\\ \end{aligned} \]

其中：

\[\begin{equation} (a^{\wedge})^{n} = \begin{cases} \pm(a^\wedge) & \text{n為奇數}\\ \pm(aa^T-I) & \text{n為偶數} \\ \end{cases} \end{equation} \]

所以:

\[\begin{aligned} \sum^{\infty}_{n=0}\frac{1}{(n+1)!}(\phi^{\wedge})^n & = I+\frac{1}{2!}\theta {a^{\wedge}}+\frac{1}{3!}\theta^2 {a^{\wedge}}^2+\frac{1}{4!}{\theta}^3 {a^{\wedge}}^3+\frac{1}{5!}\theta^4 {a^{\wedge}}^4+...\\ & = I + (\frac{1}{2!}\theta-\frac{1}{4!}\theta^3+...)a^{\wedge}+(\frac{1}{3!}\theta^2-\frac{1}{5!}\theta^4+...)(aa^T-I)\\ & = I + \frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)a^{\wedge}+\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+...)(aa^T-I)\\ & = I + \frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)a^{\wedge}+\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+...)(aa^T-I)\\ & = \frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)a^{\wedge}+\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+...)aa^T +\frac{1}{\theta}(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5+...)I\\ & = \frac{1-cos\theta}{\theta}a^{\wedge}+\frac{1}{\theta}(\theta-sin\theta)aa^T+\frac{sin\theta}{\theta}I \end{aligned} \]

即：

\[\sum^{\infty}_{n=0}\frac{1}{(n+1)!}(\phi^{\wedge})^n = \frac{1-cos\theta}{\theta}a^{\wedge}+\frac{1}{\theta}(\theta-sin\theta)aa^T+\frac{sin\theta}{\theta}I \]

五 伴隨

在SO(3) 和SE(3) 上，有⼀個東西稱為伴隨（Adjoint）。下面請你證明SO(3) 伴隨的性質。
對於SO(3)，有：

\[R\exp (p^{\wedge})R^T = \exp((Rp)^{\wedge}) \]

此時稱Ad(R) = R。
提示：首先你需要證明\(\forall a\in R^3,Ra^{\wedge}{R^T} = (Ra)^{\wedge}\)，[頁面](https://math.stackexchange.com/questions/
2190603/derivation-of-adjoint-for-so3) 提示了⼀種簡潔的途徑。
對於SE(3)，有：

\[T \exp(\xi^{\wedge})T^{-1}= \exp((Ad(T)\xi^{\wedge}) \]

其中Ad(T)定義為：

\[Ad(T) = \left[\begin{array}{ccc} R & t^{\wedge}R \\ 0&R \end{array}\right] \]

這個性質將在后文的Pose Graph 優化中用到。但是SE(3) 的證明較為復雜，不作要求。
完整的SO(3) 和SE(3) 性質見1和2。

證明：

我們先來證明\(Ra^{\wedge}R^T=(Ra)^{\wedge}\)，過程如下：·

\[a^{\wedge}v = a\times v \]

我們可以通過使等式的RHS作用於任意向量v來證明該等式：

\[\begin{aligned} (Ra)^{\wedge}v &= (Ra)\times v \\&=(Ra)\times (RR^{-1}v) &(RR^{-1}=I) \\&=R[a \times(R^{-1}v)] &(分配律) \\&=Ra^{\wedge}R^{-1}v &(結合律) \end{aligned} \]

因此得到：

\[(Ra)^{\wedge}=Ra^{\wedge}R^{-1} \]

而R是正交矩陣，因此

\[(Ra)^{\wedge}=Ra^{\wedge}R^{T} \]

令\(\rho=\theta a\)

\[\begin{aligned} R\exp(\theta a^{\wedge})R^T & = R(\cos\theta I+(1-\cos\theta)aa^T+\sin\theta a^{\wedge})R^T\\ & = \cos\theta I+(1-cos\theta)Ra(Ra)^T+sin\theta Ra^{\wedge}R^T\\ &=\cos\theta I+(1-cos\theta)Ra(Ra)^T+sin\theta (Ra)^{\wedge}\\ &= \exp(\theta (Ra)^{\wedge})\\ &= \exp((R\rho)^{\wedge}) \end{aligned} \]

證畢。

---

拓展：伴隨表示

在數學中，一個李群 G 的伴隨表示（adjoint representation）或伴隨作用（adjoint action）是 G 在它自身的李代數上的自然表示。這個表示是群 G 在自身上的共軛作用的線性化形式。

六 軌跡的描繪

我們通常會記錄機器人的運動軌跡，來觀察它的運動是否符合預期。大部分數據集都會提供標准軌跡以供參考，如 kitti、TUM-RGBD 等。這些文件會有各自的格式，但首先你要理解它的內容。記世界坐標系為 W，機器⼈坐標系為 C，那么機器人的運動可以用 \(T_{WC}\) 或 \(T_{CW}\) 來描述。現在，我們希望畫出機器⼈在世界當中的運動軌跡，請回答以下問題：

1. 事實上，\(T_{WC}\) 的平移部分即構成了機器人的軌跡。它的物理意義是什么？為何畫出 \(T_{WC}\) 的平移部分就得到了機器⼈的軌跡？

   答:物理意義: \(T_{WC}\)指的是從世界坐標系原點到相機中心的平移向量;

   世界坐標系不隨相機運動變化,因此可以認為\(T_{wc}\)是機器人相對於原點坐標在移動, 移動可視化在觀察者眼中就是是機器人的運動軌跡

2. 我為你准備了⼀個軌跡文件（code/trajectory.txt）。該文件的每⼀行由若干個數據組成，格式為

   \[[t, t_x, t_y, t_z, q_x, q_y, q_z, q_w] \]

   其中 t 為時間，\(tx, ty, tz\) 為 \(T_{WC}\) 的平移部分，\(q_x, q_y, q_z, q_w\) 是四元數表示的 \(T_{WC}\) 的旋轉部分，\(q_w\)為四元數實部。同時，我為你提供了畫圖程序 draw_trajectory.cpp 文件。該⽂件提供了畫圖部分的代碼，請你完成數據讀取部分的代碼，然后書寫 CMakeLists.txt 以讓此程序運行起來。注意我們需要用到 Pangolin 庫來畫圖，所以你需要事先安裝 Pangolin（如果你做了第⼀次作業，那么現在已經安裝了）。CMakeLists.txt 可以參照 ORB-SLAM2 部分。

   答:實現過程:使用fstream讀取文件中的數據,在編譯的時候遇到一點小bug,不過都解決了.

   讀取數據的代碼塊如下:

       double t,t_x,t_y,t_z,q_x,q_y,q_z,q_w;
       while(!inFILE.eof())
       {
           inFILE>>t;
           inFILE>>t_x;
           inFILE>>t_y;
           inFILE>>t_z;
           inFILE>>q_x;
           inFILE>>q_y;
           inFILE>>q_z;
           inFILE>>q_w;
           poses.push_back(Sophus::SE3(Eigen::Quaterniond(q_w,q_x,q_y,q_z),Eigen::Vector3d(t_x,t_y,t_z)));
       }
   

   運行結果如下所示;

該圖中:軌跡首尾顏色不一樣,通過觀察,發現是着色函數設置的顏色隨位置變化.

附加題 七 軌跡的誤差

除了畫出真實軌跡以外，我們經常需要把 SLAM 估計的軌跡與真實軌跡相比較。下面說明比較的原理，請你完成比較部分的代碼實現。

設真實軌跡（ground-truth）為 \(T_g\)，估計軌跡 \(T_e\)。它們都以 \(T_{WC}\) 的形式存儲，格式同上題。現在，你需要計算估計軌跡的誤差。我們假設每⼀個 \(T_g\) 都與給定的 \(T_e\) 對應。那么，對於任意第 i 個位姿，它的誤差可定義為：

\[e_i=||\log(T^{-1}_{gi}T_{ei})^{\vee}||_2. \]

即兩個位姿之差的李代數二范數。於是，可以定義兩條軌跡的均方根（Root-Mean-Square-Error, RMSE）誤差為：

\[RMSE(g,e)=\sqrt{\frac{1}{n}\sum^{n}_{i=1}e_i^2} \]

我為你准備了 code/ground-truth.txt 和 code/estimate.txt 兩條軌跡。請你根據上⾯公式，實現 RMSE的計算代碼，給出最后的 RMSE 結果。作為驗算，參考答案為：2.207。

注:

1. 實際當中的軌跡比較還要更復雜⼀些。通常 ground-truth 由其他傳感器記錄（如 vicon），它的采樣頻率通常高於相機的頻率，所以在處理之前還需要按照時間戳對齊。另外，由於傳感器坐標系不一致致，還需要計算兩個坐標系之間的差異。這件事也可以⽤ ICP 解得，我們將在后面的課程中講到。

2. 你可以用上題的畫圖程序將兩條軌跡畫在同⼀個圖里，看看它們相差多少。

答：添加的代碼主要包括三部分:

1. 讀取兩個文件ground-truth.txt和estimate.txt，該部分與上一題中的相同。

2. 計算rmse，對於已經得到的兩個pose集合，可以通過以下代碼計算。

   double calculateRMSE(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> truth_poses,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> estimated_poses)
   {
       double rmse=0.0;
       for(int i = 0;i<truth_poses.size();i++)
       {
           Eigen::Matrix<double ,6,1> se3;
           se3 = (truth_poses[i].inverse()*estimated_poses[i]).log(); //這里是通過一個把其中一個變換乘以一個逆變換得到一個差矩陣，再通過.log()可以轉換為向量形式
           rmse+=se3.squaredNorm();
       }
       rmse = sqrt(rmse/(double)truth_poses.size());
       return rmse;
   }
   

3. 畫圖，修改軌跡繪制代碼，函數部分代碼如下所示：

   while (pangolin::ShouldQuit() == false) {
       glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   
       d_cam.Activate(s_cam);
       glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
   
       glLineWidth(2);
       for (size_t i = 0; i < truth_poses.size() - 1; i++) {
           glColor3f(1.0f, 0.0f, 0.0f);
           glBegin(GL_LINES);
           auto p1 = truth_poses[i], p2 = truth_poses[i + 1];
           glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
           glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
           glEnd();
   
           glColor3f(0.0f, 0.0f, 1.0f);
           glBegin(GL_LINES);
           p1 = estimated_poses[i], p2 = estimated_poses[i + 1];
           glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
           glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
           glEnd();
   
       }
       pangolin::FinishFrame();
       usleep(5000);   // sleep 5 ms
   }
   

   最后運行結果如下：

   /home/guoben/Project/SLAM-homework/ch3/draw_trajectory/bin/drawtraj
   rmse:2.20727
   

其中，紅色軌跡表示真值，藍色軌跡表示估計值