高翔《視覺SLAM十四講》從理論到實踐

 

目錄

第1講 前言：本書講什麼；如何使用本書；

第2講 初始SLAM：引子-小蘿蔔的例子；經典視覺SLAM框架；SLAM問題的數學表述；實踐-編程基礎；

第3講 三維空間剛體運動 旋轉矩陣；實踐-Eigen；旋轉向量和歐拉角；四元數；類似、仿射、射影變換；實踐-Eigen幾何模塊；可視化演示；

第4講 李羣與李代數 李羣李代數基礎；指數與對數映射；李代數求導與擾動模型；實踐-Sophus；類似變換羣與李代數；小結；

第5講 相機與圖像 相機模型；圖像；實踐-圖像的存取與訪問；實踐-拼接點雲；

第6講 非線性優化 狀態估計問題；非線性最小二乘；實踐-Ceres；實踐-g2o；小結；

第7講 視覺里程計1 特徵點法；特徵提取和匹配；2D-2D：對極幾何；實踐-對極約束求解相機運動；三角測量；實踐-三角測量；3D-2D：Pnp；實踐-求解PnP；3D-3D：ICP；實踐-求解ICP；小結；

第8講 視覺里程計2 直接法的引出；光流（Optical Flow）；實踐-LK光流；直接法（Direct Methods）；實踐-RGBD的直接法；

第9講 實踐章：設計前端 搭建VO前端；基本的VO-特徵提取和匹配；改進-優化PnP的結果；改進-局部地圖；小結

第10講 後端1 概述；BA與圖優化；實踐-g2o；實踐-Ceres；小結

第11講 後端2 位姿圖（Pose Graph）；實踐-位姿圖優化；因子圖優化初步；實踐-gtsam；

第12講 迴環檢測 迴環檢測概述；詞袋模型；字典；類似度計算；實驗分析與評述；

第13講 建圖 概述；單目稠密重建；實踐-單目稠密重建；實驗分析與討論；RGBD稠密建圖；TSDF地圖和Fusion系列；小結；

第14講 SLAM：如今與將來 當前的開源方案；將來的SLAM話題；

附錄A 高斯分佈的性質

附錄B ROS入門

 

第1講 前言

1.1 本書講什麼

講的是SLAM（Simultaneous Localization And Mapping，同步定位與成圖），即利用傳感器來進行機器人的自身定位以及對周圍環境的成圖。根據傳感器來劃分主要分爲雷達SLAM和視覺SLAM。這裏固然主要講的是視覺SLAM。

從定義上能夠看出SLAM主要解決的是「自身定位」和「周圍環境的成圖」。

這裏主要把SLAM系統分紅幾個模塊：視覺里程計、後端優化、建圖以及迴環檢測。

這是一個很複雜的過程，共分十四講來說述，每一講都有一個主題，而後介紹該主題的算法和實踐，即從理論到實踐。

 ok，這講就到這裏了。

 

第2講 初始SLAM

這講主要是講SLAM的基本框架，以及開發前期準備工做。

如上圖所述，SLAM基本框架就是這樣。一個機器人，靠一個攝像頭來感知周圍的世界並評估自身的位置，它須要利用Visual Odometry視覺里程計來經過相鄰兩張相片來計算姿態參數估算距離角度這些用來計算自身位置xyz以及恢復圖像上各點的位置pxpypz，因爲有上一步有累積偏差須要Optimization後端優化，對於周圍環境的感知須要Mapping地圖構建並配準，最後須要Loop Closure迴環檢測計算閉合偏差並修正。

前期準備工做有：

準備一臺電腦和一個Turtlebot機器人（沒有也沒關係，只要有一臺Kinect相機就能夠了，你能夠用手拿着它模擬機器人的走動，它會還原你的位置信息）。這臺電腦和Turtlebot機器人都是安裝的Linux操做系統，沒錯，機器人身上有一臺電腦做爲控制，電腦就是一個高級點的單片機，這是它的大腦控制着它發射信息行走以及計算彙總，而另外一臺電腦做爲咱們的服務器它接收機器人發送的信息如圖片等以及給機器人發送指令。但願安裝的是Ubuntu這款Linux操做系統，由於咱們之後將在該操做系統下下載安裝軟件。ROS軟件就不用說了，這是機器人操做系統，是必備的，能夠利用它與機器人完成交互傳遞信息發送指令等。另外咱們還須要配置開發環境，由於咱們須要開發咱們本身的程序，調節參數。這是ROS所不具有的。

配置開發環境: Kdevelop。

編寫實例程序: HelloSLAM。

 

第3講 三維空間剛體運動

這講主要是講三維幾何空間中剛體的運動方程。

a'=Ra+t

其中R爲旋轉矩陣，t爲平移矩陣

將R和t融合到一個矩陣中，將上式變爲線性方程

那麼編程如何實現上式呢？用Eigen開源庫。

Eigen使用方法：添加頭文件，編寫程序。

旋轉向量與旋轉矩陣之間的變換：，其中n爲旋轉向量，R爲旋轉矩陣

旋轉向量只須要三個變量，比旋轉矩陣三維矩陣的9個變量要簡潔，節省運算成本。

由旋轉向量到四元數：因爲旋轉矩陣的冗餘，加上旋轉向量和角度的奇異性，引入四元數，即用複數來表示旋轉從而避免了奇異性。

用四元數來表示旋轉：設空間點p=[x,y,z]，以及一個由旋轉軸和角度指定的旋轉，那麼旋轉後的點p'用四元數怎麼表示呢？咱們知道使用矩陣描述的話p'=Rp。而用四元數表達：

1. 把三維空間點用一個虛四元數描述：

p=[0, x, y, z]=[0, v]

這至關於咱們把四元數的三個虛部與空間中的三個軸相對應。用四元數q表示這個旋轉：

q=[cos(θ/2), ncos(θ/2)]

那麼旋轉後的點p'便可表示爲這樣的乘積：

p'=qpq^(-1)

2. 四元數到旋轉矩陣的轉換

那麼用Eigen演示如何進行各類旋轉變換。在Eigen中使用四元數、歐拉角和旋轉矩陣，演示它們三者之間的變換關係。

Eigen中各個數據類型總結以下：

• 旋轉矩陣（3×3）：Eigen::Matrix3d。
• 旋轉向量（3×1）：Eigen::AngleAxisd。
• 歐拉角（3×1）：Eigen::Vector3d。
• 四元數（4×1）：Eigen::Quaterniond。
• 歐氏變換矩陣（4×4）：Eigen::Isometry3d。
• 仿射變換（4×4）：Eigen::Affine3d。
• 射影變換（4×4）：Eigen::Perspective3d。

本講結束。

 

第4講 李羣與李代數

三維旋轉矩陣構成了特殊正交羣SO(3)，而變換矩陣構成了特殊歐氏羣SE(3)

 

 但不管SO(3)，仍是SE(3)，它們都不符合加法封閉性，即加以後再也不符合旋轉矩陣的定義，可是乘法卻知足，將這樣的矩陣稱爲羣。即只有一種運算的集合叫作羣。

 羣記做G=(A, .)，其中A爲集合，.表示運算。羣要求運算知足如下幾個條件：

（1）封閉性。

（2）結合律。

（3）幺元。一種集合裏特殊的數集。

（4）逆。

能夠證實，旋轉矩陣集合和矩陣乘法構成羣，而變換矩陣和矩陣乘法也構成羣。

介紹了羣的概念以後，那麼，什麼叫李羣呢？

李羣就是連續（光滑）的羣。一個剛體的運動是連續的，因此它是李羣。

每一個李羣都有對應的李代數。那麼什麼叫李代數呢？

李代數就是李羣對應的代數關係式。

李羣和李代數之間的代數關係以下：

可見二者之間是指數與對數關係。

 那麼exp(φ^)是如何計算的呢？它是一個矩陣的指數，在李羣和李代數中，它稱爲指數映射。任意矩陣的指數映射能夠寫成一個泰勒展開式，可是隻有在收斂的狀況下才會有結果，它的結果仍然是一個矩陣。

 一樣對任意一元素φ，咱們亦可按此方式定義它的指數映射：

 因爲φ是三維向量，咱們能夠定義它的模長θ和方向向量a知足使φ=θa。那麼，對於a^，能夠推導出如下兩個公式：

 設a=(cosα, cosβ, cosγ),可知(cosα)^2+(cosβ)^2+(cosγ)^2=1

 （1）a^a^=aa^T-I

 （2）a^a^a^=-a^

 上面兩個公式說明了a^的二次方和a^的三次方的對應變換，從而可得：

exp(φ^)=exp(θa^)=∑(1/n!(θa^)ⁿ)=...=a^a^+I+sinθa^-cosθa^a^=(1-cosθ)a^a^+I+sinθa^=cosθI+(1-cosθ)aa^T+sinθa^.

回憶前一講內容，它和羅德里格斯公式一模一樣。這代表，so(3)實際上就是由旋轉向量組成的空間，而指數映射即羅德里格斯公式。經過它們咱們把so(3)中任意一個向量對應到了一個位於SO(3)中的旋轉矩陣。反之，若是定義對數映射，咱們也能把SO(3)中的元素對應到so(3)中：

但一般咱們會經過跡的性質分別求解轉角和轉軸，那種方式會更加省事一些。

 OK，講了李羣和李代數的對應轉換關係以後，有什麼用呢？

主要是經過李代數來對李羣進行優化。好比說，對李羣中的兩個數進行運算，對應的他們的李代數會有什麼變化？

首先是，兩個李羣中的數進行乘積時，對應的李代數是怎麼樣的變化，是否是指數變化呢？可是注意，李羣裏的數是矩陣，不是常數，因此不知足ln(exp(A+B))=A+B，由於A,B是矩陣，不是常數，那麼是怎麼的對應關係呢？

是

 

 

第5講 相機與圖像

以前介紹了機器人的運動方程，那麼如今來說相機的原理。相機最先是根據小孔成像原理產生的最先相機。後面又有了加凸透鏡的相機。

相機模型：

這個公式表示了世界座標Pw到像素座標Puv的轉換。表示了從世界座標(Pw)到相機座標(經過外參R,t)再到像素座標(經過K內方位元素)的轉換。

對於凸透鏡的鏡頭畸變則採用：

進行徑向畸變糾正和切向畸變糾正。其中k1,k2,k3爲徑向畸變參數，而p1,p2爲切向畸變參數。

而對於相機標定，即求解內方位參數K，能夠採用OpenCV, Matlab, ROS三種方法求解。參照：連接。

接下來是使用OpenCV處理圖像的示例。OpenCV是處理圖像的類庫，是很是重要的。

那麼接下來呢，固然是將上一步求的相機內參數應用到實際的圖像點雲數據處理上。

有了相機內參數和相片，就能夠恢復像點的像點X,Y,Z座標嗎？不能，由可知，要求像點X,Y,Z除了內參數矩陣外，還須要像點的(x,y,w)，而從像片上只能獲得x,y，那麼w是距離，怎麼得到呢？經過RGD-D相機能夠得到深度數據，即w。（經過Kinect獲取彩色和深度圖像，保存 顯示）

假設你已經經過Kinect得到彩色和深度圖像了，那麼就能夠經過上式恢復像素的相機座標點了(即經過彩色圖像(x,y)和深度圖像(w)就能夠獲得點雲了)。而後就能夠(經過IMU獲得的位姿參數)當前點的R,t這些旋轉矩陣和平移矩陣(外參)恢復到它的世界座標Pw(Xw, Yw, Zw)。

以上兩個文件夾分別爲彩色圖像和深度圖像，一一對應。

接下來來寫一個經過兩張彩色圖像和對應的兩張深度圖像獲得點雲數據，而且將兩個點雲數據融合生成地圖的程序：

程序略。

拼接完成的數據保存到map.pcd文件中。能夠經過PCL的點雲顯示程序來顯示保存的map.pcd。10萬多個點加載了進來，可是顏色在windows程序下怎麼沒有顯示出來。在Linux下試一下看看。

pcd_viewer.exe自己有問題，pcl_viewer.exe在Windows下找不到。pcl_visualization能夠顯示彩色圖像，可是隻是一個連接庫文件。

原來是使用pcd_viewer打開pcd文件後，按5鍵就能夠渲染RGB色彩了，而按4鍵則能夠渲染深度圖像。衆多使用說明請見：PCL Visualization overview。

 

 

第6講 非線性優化

經過傳感器的運動參數來估計運動方程(位姿x)，經過相機的照片來估計物體的位置(地圖y)，都是有噪聲的。由於運動參數和照片都有噪聲，因此須要進行優化。而過去卡爾曼濾波只關心當前的狀態估計，而非線性優化則對全部時刻採集的數據進行狀態估計，被認爲優於卡爾曼濾波。因爲要估計全部的採集數據，因此待估計變量就變成：

x={x1,…,xN,y1,….,yM}

因此對機器人狀態的估計，就是求已知輸入數據u(傳感器參數)和觀測數據z(圖像像素)的條件下，計算狀態x的條件機率分佈（也就是根據u和z的數據事件好壞來估計x的優劣事件機率狀況，這其中包含着關聯，就好像已知一箱子裏面有u和z個劣質的商品，求取出x個全是好商品的機率，一樣的樣本點，可是從不一樣角度分析能夠得出不一樣的事件，不一樣的事件機率之間能夠經過某些已知數據得出另些事件的機率，經過一系列函數式計算得出）：

P(x|z, u)

在不考慮運動方程，只考慮觀測照片z的狀況下求x（這個過程也稱SfM運動恢復），那麼就變成P(x|z)。這種根據已知求未知的狀況能夠經過貝葉斯公式求解，P(x|z)=P(x)P(z|x)/P(z)。

又由於只求x因此忽略P(z)，因此P(x|z)=P(x)P(z|x)

那麼根據貝葉斯公式的含義，P(x)爲先驗機率，P(z|x)爲似然機率。

而P(x)是不知道，因此主要求P(z|x)也就是最大似然機率。

最大似然的機率用天然語言描述就是在什麼狀態下最有可能產生當前的照片結果！

如何求呢？

咱們知道z與x之間存在一個函數式：zk,j = h(yj, xk)+vk,j,即觀測方程

如今要求x致使z出現的機率，要求z的最大機率，因爲v是偏差，符合高斯分佈vk~N(0, Qk,j)，因此z也符合高斯分佈，即P(zi,k|xk, yj)=N(h(yj, xk), Qk,j)。

因此求多維的機率最大值即爲

因此對於全部的運動和觀測，有：

從而獲得了一個整體意義下的最小二乘問題。它的最優解等於狀態的最大似然估計。

它的意義是說P(z,u|x)代表咱們把估計的軌跡和地圖(xk,yj)代入SLAM的運動、觀測方程中時，它們並不會完美的成立。這時候怎麼辦呢？咱們把狀態的估計值進行微調，使得總體的偏差降低一些。它通常會到極小值。這就是一個典型的非線性優化過程。

由於它非線性，因此df/dx有時候很差求，那麼能夠採用迭代法（有極值的話，那麼它收斂，一步步逼近）：

1.給定某個初始值x0。

2.對於第k次迭代，尋找一個增量△xk，使得||f(xk+△xk)||2²達到極小值。

3. 若△xk足夠小，則中止。

4. 不然，令xk+1=xk+ △xk，返回2。

這樣求導問題就變成了遞歸逼近問題，那麼增量△xk如何肯定？

這裏介紹三種方法：

（1）一階和二階梯度法

將目標函數在x附近進行泰勒展開：

其中J爲||f(x)||2關於x的導數（雅克比矩陣），而H則是二階導數（海塞(Hessian)矩陣）。咱們能夠選擇一階或二階，增量不一樣。但都是爲了求最小值，使函數值最小。

但最速降低法降低太快容易走出鋸齒路線，反而增長了迭代次數。而牛頓法則須要計算目標函數的H矩陣，這在問題規模較大時很是困難，咱們一般爲了不求解H。因此，接下來的兩種最經常使用的方法：高斯牛頓法和列文伯格-馬夸爾特方法。

（2）高斯牛頓法

將f(x)一階展開：

這裏J(x)爲f(x)關於x的導數，其實是一個m×n的矩陣，也是一個雅克比矩陣。如今要求△x，使得||f(x+△x)|| 達到最小。爲求△ x，咱們須要解一個線性的最小二乘問題：

這裏注意變量是△ x，而非以前的x了。因此是線性函數，好求。爲此，先展開目標函數的平方項：

求導，令其等於零從而獲得：

咱們稱爲高斯牛頓方程。咱們把左邊的係數定義爲H，右邊定義爲g，那麼上式變爲：

H △x=g

跟第（1）種方法對比能夠發現，咱們用J(x)^TJ(x)代替H的求法，從而節省了計算量。因此高斯牛頓法跟牛頓法相比的優勢就在於此，步驟列於下：

1.給定初始值x0。

2.對於第k次迭代，求出當前的雅克比矩陣J(xk)和偏差f(xk)。

3.求解增量方程：H△xk=g。

4.若△xk足夠小，則中止。不然，令xk+1=xk+△xk，返回2。

可是，仍是有缺陷，就是它要求咱們所用的近似H矩陣是可逆的（並且是正定的），但實際數據中計算獲得的J^TJ卻只有半正定性。也就是說，在使用Gauss Newton方法時，可能出現J^TJ爲奇異矩陣或者病態（ill-condition）的狀況，此時增量的穩定性較差，致使算法不收斂。更嚴重的是，就算咱們假設H非奇異也非病態，若是咱們求出來的步長△x太大，也會致使咱們採用的局部近似（6.19）不夠準確，這樣一來咱們甚至都沒法保證它的迭代收斂，哪怕是讓目標函數變得更大都是可能的。

因此，接下來的Levenberg-Marquadt方法加入了α，在△x肯定了以後又找到了α，從而||f(x+α△x)||²達到最小，而不是直接令α=1。

（3）列文伯格-馬夸爾特方法（Levenberg-Marquadt法）

因爲Gauss-Newton方法中採用的近似二階泰勒展開只能在展開點附近有較好的近似效果，因此咱們很天然地想到應該給△x添加一個信賴區域（Trust Region），不能讓它太大而使得近似不許確。非線性優化中有一系列這類方法，這類方法也被稱之爲信賴區域方法（Trust Region Method）。在信賴區域裏邊，咱們認爲近似是有效的；出了這個區域，近似可能會出問題。

那麼如何肯定這個信賴區域的範圍呢？一個比較好的方法是根據咱們的近似模型跟實際函數之間的差別來肯定這個範圍：若是差別小，咱們就讓範圍儘量大；若是差別大，咱們就縮小這個近似範圍。所以，考慮使用：

來判斷泰勒近似是否夠好。ρ的分子是實際函數降低的值，分母是近似模型降低的值。若是ρ接近於1，則近似是好的。若是ρ過小，說明實際減少的值遠少於近似減少的值，則認爲近似比較差，須要縮小近似範圍。反之，若是ρ比較大，則說明實際降低的比預計的更大，咱們能夠放大近似範圍。

因而，咱們構建一個改良版的非線性優化框架，該框架會比Gauss Newton有更好的效果。

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

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

 

 這裏面的限制條件μ是信賴區域的半徑，D將在後文說明。

3. 計算ρ。

4. 若ρ>3/4，則μ=2μ；

5. 若ρ<1/4，則μ=0.5μ；

6. 若是ρ大於某閾值，認爲近似可行。令xk+1=xk+△xk。

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

最後化簡獲得：

當λ比較小時，接近於牛頓高斯法，當λ比較大時，接近於最速降低法。L-M的求解方式，可在必定程度上避免線性方程組的係數矩陣的非奇異和病態問題，提供更穩定更準確的增量△x。

下面是實踐：

1. Ceres：最小二乘法類庫。

編譯glog、gflags和ceres-solver，而後在項目工程中設置：

include頭文件：

E:\ceres\ceres-solver-1.3.0\ceres-solver-master\config;
E:\ceres\eigen\Eigen 3.3.3\Eigen;
E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\src\windows;
E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\include;
E:\ceres\ceres-solver-1.3.0\ceres-solver-master\include;

庫文件：

E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\lib\Debug;
E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\Debug;
E:\ceres\ceres-bin3\lib\Debug;

Executable目錄：

E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\Debug;
E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\Debug;
E:\ceres\ceres-bin3\bin;

附加依賴：

curve_fitting.lib
ellipse_approximation.lib
sampled_function.lib
robust_curve_fitting.lib
helloworld.lib
helloworld_analytic_diff.lib
helloworld_numeric_diff.lib
rosenbrock.lib
simple_bundle_adjuster.lib
ceres-debug.lib
libglog.lib
libglog_static.lib
gflags_nothreads_static.lib
gflags_static.lib
opencv_calib3d310d.lib
opencv_core310d.lib
opencv_features2d310d.lib
opencv_flann310d.lib
opencv_highgui310d.lib
opencv_imgcodecs310d.lib
opencv_imgproc310d.lib
opencv_ml310d.lib
opencv_objdetect310d.lib
opencv_photo310d.lib
opencv_shape310d.lib
opencv_stitching310d.lib
opencv_superres310d.lib
opencv_ts310d.lib
opencv_video310d.lib
opencv_videoio310d.lib
opencv_videostab310d.lib

 

注意Release/Debug要匹配，VS編譯器要匹配，以及在工程中添加glog和gflags的目錄。

2. g2o：圖優化。

 在視覺slam裏更經常使用的非線性優化是圖優化，它是將非線性優化和圖論結合在一塊兒的理論。它不是僅僅將幾種類型的偏差加在一塊兒用最小二乘法求解，而是增長了位姿最小二乘和觀測最小二乘之間的關係（好比位姿方程xk=f(fk-1,uk)+wk和觀測方程zk,j=h(yj,xk)+vk,j都有一個變量xk聯繫了位姿方程和觀測方程）。因此就用到了圖優化。

藍線表示了位姿最小二乘優化結果（運動偏差），而紅線表示觀測最小二乘結果（觀測偏差），二者之間用位置變量xk關聯，或者說約束。頂點表示優化變量（xk處的u致使的位姿參數偏差和pk,j處的圖像噪聲致使的觀測值偏差），

因此在g2o圖優化中，就能夠將問題抽象爲要優化的點和偏差邊，而後求解。

 

第7講 視覺里程計1

根據相鄰圖像的信息估算相機的運動稱爲視覺里程計（VO）。通常須要先提取兩幅圖像的特徵點，而後進行匹配，根據匹配的特徵點估計相機運動。從而給後端提供較爲合理的初始值。

1. 特徵點：特徵檢測算子SIFT，SURF，ORB等等。

SIFT算子比較「奢侈」，考慮的比較多，對於SLAM算法來講有點太「奢侈」。不太經常使用目前。

ORB算子是在FAST算子基礎上發展起來，在特徵點數量上精簡了，並且加上了方向和旋轉特性(經過求質心)，並改進了尺度不變性（經過構建圖像金字塔）。從而實現經過FAST提取特徵點並計算主方向，經過BRIEF計算描述子的ORB算法。

2. 特徵匹配：特徵匹配精度將影響到後續的位姿估計、優化等。

實踐：特徵提取和匹配

略。

根據提取的匹配點對，估計相機的運動。因爲相機的不一樣，狀況不一樣：

1.當相機爲單目時，咱們只知道2D的像素座標，於是問題是根據兩組2D點估計運動。該問題用對極幾何來解決。

2.當相機爲雙目、RGB-D時，或者咱們經過某種方法獲得了距離信息，那問題就是根據兩組3D點估計運動。該問題一般用ICP來解決。

3.若是咱們有3D點和它們在相機的投影位置，也能估計相機的運動。該問題經過PnP求解。

分別來介紹。

1. 對極幾何

假設咱們從兩張圖像中，獲得了若干對這樣的匹配點，就能夠經過這些二維圖像點的對應關係，恢復出在兩幀之間攝像機的運動。

根據小孔成像原理（s₁p₁=KP, s₂p₂=K(RP+t)）能夠獲得x₂^Tt^Rx₁=0

其中x2,x1爲兩個像素點的歸一化平面上的座標。

代入x₂=K^-1p₂,x₁=K^-1p₁,獲得：

p₂^TK^-Tt^RK^-1p₁=0

上面兩式都稱爲對極約束。

它表明了O1,O2,P三點共面。它同時包含了旋轉R和平移t。把中間部分分別記爲基礎矩陣F和本質矩陣E，能夠進一步化簡對極約束公式：

E=t^R, F=K^-TEK^-1, x₂^TEx₁=p₂^TFp₁=0

它給出了兩個匹配點的空間位置關係，因而，相機位姿估計問題能夠變爲兩步：

1. 根據配對點的像素位置，求出E或者F；

2. 根據E或者F，求出R, t。

因爲K通常已知，因此通常求E。而E=t^R可知共有6個自由度，可是因爲尺度不變性，因此共有5個自由度。能夠利用八點法採用線性代數求解。

那麼獲得本質矩陣E以後，如何求出R,t呢？這個過程是由奇異值分解（SVD）獲得的。設E的SVD分解爲：

E=UΣV^T

其中U,V爲正交陣，Σ爲奇異值矩陣。根據E的內在性質，咱們知道Σ=diag(δ, δ, 0)。對於任意一個E，存在兩個可能的t,R與它對應：

其中Rz(π/2)表示繞Z軸旋轉90度獲得的旋轉矩陣。同時對E來講，E與-E對零等式無影響。因此能夠獲得4種組合。

但根據點的深度值能夠肯定正確的那一組組合。

除了本質矩陣，咱們還要求單應矩陣，單應矩陣是指當特徵點位於同一平面時能夠構建單應矩陣。它的做用是在相機不知足八個參數時，好比只旋轉沒有移動，那麼就能夠經過單應矩陣來估計。求法略。

實踐：對極約束求解相機運動

略。

求得了相機運動參數以後，那麼須要利用相機的運動參數估計特徵點的空間位置。由s₁x₁=s₂Rx₂+t可得s₁和s₂的值（經過s₁x₁^x₁=0=s₂x₁^Rx₂+x₁^t）。由R,t以及深度信息能夠求得特徵像素點的空間點座標。

實踐：三角測量

略。

下面介紹3D-2D方法，即利用RGB-D獲取的深度數據和彩色圖像進行的計算。

能夠直接採用直接線性變換，即不須要求解內外方位元素的變換。直接線性變換即DLT。

而P3P是另外一種解PnP的方法。P3P即只利用三對匹配點，它僅使用三對匹配點，對數據要求較少。推導過程：

經過三角形類似原理，求得OA,OB,OC的長度，從而求得R,t。

 

它存在2個缺點：（1）P3P只利用三個點的信息。當給定的配對點多於3組時，難以利用更多的信息。（2）若是3D點或2D點受噪聲影響，或者存在誤匹配，則算法失效。

那麼，後續人們還提出了許多別的方法，如EPnP、UPnP等。它們利用更多的信息，並且用迭代的方式對相機位姿進行優化，以儘量地消除噪聲的影響。

在SLAM中一般作法是先使用P3P/EPnP等方法估計相機位姿(R,t)，而後構建最小二乘優化問題對估計值(R,t)進行調整（Bundle Adjustment）。下面，介紹一下Bundle Adjustment。

Bundle Adjustment是一種非線性的方式，它是將相機位姿和空間點位置都當作優化變量，放在一塊兒優化。能夠用它對PnP或ICP給出的結果進行優化。在PnP中，這個Bundle Adjustment問題，是一個最小化重投影偏差的問題。本節給出此問題在兩個視圖下的基本形式，而後在第十講討論較大規模的BA問題。

考慮n個三維空間點P和它們的投影p，咱們但願計算相機的位姿R,t，它的李代數表示爲ζ。假設某空間點座標爲P_i=[X_i, Y_i, Z_i]^T。其投影的像素座標爲u_i=[u_i,v_i]。根據第五章的內容，像素位置與空間點位置的關係以下：

 除了用ζ爲李代數表示的相機姿態以外，別的都和前面的定義保持一致。寫成矩陣形式就是：

s_iu_i=Kexp(ζ^{^})Pi

但因爲噪聲以及相機位姿的緣由，該等式並不總成立。因此，須要用最小二乘法

這也叫重投影偏差。因爲須要考慮不少個點，因此最後每一個點的偏差一般都不會爲零。最小二乘優化問題已經在第六講介紹過了。使用李代數，能夠構建無約束的優化問題，很方便地經過G-N，L-M等優化算法進行求解。不過，在使用G-N和L-M以前，咱們須要知道每一個偏差項關於優化變量的導數，也就是線性化：

e(x+Δx)≈e(x)+JΔx

其中Δx即像素變量座標偏差。e(x+Δx)即偏移後的值。

當e爲像素座標偏差(2維)，x爲相機位姿(6維,旋轉+平移)，J將是一個2×6的矩陣。推導一下J的形式。

回憶李代數的內容，咱們介紹瞭如何使用擾動模型來求李代數的導數。首先，記變換到相機座標系下的空間點座標爲P’，而且把它前三維取出來：

P'=(exp(ζ^{^})P)_1:3=[X', Y', Z']^T

那麼，相機投影模型相對於P'則爲：

                 su=KP'

展開之：

 

利用第3行消去s，實際上就是P'的距離，得：

這與以前講的相機模型是一致的。當咱們求偏差時，能夠把這裏的u,v與實際的測量值比較，求差。在定義了中間變量後，咱們對ζ^左乘擾動量δζ，而後考慮e的變化關於擾動量的導數。利用鏈式法則，能夠列寫以下(這裏ζ指的是旋轉量和平移量6個參數)：

        --->     

 

除了擾動量，還有但願優化特徵點的空間位置。

      --->     

 

因而，推導出了觀測相機方程關於相機位姿與特徵點的兩個導數矩陣。它們特別重要，可以在優化過程當中提供重要的梯度方向，指導優化的迭代。

實踐：（1）先使用EPnP程序求解位姿，而後（2）對位姿和點座標進行BA非線性優化。

那麼對於兩組3D點怎麼求解參數呢，下面介紹3D-3D的方法：ICP。ICP是Iterative Closet Point的簡稱，即迭代最近點算法。它也有兩種求解方法：SVD和非線性兩種方法。

ICP:SVD法：已有兩個RGB-D圖像，經過特徵匹配獲取兩組3D點，最後用ICP計算它們的位姿變換。先定義第i對點的偏差項：e_i=p_i-(Rp_i'+t)。而後構建最小二乘問題，求使偏差平方和達到極小的R,t。先求R,再求t。爲求R，獲得-tr(R∑q_i'q_i^T)，要使R最小從而定義矩陣W=U∑V^T，其中∑爲奇異值組成的對角矩陣，對角線元素從大到小排列，而U和V爲正交矩陣。當W滿秩時，R爲：

R=UV^T.

獲得R後，便可求解t。

ICP:非線性方法。跟第四講的內容同樣。直接使用。

實踐：求解ICP，略。

 

第8講 視覺里程計2

本節先介紹光流法跟蹤特徵點的原理，而後介紹另外一種估計相機位姿的方法——直接法，如今直接法已經必定程度上能夠和特徵點法估計相機位姿勢均力敵。

光流即圖像上的某個灰度值在不一樣時刻的圖像上的流動。用圖像表示就是，不一樣圖像的灰度像素之間的關聯

 按照追蹤的像素的多少能夠分爲稀疏光流和稠密光流，這裏主要講稀疏光流中的Lucas-Kanade法，稱爲LK光流。

它假設一個像素在不一樣的圖像中是固定不變的，咱們知道這並不老是成立的，但咱們只是先這樣假設，否則什麼都無法作。

因此對於t時刻圖像(x,y)處的像素I(x, y, t)，通過dt時刻後在t+dt它來到了(x+dx, y+dy)處，這是一個運動過程，具體怎麼運動方向(實際運動是三維反映在圖像上是二維)怎麼樣不去管它，那麼由假設可知I(x+dx, y+dy, t+dt)=I(x, y, t)。咱們知道一個像素沿着x,y軸各移動必定的距離，距離上的速度分別爲u,v。那麼u,v各是多少呢？怎麼求呢？求它們有什麼用呢？看下圖，假設前一秒和後一秒相機運動以下

左上方(1,1)灰色方格超右下方瞬間移動到了(4.2)處灰色方格，如何用方程來描述這個過程呢？

能夠經過相鄰像素的顏色梯度和自身像素的顏色變化來描述。也就是經過相鄰像素梯度和運行速度獲得自身該位置的顏色值變化，列出一個方程，以此來描述圖像的平移。

當這個運動足夠小時能夠對左邊的式子進行泰勒展開，由於I(x,y,t)部分是不變的因此獲得：

--->

 兩邊除以dt，得：

 記梯度分別爲I_x,I_y，速度分別爲u,v，而右側的灰度對時間的變化量I_t。

 能夠獲得：

根據最小二乘法能夠獲得： 經過特徵塊求解u,v。

獲得了像素在圖像間的運行速度u,v，就能夠估計特徵點在不一樣圖像中的位置了。當t取離散的時刻而不是連續時間時，咱們能夠估計某塊像素在若干個圖像中出現的位置。因爲像素梯度僅在局部有效，因此若是一次迭代不夠好的話，咱們會多迭代幾回這個方程。

下面經過實踐來體會一下LK光流在跟蹤角點上的應用。

例：使用LK光流法對第一張圖像提取FAST角點，而後用LK光流跟蹤它們，畫在圖中。

slambook/ch8/useLK/useLK.cpp

 

能夠看到特徵點在逐漸減小，須要不斷添加新的特徵點進去。光流法的優勢是能夠避免描述子的計算，這能夠避免誤匹配，可是若是運動太快時就容易追蹤錯誤仍是描述子好些。

而後就能夠根據光流法跟蹤的特徵點經過PnP、ICP或對極幾何來估計相機運動，這以前已講過，再也不贅述。

而直接法是直接連特徵點都不提取了，直接用相似光流法的梯度迭代完成。計算獲得位姿估計。它的思想是光流法+迭代近似。從而在光流法的基礎上繼續減小特徵提取的時間。

要求的是位姿。根據光流法假設的像素差最小原則，得：

   e=I₁(p₁)-I₂(p₂)

利用最小二乘法

當有多個點時，，ei爲第i像素的像素差

咱們要求的是ζ的優化值，假設ei有誤差，那麼J(ζ)有如何的變更呢？咱們須要求二者之間的關係

咱們假設ζ出現了一點誤差，考慮ei的變化，道理是同樣的，求二者的關係，獲得：

最後通過一系列變換獲得一個e與ζ的關係式：

其中，而[X, Y, Z]爲三維點座標。

對於RGBD相機來講像素對應的XYZ比較好肯定(咱們不知道第二個像素是什麼咱們不獲取第二個像素對應的XYZ，假設第二個像素是由第一個像素和第一個像素對應的XYZ通過姿態影射獲得的)，但對於沒有深度的純單目相機來講要麻煩，由於深度很差肯定，那隻能假設其有一個深度，通過姿態後投影到第二個像素上，詳細深度估計放到第13講。

下面進行直接法的實踐。[這裏Eigen要使用Eigen3以上的包，由於要使用Eigen::Map函數。]

（1）稀疏直接法。基於特徵點的深度恢復上一講介紹過了，基於塊匹配的深度恢復將在後面章節中介紹，因此本節咱們來考慮RGB-D上的稀疏直接法VO。求解直接法最後等價於求解一個優化問題，所以可使用g2o或Ceres這些優化庫來求解。稀疏怎麼個稀疏嗎？求特徵點。稀疏直接法跟光流法的區別在於光流法是經過平面圖像的運動來估計第二個圖像上的關鍵點像素，而稀疏直接法是經過位姿估計來實現。

（2）半稠密直接法。把特徵點改成凡是像素梯度較大的點就是半稠密直接法。

 

第9講 實踐章：設計前端

前面兩節咱們學習了視覺里程計（VO）的原理，如今設計一個VO。VO也就是前端，與後端優化相對應。這節咱們利用前面所學的知識實際設計一個前端框架。你將會發現VO前端會遇到不少突發情況，畢竟咱們前面所學的只是完美假設狀況下的理論，但實際狀況是總會有意外的狀況如相機運動過快、圖像模糊、誤匹配等。能夠看到這節主要是前面的理論的實踐，你將實際動手製做一個視覺里程計程序。你會管理局部的機器人軌跡與路標點(參考論文)，並體驗一下一個軟件框架是如何組成的。

咱們知道光知道原理跟建造起偉大的建築做品之間是有很大的區別的，能夠說這裏面是還有長長的路要走的。就像你知道三維建模的原理，知道各個部件的建造原理知道怎麼搭建，這跟真正建造起美輪美奐的建築物之間是有區別的，這須要發揮你的創造力和毅力來建造起復雜的建築模型，光知道各個部分原理是不夠的，你既須要統籌全局又須要優化部分的能力，這是須要廣與小都具有才行。這跟SLAM類似，SLAM是個很偉大的建築物，而各個柱子和屋頂之間如何搭建須要考慮之間的關聯，而各個柱子、各個瓦片又須要再細細優化打磨，因此說，從局部到總體都須要考慮到，總體須要局部以及局部之間的關係。

這是一個從簡到繁的過程，咱們會從簡單的數據結構開始，慢慢從簡單到複雜創建起整個SLAM。在project文件夾下，從0.1到0.4你能夠感覺到版本的變化，從簡單到複雜的整個過程。

如今咱們要寫一個SLAM庫文件，目標是幫讀者將本書用到的各類算法融會貫通，書寫本身的SLAM程序。

程序框架：新建文件夾，把源代碼、頭文件、文檔、測試數據、配置文件、日誌等等分類存放

基本的數據結構：

數據是基礎，數據和算法構成程序。那麼，有哪些基本的數據結構呢？

1. 圖像幀。一幀是相機採集到的圖像單位。它主要包含一個圖像。此外還有特徵點、位姿、內參等信息。並且圖像通常要選擇關鍵幀，不會都存儲那樣成本過高。

2. 路標。路標點即圖像中的特徵點。

3. 配置文件。將各類配置參數單獨存儲到一個文件中，方便讀取。

4. 座標變換類。你須要常常進行座標系間的座標變換。

因此咱們創建五個類：Frame爲幀，Camera爲相機模型，MapPoint爲特徵點/路標點，Map管理特徵點，Config提供配置參數。

其中的包含關係。一幀圖像有一個Camera模型和多個特徵點。而一個地圖有多個特徵點組成。

 1. Camera類

#ifndef CAMERA_H
#define CAMERA_H
#include "common_include.h"

namespace myslam
{
    //Pinhole RGB-D camera model
    class Camera
    {
    public:
        typedef std::shared_ptr<Camera> Ptr; //公共camera指針成員變量
        float fx_, fy_, cx_, cy_, depth_scale_; //Camera intrinsics

        Camera();
        Camera(float fx, float fy, float cx, float cy, float depth_scale=0):
        fx_(fx), fy_(fy), cx_(cx), cy_(cy), depth_scale_(depth_scale)
        {}

        //coordinate transform:world, camera, pixel
        Vector3d world2camera(const Vector3d& p_w, const SE3& T_c_w);
        Vector3d camera2world(const Vector3d& p_c, const SE3& T_c_w);
        Vector2d camera2pixel(const Vector3d& p_c);
        Vector3d pixel2camera(const Vector3d& p_p, double depth=1);
        Vector3d pixel2world(const Vector2d& p_p, const SE3& T_c_w, double depth=1);
        Vector2d world2pixel(const Vector3d& p_w, const SE3& T_c_w);
    };
}


#endif //CAMERA_H

其中#ifndef是爲了不兩次重複引用致使出錯，在頭文件中必需要加上(這點很重要切記，否則A->B,A->C,那麼B,C->D時，D類中就引用了兩次A)，而這樣還不夠，有時候咱們總喜歡用一樣的類名，爲了儘可能避免這種狀況發生，咱們也必須加上本身的namespace命名空間，通常是以本身的個性化來命名區分。並且爲了不發生內存泄露，咱們應該爲咱們的類加上智能指針shared_ptr，之後在傳遞參數時只須要使用Camera::Ptr類型便可。

2. Frame類

#ifndef FRAME_H
#define FRAME_H

#include "common_include.h"
#include "camera.h"

namespace myslam 
{
    
// forward declare 
class MapPoint;
class Frame
{
public:
    typedef std::shared_ptr<Frame> Ptr;
    unsigned long                  id_;         // id of this frame
    double                         time_stamp_; // when it is recorded
    SE3                            T_c_w_;      // transform from world to camera
    Camera::Ptr                    camera_;     // Pinhole RGBD Camera model 
    Mat                            color_, depth_; // color and depth image 
    
public: // data members 
    Frame();
    Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
    ~Frame();
    
    // factory function
    static Frame::Ptr createFrame(); 
    
    // find the depth in depth map
    double findDepth( const cv::KeyPoint& kp );
    
    // Get Camera Center
    Vector3d getCamCenter() const;
    
    // check if a point is in this frame 
    bool isInFrame( const Vector3d& pt_world );
};

}

#endif // FRAME_H

3. MapPoint類

#ifndef MAPPOINT_H
#define MAPPOINT_H

namespace myslam
{
    
class Frame;
class MapPoint
{
public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long      id_; // ID
    Vector3d    pos_;       // Position in world
    Vector3d    norm_;      // Normal of viewing direction 
    Mat         descriptor_; // Descriptor for matching 
    int         observed_times_;    // being observed by feature matching algo.
    int         correct_times_;     // being an inliner in pose estimation
    
    MapPoint();
    MapPoint( long id, Vector3d position, Vector3d norm );
    
    // factory function
    static MapPoint::Ptr createMapPoint();
};
}

#endif

4. Map類

#ifndef MAP_H
#define MAP_H

#include "common_include.h"
#include "frame.h"
#include "mappoint.h"

namespace myslam
{
class Map
{
public:
    typedef shared_ptr<Map> Ptr;
    unordered_map<unsigned long, MapPoint::Ptr >  map_points_;        // all landmarks
    unordered_map<unsigned long, Frame::Ptr >     keyframes_;         // all key-frames

    Map() {}
    
    void insertKeyFrame( Frame::Ptr frame );
    void insertMapPoint( MapPoint::Ptr map_point );
};
}

#endif // MAP_H

5. Config類

#ifndef CONFIG_H
#define CONFIG_H

#include "common_include.h" 

namespace myslam 
{
    class Config
    {
    private:
        static std::shared_ptr<Config> config_;
        cv::FileStorage file_;

        Config(){} //private constructor makes a singleton
    public:
        ~Config(); //close the file when deconstructing

        //set a new config file
        static void setParameterFile(const std::string& filename);

        //access the parameter values
        template<typename T>
        static T get(const std::string& key)
        {
            return T(Config::config_->file_[key]);
        }
    };
}

#endif

數據結構設計完以後就能夠設計程序了，先進行最基本的VO：特徵提取和匹配

第一種是兩兩幀的視覺里程計，不進行優化，不計算特徵點位置，只進行兩兩幀的運動估計。

slambook/project/0.2/include/myslam/visual_odometry.h

#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H

#include "common_include.h"
#include "map.h"

#include <opencv2/features2d/features2d.hpp>

namespace myslam 
{
    class VisualOdometry
    {
    public:
        typedef shared_ptr<VisualOdometry> Ptr;
        enum VOState{INITIALIZING=-1, OK=0, LOST};

        VOState state_; //current VO status
        Map::Ptr map_; //map with all frames and map points
        Frame::Ptr ref_; //reference frame
        Frame::Ptr curr_; //current frame

        cv::Ptr<cv::ORB> orb_; //orb detector and computer
        vector<cv::Point3f> pts_3d_ref_; //3d points in reference frame
        vector<cv::KeyPoint> keypoints_curr_; //keypoints in current frame
        Mat descriptors_curr_; //descriptor in current frame
        Mat descriptors_ref_; //descriptor in reference frame
        vector<cv::DMatch> feature_matches_;

        SE3 T_c_r_estimated_; //the estimated pose of current frame
        int num_inliers_; //number of inlier features in icp
        int num_lost_; //number of lost times

        //parameters
        int num_of_features_; //number of features
        double scale_factor_; //scale in image pyramid
        int level_pyramid_; //number of pyramid levels
        float match_ratio_; //ratio for selecting good matches
        int max_num_lost_; //max number of continuos lost times
        int min_inliers_; //minimum inliers

        double key_frame_min_rot; //minimal rotation of tow key-frames
        double key_frame_min_trans; //minimal translation of two key-frames
    public: //functions
        VisualOdometry();
        ~VisualOdometry();

        bool addFrame(Frame::Ptr frame); //add a new frame

    protected:
        //inner operation
        void extractKeyPoints();
        void computeDescriptors();
        void featureMatching();
        void poseEstimationPnP();
        void setRef3DPoints();

        void addKeyFrame();
        bool checkEstimatedPose();
        bool checkKeyFrame();
    };
}

#endif

關於這個VisualOdometry類，有幾點須要解釋：

1. addFrame函數是外部調用的接口。使用VO時，將圖像數據裝入Frame類後，調用addFrame估計其位姿。該函數根據VO所處的狀態實現不一樣的操做：

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
switch(state_)
{
case INITIALIZING: //初始化，該幀爲第一幅
{
state_=OK; //修改狀態爲OK
curr_=ref_=frame; //將該幀賦給當前幀和參考幀
map_->insertKeyFrame(frame); //將該幀插入地圖
//從第一幀中提取特徵點
extractKeyPoints(); //提取特徵點
computeDescriptors(); //計算描述子
//計算參考幀中的特徵點的三維座標點
setRef3DPoints();
break;
}
case OK:
{
curr_=frame;
extractKeyPoints();
computeDescriptors();
featureMatching();
poseEstimationPnP();
if(checkEstimatedPose()==true) //a good estimation
{
curr_->T_c_w_=T_c_r_estimated_*ref_->T_c_w_; //T_c_w=T_c_r*T_r_w
ref_=curr_;
setRef3DPoints();
num_lost_=0;
if(checkKeyFrame()==true) //is a key-frame
{
addKeyFrame();
}
}
else //bad estimation due to various reasons
{
num_lost_++;
if(num_lost_>max_num_lost_)
{
state_=LOST;
}
return false;
}
break;
}
case LOST:
{
cout<<"vo has lost."<<endl;
break;
}
}
return true;
}

 值得一提的是，因爲各類緣由，咱們設計的上述VO算法，每一步都有可能失敗。例如圖片中不易提特徵、特徵點缺乏深度值、誤匹配、運動估計出錯等等。所以，要設計一個魯棒的VO，必須（最好是顯式地）考慮到上述全部可能出錯的地方——那天然會使程序變得複雜。咱們在checkEstimatedPose中，根據內點（inlier）的數量以及運動的大小作一個簡單的檢測：認爲內點不可太少，而運動不可能過大。固然，讀者也能夠思考其餘檢測問題的手段，嘗試一下效果。

VisualOdometry的其他部分略，下面進行一下測試Test。

slambook/project/0.2/test/run_vo.cpp

程序略，結果以下圖所示。

注意：這裏使用的OpenCV3的viz模塊顯示估計位姿，請確保你安裝的是OpenCV3，而且viz模塊也編譯安裝了。準備tum數據集中的其中一個。簡單起見，我推薦fr1_xyz那一個。請使用associate.py生成一個配對文件associate.txt。關於tum數據集格式咱們已經在8.3節中介紹過了。在config/defualt.yaml中填寫你的數據集所在路徑，參照個人寫法便可。而後用bin/run_vo config/default.yaml執行程序，就能夠看到實時的演示了。

改進：優化PnP的結果。沿着以前的內容，嘗試一些VO的改進。嘗試RANSAC PnP加上迭代優化的方式估計相機位姿。非線性優化以前已經介紹過了，此處略。

改進：局部地圖。將VO匹配到的特徵點放到地圖中，並將當前幀跟地圖點進行匹配，計算位姿。地圖又分爲全局地圖和局部地圖，全局地圖太奢侈了，主要用於迴環檢測和地圖表達。局部地圖是指隨着相機運動，往地圖裏添加新的特徵點，並將視野以外的特徵點刪掉，而且統計每一個地圖點被觀測到的次數等等。

 

第10講 後端1

本節咱們開始轉入SLAM系統的另外一個重要模塊：後端優化。前面的視覺里程計能夠給出一個短期內的軌跡和地圖，但因爲不可避免的偏差累積，若是時間長了這個地圖是不許確的。因此咱們但願構建一個尺度、規模更大的優化問題，以考慮長時間內的最優軌跡和地圖。實際當中考慮到精度與性能的平衡，有許多不一樣的作法。

1. 什麼叫後端？

 以前提到，視覺里程計只有短暫的記憶，而咱們但願整個運動軌跡在較長時間內都能保持最優的狀態。咱們可能會用最新的知識，更新較久遠的狀態——站在「久遠的狀態」的角度上看，彷彿是將來的信息告訴它「你應該在哪裏」。因此，在後端優化中，咱們一般考慮一段更長時間內（或全部時間內）的狀態估計問題，並且不只使用過去的信息更新本身的狀態，也會用將來的信息來更新本身，這種處理方式不妨稱爲「批量的」（Batch）。不然，若是當前的狀態只由過去的時刻決定，甚至只由前一個時刻決定，那不妨稱爲「漸進的」（Incremental）。

因此這是一個假設檢驗的過程。先驗和後驗。先由之前的位姿和觀測方程預測下一步，而後利用下一步的內容預測這一步的最大可能。是機率統計中的知識。

2. 狀態估計的機率解釋

咱們已經知道SLAM過程能夠由運動方程和觀測方程來描述。那麼，假設在t=0到t=N的時間內，有位姿x₀到x_N，而且有路標y₁,…,y_M。按照以前的寫法，運動和觀測方程爲：

 注意如下幾點：

（1）在觀測方程中，只有當x_k看到了y_j時，纔會產生觀測數據。

（2）咱們可能沒有測量運動的裝置，因此也可能沒有運動方程。在這個狀況下，有若干種處理方式：認爲確實沒有運動方程，或假設相機不動，或假設相機勻速運動。這幾種方式都是可行的。在沒有運動方程的狀況下，整個優化問題就只由許多個觀測方程組成。這就很是相似於StM(Structure from Motion)問題，由一組圖像恢復運動和結構。與StM中不一樣的是，SLAM中的圖像有時間上的前後順序，而StM中容許使用徹底無關的圖像。

咱們知道每一個方程都受噪聲影響，因此要把這裏的位姿x和路標y當作服從某種機率分佈的隨機變量，而不是單獨的一個數。因此當存在一些運動數據u和觀測數據z時，如何去估計狀態量的高斯分佈？

當只有位姿數據時，偏差累積會愈來愈大，不肯定性增高，而若是加上路標則會將不肯定性減少。以下圖所示。

下面以定量的方式來看待它。

咱們但願用過去0到k中的數據來估計如今的狀態分佈：

P(x_k|x₀,u_1:k,z_1:k)

其中xk爲第k時刻的未知量。上式表示根據初始位置、1到k的運動參數、1到k的圖像來估計第k時刻的未知量。

由x_k和z_k的相互依賴關係，能夠獲得以下近似關係：

P(x_k|x₀,u_1:k,z_1:k) ∝P(z_k|x_k)P(x_k|x₀,u_1:k,z_1:k-1)

上式表示由z_k推x_k，跟由x_k推z_k結果是同樣的機率上來講。

上式第一項爲似然，第二項爲先驗。似然由觀測方程給定，而先驗部分咱們明白當前狀態x_k是基於過去全部的狀態估計得來的。至少它會受x_k-1影響，因而按照x_k-1時刻爲條件機率展開：

P(x_k|x₀,u_1:k,z_1:k-1)= ∫P(x_k|x_k-1,x₀,u_1:k, z_1:k-1)P(x_k-1|x₀,u_1:k,z_1:k-1)dx_k-1.

若是考慮更久以前的狀態，也能夠繼續對此式進行展開，但如今咱們只關心k時刻和k-1時刻的狀況。至此，咱們給出了貝葉斯估計，雖然上式尚未具體的機率分佈形式，因此還無法實際地操做它。對這一步的後續處理，方法上產生了一些分歧。大致上講，存在若干種選擇：其一是假設馬爾可夫性，簡單的一階馬氏性認爲，k時刻狀態只與k-1時刻狀態有關，而與再以前的無關。若是作出這樣的假設，咱們就會獲得以擴展卡爾曼濾波（EKF）爲表明的濾波器方法。時刻注意這種方法與里程計的區別，在於經過幾率統計的方法來消除偏差累積致使的軌跡偏移。在濾波方法中，咱們會從某時刻的狀態估計，推導到下一個時刻。另一種方法是依然考慮k時刻狀態與以前全部狀態的關係，此時將獲得非線性優化爲主體的優化框架。這也能消除偏差累積的影響。非線性優化的基本知識已在前文介紹過。目前視覺SLAM的主流爲非線性優化方法。不過，爲了讓本書更全面，咱們要先介紹一下卡爾曼濾波器和EKF的原理。

卡爾曼濾波器的推導方法有若干種，好比從機率角度出發的最大後驗機率估計的形式（貝葉斯），從最小均方差爲目的推導的遞推等式（連接）。總之，卡爾曼濾波就是推導出先驗和後驗之間的關係式，也就是增益K。經過預測（先驗）和測量反饋（後驗）來提升準確度。

可是EKF存在一些缺點，因此如今在SLAM中通常採用BA與圖優化，這個以前提到過，這裏詳細闡述它的原理。

 

第11講 後端2

雖然BA有各類優勢，可是速度太慢，尤爲是隨着特徵點愈來愈多它的效率會愈來愈慢。因此引出了位姿圖。即將特徵點視爲輸入值，只考慮位姿的優化。

 

第12講 迴環檢測

迴環檢測的優勢，能夠充分利用每個環來調整地圖。

如上圖所示第一個圖表示真實的軌跡，第二個圖紅色表示位姿的偏移，而若是有迴環檢測的話則能夠將後面偏離的軌跡拉回到重合處。這就是迴環檢測的好處。

迴環檢測若是真的檢測到真實的迴環則有很大的優化做用，但錯誤的檢測則會形成很大的損失，還不如不檢測，因此這裏對準確率要求較高，而對召回率要求小一些（召回率即有些真實的迴環沒有檢測到的機率），也就是說對於模棱兩可不肯定的迴環，寧肯不迴環也不要冒險認定爲迴環。

那麼迴環檢測用什麼模型呢？這裏用詞袋模型，是特徵點法的聚類模型。

聚類問題在無監督機器學習（Unsupervised ML）中特別常見，用於讓機器自行尋找數據中的規律，無人工干預。詞袋的字典生成亦屬於其中之一。先對大量的圖像提取了特徵點，好比說有N個。如今，咱們想找一個有k個單詞的字典，每一個單詞是由特徵點組成的，那麼特徵點該如何組合成有效的單詞呢？咱們人通常會自動聚合爲「書包」、「杯子」、「桌子」這樣的單詞。每一個單詞能夠看做局部相鄰特徵點的集合，應該怎麼作呢？這能夠用經典的K-means（K均值）算法解決。

 

第13講 建圖

以前只是提取了特徵點，可是稠密的地圖須要更多的點。特徵點主要用於估計機器人位姿，而稠密的點用於避障和交互等。避障跟導航相似，可是更注重局部的、動態的障礙物的處理。一樣，僅有特徵點，咱們沒法判斷某個特徵點是否爲障礙物，因此須要稠密地圖。