高翔《視覺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講 建圖

之前只是提取了特征點，但是稠密的地圖需要更多的點。特征點主要用於估計機器人位姿，而稠密的點用於避障和交互等。避障跟導航類似，但是更注重局部的、動態的障礙物的處理。同樣，僅有特征點，我們無法判斷某個特征點是否為障礙物，所以需要稠密地圖。