【導(dǎo)讀】SLAM是“Simultaneous Localization And Mapping”的縮寫(xiě)，可譯為同步定位與建圖。最早，SLAM 主要用在機(jī)器人領(lǐng)域，是為了在沒(méi)有任何先驗(yàn)知識(shí)的情況下，根據(jù)傳感器數(shù)據(jù)實(shí)時(shí)構(gòu)建周?chē)h(huán)境地圖，同時(shí)根據(jù)這個(gè)地圖推測(cè)自身的定位。因此本文以簡(jiǎn)單清晰的文字為大家介紹了視覺(jué)V-SLAM。

簡(jiǎn)單的V-SLAM介紹，就當(dāng)入門(mén)：）。

首先，從雙目立體幾何（stereo geometry）原理開(kāi)始來(lái)定義外極（epipolar）約束：兩個(gè)攝像頭光心分別是 c0 和 c1，3-D 空間點(diǎn) p 在兩個(gè)圖像平面的投影點(diǎn)分別是 x0 和 x1，那么直線 c0c1 和兩個(gè)圖像平面的交點(diǎn)即外極點(diǎn)（epipole）e0 和 e1，pc0c1 平面稱(chēng)為外極平面（epipolar plane），它和兩個(gè)圖像平面的交線 l0 和 l1 即外極線（epipolar line）；可以看到兩個(gè)攝像頭坐標(biāo)系之間的轉(zhuǎn)換滿足（R，t），同時(shí)說(shuō)明攝像頭 c0 的圖像點(diǎn) x0 在攝像頭 c1 的圖像對(duì)應(yīng)點(diǎn) x1 一定落在其外極線 l1，反之依然；

（a）對(duì)應(yīng)一條光線的外極線 （b）對(duì)應(yīng)的外極線集合和外極平面 外極線約束

這里需要定義一個(gè)本質(zhì)矩陣（essential matrix）E 和一個(gè)基礎(chǔ)矩陣（fundamental matrix）F：E = [t]×R，其中 [t]× 是反對(duì)稱(chēng)矩陣，即定義向量

那么

而 F = A?TEA?1，其中 A 是攝像頭內(nèi)參數(shù)矩陣；對(duì)矩陣 E 和矩陣 F 來(lái)說(shuō)，均滿足外極約束方程x1TEx0 =0，x1TFx0 =0

前者是攝像頭已標(biāo)定情況下圖像特征點(diǎn)的對(duì)應(yīng)關(guān)系，后者是攝像頭未標(biāo)定情況下圖像特征點(diǎn)的對(duì)應(yīng)關(guān)系；

其次，得到外極線約束（以F矩陣為例，E矩陣同樣）如下

l1=Fx0，l0=x1F，

以及，外極點(diǎn)約束（以F矩陣為例，E矩陣同樣）如下

Fe0=0，F(xiàn)Te1=0；

根據(jù)上面圖像特征點(diǎn)的外極約束方程，有8 點(diǎn)算法求解 F 和 E，以 F 為例，給定一組匹配特征點(diǎn)（n> 7）

定義F矩陣元素fij（i=1～3，j=1～3），那么線性齊次方程組為

簡(jiǎn)記為

Qf = 0

做SVD得到Q = USVT，而最終F的解是V最右的奇異向量（singular vector）；

另外，因?yàn)镋矩陣完全由攝像頭外參數(shù)（R和t共6個(gè)未知參數(shù)，但有一個(gè)不確定平移尺度量）決定，所以存在5 點(diǎn)算法求解E矩陣；

可以分解E得到攝像頭外參數(shù)，其步驟如下：

i. 同樣對(duì)E矩陣做奇異值分解（SVD）：

ii. 那么第一個(gè)攝像頭投影矩陣簡(jiǎn)單設(shè)為

而第二個(gè)攝像頭矩陣P2有四種可能情況，如圖（a）-（d）所示：

其中

從E矩陣分解得到R和t

根據(jù)視圖方向與攝像頭中心到3-D點(diǎn)的方向之間夾角可以發(fā)現(xiàn)，四個(gè)可能中只有情況（a）是合理的解；

確定兩個(gè)視角的姿態(tài)之后，匹配的特征點(diǎn) x，x’可以重建其 3-D 坐標(biāo)X，即三角化（triangulation）理論；首先存在一個(gè)線性解：設(shè)兩個(gè)攝像頭投影矩陣為 P 和 P’，相應(yīng)的它們列向量為pi，pi’，i=1～3，則有方程組：AX= 0

其中

但一些誤差干擾的存在，上述線性解是不存在的；所以需要一個(gè)非線性的解，這里采用F 矩陣定義的外極約束方程xTFx’= 0，得到最小化誤差函數(shù)為

下面采用外極線 l, l’的來(lái)約束定義誤差，如圖所示，將目標(biāo)函數(shù)重寫(xiě)為

外極線定義的誤差

另外，在已知重建的 3-D 點(diǎn)集，如何和新視角的 2-D 圖像特征點(diǎn)匹配呢？這個(gè)問(wèn)題解法稱(chēng)為PnP（Perspective n Points），算法如下：

i. 首先，3D 點(diǎn)表示為 4 個(gè)控制點(diǎn)的加權(quán)和；

ii. 控制點(diǎn)坐標(biāo)是求解的（12）未知數(shù)；

iii. 3D點(diǎn)投影到圖像上以控制點(diǎn)坐標(biāo)建立線性方程；

iv. 控制點(diǎn)坐標(biāo)表達(dá)為零特征向量（null eigenvectors）線性組合；

v. 上面組合的權(quán)重（bi）是新未知數(shù)（<4）；

vi. 增加剛體（rigidity）約束以得到bi二次方程；

vii. 根據(jù)bi數(shù)目（無(wú)論線性化，或重新線性化）求解。

（注：有時(shí)候3-D-2-D匹配比3-D之間匹配的精度高）

PnP求解的示意圖

這里需要補(bǔ)充兩個(gè)概念，一是魯棒估計(jì)的隨機(jī)樣本共識(shí)法（RANSAC，RANdom SAmple Consensus），另一個(gè)是全局優(yōu)化的集束修正法（BA，bundle adjustment）：

i. RANSAC的目的是在包含異常點(diǎn)（outlier）的數(shù)據(jù)集上魯棒地?cái)M合一個(gè)模型，如圖 2-12 所示：

1. 隨機(jī)選擇（最小）數(shù)據(jù)點(diǎn)子集并實(shí)例化（instantiate）模型;

2. 基于此模型，將所有數(shù)據(jù)點(diǎn)分類(lèi)為內(nèi)點(diǎn)（inlier）或異常點(diǎn)；

3. 迭代重復(fù) 1-2 步；

4. 選擇最大的內(nèi)地集，以此重新估計(jì)最終模型。

RANSAC示意圖

ii. BA的目的是優(yōu)化全局參數(shù)估計(jì)，對(duì) SLAM 來(lái)說(shuō)，結(jié)構(gòu)重建的 3-D 點(diǎn)X和其對(duì)應(yīng)的 2-D 特征點(diǎn)x，還有估計(jì)的視角變換參數(shù)（甚至包括攝像頭內(nèi)參數(shù)）P，位于一個(gè)重投影（reprojection）誤差函數(shù) D 最小化框架下（如圖所示），即

BA示意圖

這里集束（Bundle）指2-D點(diǎn)和3-D點(diǎn)之間的光線集，而修正（Adjustment）是指全局優(yōu)化過(guò)程；其解法是非線性迭代的梯度下降法，如Gauss-Newton 方法和其修正 Levenberg-Marquardt 方法，因?yàn)閱?wèn)題自身的特性，這里的雅可比矩陣是非常稀疏的；另外，只取重建 3-D 點(diǎn)修正的話，稱(chēng)為 structure only BA，而只取視角變換修正的話，稱(chēng)為 motion-only BA；

在以上基礎(chǔ)之上，再重新審視SLAM過(guò)程：

i. 首先要提到概念 VO，即視覺(jué)里程計(jì)（visual odometry）；VO 是 SLAM 的一部分，VO 主要是估計(jì)視角之間的變換，或者運(yùn)動(dòng)參數(shù)，它不需要輸出制圖（mapping）的結(jié)果，而且 BA 就是 motion-only 的模式；

ii. SLAM 方法分兩種途徑，一種是遞推濾波器方法，另一種是批處理估計(jì)方法；

a)濾波器方法，比如卡爾曼濾波遞推估計(jì)，實(shí)際上建立一個(gè)狀態(tài)空間的觀測(cè)模型和狀態(tài)轉(zhuǎn)換（運(yùn)動(dòng)）模型；觀察模型描述當(dāng)攝像頭姿態(tài)和地標(biāo)位置（來(lái)自于地圖）已知時(shí)觀測(cè)（地標(biāo)）的概率; 運(yùn)動(dòng)模型是系統(tǒng)狀態(tài)（攝像頭姿態(tài)）轉(zhuǎn)換的概率分布，即馬爾可夫過(guò)程; 那么在遞歸貝葉斯估計(jì)中，同時(shí)更新系統(tǒng)狀態(tài)和建立的地圖，其中融合來(lái)自不同視角的觀測(cè)來(lái)完成制圖，而估計(jì)系統(tǒng)狀態(tài)可計(jì)算攝像頭的姿態(tài)，即定位問(wèn)題;

b)批處理估計(jì)方法，也稱(chēng)“關(guān)鍵幀”方法，其步驟是：

1）首先通過(guò)選擇的頭兩（關(guān)鍵）幀，采用雙目幾何的特征點(diǎn)匹配得到初始的3-D點(diǎn)云重建；

2）正常模式：假設(shè) 3D 地圖可用，并且估計(jì)出攝像頭增量運(yùn)動(dòng)，跟蹤特征點(diǎn)并使用 PnP 進(jìn)行攝像頭姿態(tài)估計(jì)；

3）恢復(fù)模式：假設(shè) 3D 地圖可用，但跟蹤失敗故沒(méi)有增量運(yùn)動(dòng)，可相對(duì)于先前重建的地圖重新定位（re-localize）攝像頭姿勢(shì)；

4）關(guān)鍵幀 BA：保持一個(gè)“關(guān)鍵幀”子集，狀態(tài)向量是所有關(guān)鍵幀的 3D 地標(biāo)和對(duì)應(yīng)攝像頭姿勢(shì)，BA 可以在與跟蹤模塊并列的線程中調(diào)整狀態(tài)估計(jì)；

（注意：關(guān)鍵幀的選擇策略是算法性能很重要的一個(gè)因素）

SLAM 中的閉環(huán)（loop closure）問(wèn)題：當(dāng)攝像頭又回到原來(lái)位置，稱(chēng)為閉環(huán)，需要檢測(cè)閉環(huán)，并以此在 BA 框架下優(yōu)化結(jié)構(gòu)和運(yùn)動(dòng)估計(jì)；閉環(huán)檢測(cè)和重定位是類(lèi)似的，可以基于圖像特征的匹配實(shí)現(xiàn)，俗稱(chēng)“基于圖像的重定位（image-based re-localization），當(dāng)關(guān)鍵幀子集較大的時(shí)候，需要對(duì)特征匹配進(jìn)行壓縮和加速，比如詞包（bag of words）法和K維-樹(shù)（KD-tree）數(shù)據(jù)結(jié)構(gòu)等等；

SLAM 中的傳感器可以是單目、雙目、深度傳感器（RGB-D）甚至激光雷達(dá)，也可以和 IMU 融合，稱(chēng)為 VINS（visual inertial navigation system）。

附錄：G-N 和 L-M 的非線性最小二乘算法

假設(shè)有觀測(cè)向量 zi’，其預(yù)測(cè)模型為 zi = zi(x)，其中x為模型參數(shù)；那么最小二乘（LS）法就是最小化如下代價(jià)函數(shù)：平方誤差加權(quán)和（weighted Sum of Squared Error，SSE)

其中 Wi 是一個(gè)任意對(duì)稱(chēng)正定（symmetric positive definite，SPD) 矩陣，特征誤差函數(shù)為

雅可比矩陣J和黑森（Hessian）矩陣H的計(jì)算為

那么 H 近似為

H≈J?WJ

作為梯度下降法，其 G-N 迭代的步進(jìn)量即 z → z + delta z，由下面方程組計(jì)算

對(duì)于上面 G-N 的迭代步進(jìn)量計(jì)算，可能左邊的矩陣不可逆。為此，一種改進(jìn)的方法是在對(duì)角線元素中加入阻尼因子（Damped factor），即 L-M 迭代的步進(jìn)量計(jì)算變成如下方程組