視覺 SLAM 十四講-基礎數學篇

1、第1講前言

SLAM 是 Simultaneous Localization and Mapping 的縮寫，中文譯作“同時定位與地圖構建”。本書介紹以視覺感測器為主體的視覺 SLAM 技術，分為兩部分：第一部分為數學基礎篇，第二部分為 SLAM 技術篇。

本書採用C++ 作為編碼語言，所有原始碼均託管到 github 上:https://github.com/gaoxiang12/slambook

2、第1講初識SLAM

（1）引子:小蘿蔔的例子

如下圖所示我們希望機器人小蘿蔔具有自主運動能力，夠在房間裡自由的移動，它至少需要知道兩件事:1. 我在什麼地方?——定位。2. 周圍環境是什麼樣?——建圖。

“定位”和“建圖”，可以看成感知的“內外之分”。內就是白自身的狀態(即位置)，外就是外在的環境(即地圖)。

視覺 SLAM 是本書的主題，按SLAM照相機的工作方式，我們把相機分為單目(Monocular)、雙目(Stereo)和深度相機(RGB-D)三個大類，此外還有全景相機和事件相機等特殊或新興的種類。

（2）經典視覺 SLAM 框架

如下圖所示即為經典的視覺 SLAM 框架：

我們把整個視覺 SLAM 流程分為以下幾步:

感測器資訊讀取。在視覺 SLAM 中主要為相機影像資訊的讀取和預處理。如果在機器人中，還可能有碼盤、慣性感測器等資訊的讀取和同步。
視覺里程計 (Visual Odometry, VO)。視覺里程計任務是估算相鄰影像間相機的運動，以及區域性地圖的樣子。VO 又稱為前端(Front End)。
後端優化(Optimization)。後端接受不同時刻視覺里程計測量的相機位姿，以及回環檢測的資訊，對它們進行優化，得到全域性一致的軌跡和地圖。由於接在 VO 之後，又稱為後端(Back End)。
迴環檢測(Loop Closing)。迴環檢測判斷機器人是否曾經到達過先前的位置。如果檢測到迴環，它會把資訊提供給後端進行處理。
建圖(Mapping)。它根據估計的軌跡，建立與任務要求對應的地圖。

（3）SLAM 問題的數學表述

運動方程：

觀測方程：

引數化運動方程：

引數化觀測方程：

把運動方程和觀測方程取成通用的抽象形式，那麼 SLAM 過程可總結為兩個基本方程:

我們按照運動和觀測方程是否為線性，噪聲是否服從高斯分佈進行分類，分為線性/非線性和高斯/非高斯系統。

（4）實踐：程式設計基礎

這部分主要講了安裝linux系統和使用g++、cmake、IDE等不同編譯方式編譯HelloSLAM程式，順帶講了講如在程式設計中何使用庫。

3、三維空間鋼體運動

（1）旋轉矩陣

1.點和向量、座標系

2.座標系間的歐氏變換

3.變換矩陣與齊次座標

（2）實踐：Eigen

這個例程演示了 Eigen 矩陣的基本操作與運算。比如Matrix定義方式：Eigen::Matrix<資料型別, 行數, 列數> Matrix例項名稱，矩陣Amatrix_33的轉置、求逆、求和、求跡、行列式等方法以及矩陣方程求解等，詳見程式碼。

（3）旋轉向量和尤拉角

旋轉向量羅德里格斯公式：

以ZY X 為例的尤拉角：

1. 繞物體的 Z 軸旋轉，得到偏航角 yaw;
2. 繞旋轉之後的 Y 軸旋轉，得到俯仰角 pitch; 3. 繞旋轉之後的 X 軸旋轉，得到滾轉角 roll。

（4）四元數

（5）相似、仿射、射影變換

（6）實踐：Eigen幾何模組

（7）視覺化演示

4、李群與李代數

（1）李群與李代數基礎

1）群

群(Group)是一種集合加上一種運算的代數結構。我們把集合記作 A，運算記作 ·，那麼群可以記作 G = (A, ·)。群要求這個運算滿足以下幾個條件:

1. 封閉性: ∀a1,a2 ∈A, a1 ·a2 ∈A.
2. 結合律: ∀a1,a2,a3 ∈ A, (a1 ·a2)·a3 = a1 ·(a2 ·a3).
3. 么元: ∃a0 ∈A, s.t. ∀a∈A, a0 ·a=a·a0 =a. 4. 逆: ∀a∈A, ∃a−1 ∈A, s.t. a·a−1 =a0.

矩陣中常見的群有:一般線性群 GL(n)，特殊正交群 SO(n)，特殊歐氏群 SE(n)，群結構保證了在群上的運算具有良好的性質，而群論則是研究群的各種結構和性質的理論。李群是指具有連續(光滑)性質的群。 SO(n) 和 SE(n)，它們在實數空間上是連續的，都是李群。

2）李代數的引出

考慮任意旋轉矩陣 R，我們知道它滿足:

（假設t0=0，同時在 t0 附近，設 φ 保持為常數 φ(t0) = φ0）

3）李代數的定義

每個李群都有與之對應的李代數。李代數描述了李群的區域性性質。通用的李代數的定義如下:

李代數由一個集合 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 ∀X∈V,[X,X]=0.
4. 雅可比等價 ∀X,Y ,Z ∈ V,[X,[Y ,Z]] + [Z,[Y ,X]] + [Y ,[Z,X]] = 0.

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

4）李代數so(3)

5）李代數se(3)

（2）指數與對數對映

1）SO(3)上的指數對映

2）SE(3)上的指數對映

（3）李代數求導與擾動模型

1）BCH公式與近似形式

兩個李代數指數對映乘積的完整形式，由 Baker-Campbell-Hausdorff 公式(BCH 公式)x給出：

特別地，考慮 SO(3) 上的李代數 ln (exp (φ∧1 ) exp (φ∧2 ))∨，當 φ1 或 φ2 為小量時，小量二次以上的項都可以被忽略掉。此時，BCH 擁有線性近似表達:

同樣的，對於 SE(3)，亦有類似的BCH 近似公式:

2）SO(3)李代數上的求導

設某個時刻小蘿蔔的位姿為 T。它觀察到了一個世界座標位於 p 的點，產生了一個觀測資料 z。那麼，由座標變換關係知:z = T p + w. 我們通常會計算理想的觀測與實際資料的誤差: e = z − T p.

求解此問題，需要計算目標函式 J 關於變換矩陣 T 的導數。我們經常會構建與位姿有關的函式，然後討論該函式關於位姿的導數，以調整當前的估計值。使用李代數解決求導問題的思路分為兩種: 1. 用李代數表示姿態，然後對根據李代數加法來對李代數求導。 2. 對李群左乘或右乘微小擾動，然後對該擾動求導，稱為左擾動和右擾動模型。

3）李代數求導

4）擾動模型（左乘）

5）SE(3)上李代數上的求導

（4）實踐：Sophus

一個較好的李代數庫是 Strasdat 維護的 Sophus 庫。Sophus庫有模板類和非模板類的版本。你可以輸入以下命令獲得非模板類的 Sophus:

git clone https://github.com/strasdat/Sophus.git
2 cd Sophus
3 git checkout a621ff

（5）相似變換群與李代數

（6）小結

本講引入了李群 SO(3) 和 SE(3)，以及它們對應的李代數 so(3) 和 se(3)。我們介紹了位姿在它們上面的表達和轉換，然後通過 BCH 的線性近似，我們可以對位姿進行求導和擾動了。

5、相機與影像

前面兩講中，我們介紹了“機器人如何表示自身位姿”的問題，部分地解釋了 SLAM 經典模型中變數的含義和運動方程部分。本講，我們要討論“機器人如何觀測外部世界”，也就是觀測方程部分。而在以相機為主的視覺 SLAM 中，觀測主要是指相機成像的過程。

（1）相機模型

1）針孔相機模型

針孔相機模型

設 P 的座標為 [X, Y, Z]T ，P ′ 為 [X′, Y ′, Z′]T ，並且設物理成像平面到小孔的距離為 f(焦距)。那麼，根據三角形相似關係，有:

2）畸變

徑向畸變的兩種型別

切向畸變來源示意圖

徑向畸變

切向畸變

3）雙目相機模型

畫素點可能存在的位置

雙目相機的成像模型

4）RGB-D相機模型

（2）影像

（3）實踐：影像的存取與訪問

1）安裝 OpenCV

2）操作OpenCV影像

（4）實踐：拼接點雲

6、非線性優化

（1）狀態估計問題

1）最大後驗與最大似然

貝葉斯法則左側通常稱為後驗概率。它右側的 P (z|x) 稱為似然，另一部分 P (x) 稱為先驗。直接求後驗分佈是困難的，但是求一個狀態最優估計，使得在該狀態下，後驗概率最大化(Maximize a Posterior，MAP)，則是可行的:

請注意貝葉斯法則的分母部分與待估計的狀態 x 無關，因而可以忽略。貝葉斯法則告訴我們，求解最大後驗概率，相當於最大化似然和先驗的乘積。進一步，我們當然也可以說，對不起，我不知道機器人位姿大概在什麼地方，此時就沒有了先驗。那麼，可以求解 x 的最大似然估計(Maximize Likelihood Estimation, MLE):

“在什麼樣的狀態下，最可能產生現在觀測到的資料”。這就是最大似然估計的直觀意義。

2）最小二乘的引出

（2）非線性最小二乘

1. 給定某個初始值 x0。
2. 對於第 k 次迭代，尋找一個增量 ∆xk，使得 ∥f (xk + ∆xk)∥2 達到極小值。 3. 若 ∆xk 足夠小，則停止。
4. 否則，令 xk+1 = xk + ∆xk，返回 2.

1）一階和二階梯度法

最速下降法

一階梯度法（最速下降法）：

二階梯度法（牛頓法）：

2）Gauss-Newton

1. 給定初始值 x0。
2. 對於第 k 次迭代，求出當前的雅可比矩陣 J(xk) 和誤差 f(xk)。 3. 求解增量方程:H∆xk = g.
4. 若 ∆xk 足夠小，則停止。否則，令 xk+1 = xk + ∆xk，返回 2.

3）Levenberg-Marquadt

1.給定初始值 x0，以及初始優化半徑 μ。

2.對於第 k 次迭代，求解:

這裡 μ 是信賴區域的半徑，D 將在後文說明。

3.計算 ρ。

4.若 ρ > 43 ，則 μ = 2μ;

5.若 ρ < 41 ，則 μ = 0.5μ;

6.如果 ρ 大於某閾值，認為近似可行。令 xk+1 = xk + ∆xk。

7.判斷演算法是否收斂。如不收斂則返回 2，否則結束。

4）小結

這裡只羅列了最常見的兩種非線性優化方案，Gauss Newton 和 Levernberg-Marquardt

（3）實踐：Ceres

1）Ceres簡介

2）安裝Ceres

3）使用Ceres擬合曲線

（4）實踐：g2o

1）圖優化理論簡介

圖優化，是把優化問題表現成圖(Graph)的一種方式。這裡的圖是圖論意義上的圖。一個圖由若干個頂點(Vertex)，以及連線著這些節點的邊(Edge)組成。進而，用頂點表示優化變數，用邊表示誤差項。於是，對任意一個上述形式的非線性最小二乘問題，我們可以構建與之對應的一個圖。

2）g2o的編譯與安裝

3）使用g2o擬合曲線

（5）小結

本節介紹了 SLAM 中經常碰到的一種非線性優化問題:由許多個誤差項平方和組成的最小二乘問題。我們介紹了它的定義和求解，並且討論了兩種主要的梯度下降方式:Gauss- Newton 和 Levenberg-Marquardt。在實踐部分中，我們分別使用了 Ceres 和 g2o 兩種優化庫求解同一個曲線擬合問題，發現它們給出了相似的結果。