?

目錄

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

第2講 初始SLAM：引子-小蘿卜的例子；經(jīng)典視覺SLAM框架；SLAM問題的數(shù)學(xué)表述；實(shí)踐-編程基礎(chǔ)；

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

第4講 李群與李代數(shù) 李群李代數(shù)基礎(chǔ)；指數(shù)與對數(shù)映射；李代數(shù)求導(dǎo)與擾動模型；實(shí)踐-Sophus；相似變換群與李代數(shù)；小結(jié)；

第5講 相機(jī)與圖像 相機(jī)模型；圖像；實(shí)踐-圖像的存取與訪問；實(shí)踐-拼接點(diǎn)云；

第6講 非線性優(yōu)化 狀態(tài)估計(jì)問題；非線性最小二乘；實(shí)踐-Ceres；實(shí)踐-g2o；小結(jié)；

第7講 視覺里程計(jì)1 特征點(diǎn)法；特征提取和匹配；2D-2D：對極幾何；實(shí)踐-對極約束求解相機(jī)運(yùn)動；三角測量；實(shí)踐-三角測量；3D-2D：Pnp；實(shí)踐-求解PnP；3D-3D：ICP；實(shí)踐-求解ICP；小結(jié)；

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

第9講 實(shí)踐章：設(shè)計(jì)前端 搭建VO前端；基本的VO-特征提取和匹配；改進(jìn)-優(yōu)化PnP的結(jié)果；改進(jìn)-局部地圖；小結(jié)

第10講 后端1 概述；BA與圖優(yōu)化；實(shí)踐-g2o；實(shí)踐-Ceres；小結(jié)

第11講 后端2 位姿圖（Pose Graph）；實(shí)踐-位姿圖優(yōu)化；因子圖優(yōu)化初步；實(shí)踐-gtsam；

第12講 回環(huán)檢測 回環(huán)檢測概述；詞袋模型；字典；相似度計(jì)算；實(shí)驗(yàn)分析與評述；

第13講 建圖 概述；單目稠密重建；實(shí)踐-單目稠密重建；實(shí)驗(yàn)分析與討論；RGBD稠密建圖；TSDF地圖和Fusion系列；小結(jié)；

第14講 SLAM：現(xiàn)在與未來 當(dāng)前的開源方案；未來的SLAM話題；

附錄A 高斯分布的性質(zhì)

附錄B ROS入門

?

第1講?前言

1.1 本書講什么

講的是SLAM（Simultaneous Localization And Mapping，同步定位與成圖），即利用傳感器來進(jìn)行機(jī)器人的自身定位以及對周圍環(huán)境的成圖。根據(jù)傳感器來劃分主要分為雷達(dá)SLAM和視覺SLAM。這里當(dāng)然主要講的是視覺SLAM。

從定義上可以看出SLAM主要解決的是“自身定位”和“周圍環(huán)境的成圖”。

這里主要把SLAM系統(tǒng)分成幾個(gè)模塊：視覺里程計(jì)、后端優(yōu)化、建圖以及回環(huán)檢測。

這是一個(gè)很復(fù)雜的過程，共分十四講來講述，每一講都有一個(gè)主題，然后介紹該主題的算法和實(shí)踐，即從理論到實(shí)踐。

?ok，這講就到這里了。

?

第2講 初始SLAM

這講主要是講SLAM的基本框架，以及開發(fā)前期準(zhǔn)備工作。

如上圖所述，SLAM基本框架就是這樣。一個(gè)機(jī)器人，靠一個(gè)攝像頭來感知周圍的世界并評估自身的位置，它需要利用Visual Odometry視覺里程計(jì)來通過相鄰兩張相片來計(jì)算姿態(tài)參數(shù)估算距離角度這些用來計(jì)算自身位置xyz以及恢復(fù)圖像上各點(diǎn)的位置pxpypz，由于有上一步有累積誤差需要Optimization后端優(yōu)化，對于周圍環(huán)境的感知需要Mapping地圖構(gòu)建并配準(zhǔn)，最后需要Loop Closure回環(huán)檢測計(jì)算閉合誤差并修正。

前期準(zhǔn)備工作有：

準(zhǔn)備一臺電腦和一個(gè)Turtlebot機(jī)器人（沒有也不要緊，只要有一臺Kinect相機(jī)就可以了，你可以用手拿著它模擬機(jī)器人的走動，它會還原你的位置信息）。這臺電腦和Turtlebot機(jī)器人都是安裝的Linux操作系統(tǒng)，沒錯，機(jī)器人身上有一臺電腦作為控制，電腦就是一個(gè)高級點(diǎn)的單片機(jī)，這是它的大腦控制著它發(fā)射信息行走以及計(jì)算匯總，而另一臺電腦作為我們的服務(wù)器它接收機(jī)器人發(fā)送的信息如圖片等以及給機(jī)器人發(fā)送指令。希望安裝的是Ubuntu這款Linux操作系統(tǒng)，因?yàn)槲覀円院髮⒃谠摬僮飨到y(tǒng)下下載安裝軟件。ROS軟件就不用說了，這是機(jī)器人操作系統(tǒng)，是必備的，可以利用它與機(jī)器人完成交互傳遞信息發(fā)送指令等。另外我們還需要配置開發(fā)環(huán)境，因?yàn)槲覀冃枰_發(fā)我們自己的程序，調(diào)節(jié)參數(shù)。這是ROS所不具備的。

配置開發(fā)環(huán)境: Kdevelop。

編寫實(shí)例程序: HelloSLAM。

?

第3講 三維空間剛體運(yùn)動

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

a'=Ra+t

其中R為旋轉(zhuǎn)矩陣，t為平移矩陣

將R和t融合到一個(gè)矩陣中，將上式變?yōu)榫€性方程

那么編程如何實(shí)現(xiàn)上式呢？用Eigen開源庫。

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

旋轉(zhuǎn)向量與旋轉(zhuǎn)矩陣之間的變換：，其中n為旋轉(zhuǎn)向量，R為旋轉(zhuǎn)矩陣

旋轉(zhuǎn)向量只需要三個(gè)變量，比旋轉(zhuǎn)矩陣三維矩陣的9個(gè)變量要簡潔，節(jié)省運(yùn)算成本。

由旋轉(zhuǎn)向量到四元數(shù)：由于旋轉(zhuǎn)矩陣的冗余，加上旋轉(zhuǎn)向量和角度的奇異性，引入四元數(shù)，即用復(fù)數(shù)來表示旋轉(zhuǎn)從而避免了奇異性。

用四元數(shù)來表示旋轉(zhuǎn)：設(shè)空間點(diǎn)p=[x,y,z]，以及一個(gè)由旋轉(zhuǎn)軸和角度指定的旋轉(zhuǎn)，那么旋轉(zhuǎn)后的點(diǎn)p'用四元數(shù)怎么表示呢？我們知道使用矩陣描述的話p'=Rp。而用四元數(shù)表達(dá)：

1. 把三維空間點(diǎn)用一個(gè)虛四元數(shù)描述：

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

這相當(dāng)于我們把四元數(shù)的三個(gè)虛部與空間中的三個(gè)軸相對應(yīng)。用四元數(shù)q表示這個(gè)旋轉(zhuǎn)：

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

那么旋轉(zhuǎn)后的點(diǎn)p'即可表示為這樣的乘積：

p'=qpq^(-1)

2. 四元數(shù)到旋轉(zhuǎn)矩陣的轉(zhuǎn)換

那么用Eigen演示如何進(jìn)行各種旋轉(zhuǎn)變換。在Eigen中使用四元數(shù)、歐拉角和旋轉(zhuǎn)矩陣，演示它們?nèi)咧g的變換關(guān)系。

Eigen中各個(gè)數(shù)據(jù)類型總結(jié)如下：

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

本講結(jié)束。

?

第4講 李群與李代數(shù)

三維旋轉(zhuǎn)矩陣構(gòu)成了特殊正交群SO(3)，而變換矩陣構(gòu)成了特殊歐氏群SE(3)

?

?但無論SO(3)，還是SE(3)，它們都不符合加法封閉性，即加之后不再符合旋轉(zhuǎn)矩陣的定義，但是乘法卻滿足，將這樣的矩陣稱為群。即只有一種運(yùn)算的集合叫做群。

?群記作G=(A, .)，其中A為集合，.表示運(yùn)算。群要求運(yùn)算滿足以下幾個(gè)條件：

（1）封閉性。

（2）結(jié)合律。

（3）幺元。一種集合里特殊的數(shù)集。

（4）逆。

可以證明，旋轉(zhuǎn)矩陣集合和矩陣乘法構(gòu)成群，而變換矩陣和矩陣乘法也構(gòu)成群。

介紹了群的概念之后，那么，什么叫李群呢？

李群就是連續(xù)（光滑）的群。一個(gè)剛體的運(yùn)動是連續(xù)的，所以它是李群。

每個(gè)李群都有對應(yīng)的李代數(shù)。那么什么叫李代數(shù)呢？

李代數(shù)就是李群對應(yīng)的代數(shù)關(guān)系式。

李群和李代數(shù)之間的代數(shù)關(guān)系如下：

可見兩者之間是指數(shù)與對數(shù)關(guān)系。

?那么exp(φ^)是如何計(jì)算的呢？它是一個(gè)矩陣的指數(shù)，在李群和李代數(shù)中，它稱為指數(shù)映射。任意矩陣的指數(shù)映射可以寫成一個(gè)泰勒展開式，但是只有在收斂的情況下才會有結(jié)果，它的結(jié)果仍然是一個(gè)矩陣。

?同樣對任意一元素φ，我們亦可按此方式定義它的指數(shù)映射：

?由于φ是三維向量，我們可以定義它的模長θ和方向向量a滿足使φ=θa。那么，對于a^，可以推導(dǎo)出以下兩個(gè)公式：

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

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

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

?上面兩個(gè)公式說明了a^的二次方和a^的三次方的對應(yīng)變換，從而可得：

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^.

回憶前一講內(nèi)容，它和羅德里格斯公式如出一轍。這表明，so(3)實(shí)際上就是由旋轉(zhuǎn)向量組成的空間，而指數(shù)映射即羅德里格斯公式。通過它們我們把so(3)中任意一個(gè)向量對應(yīng)到了一個(gè)位于SO(3)中的旋轉(zhuǎn)矩陣。反之，如果定義對數(shù)映射，我們也能把SO(3)中的元素對應(yīng)到so(3)中：

但通常我們會通過跡的性質(zhì)分別求解轉(zhuǎn)角和轉(zhuǎn)軸，那種方式會更加省事一些。

?OK，講了李群和李代數(shù)的對應(yīng)轉(zhuǎn)換關(guān)系之后，有什么用呢？

主要是通過李代數(shù)來對李群進(jìn)行優(yōu)化。比如說，對李群中的兩個(gè)數(shù)進(jìn)行運(yùn)算，對應(yīng)的他們的李代數(shù)會有什么變化？

首先是，兩個(gè)李群中的數(shù)進(jìn)行乘積時(shí)，對應(yīng)的李代數(shù)是怎么樣的變化，是不是指數(shù)變化呢？但是注意，李群里的數(shù)是矩陣，不是常數(shù)，所以不滿足ln(exp(A+B))=A+B，因?yàn)锳,B是矩陣，不是常數(shù)，那么是怎么的對應(yīng)關(guān)系呢？

是

?

?

第5講 相機(jī)與圖像

之前介紹了機(jī)器人的運(yùn)動方程，那么現(xiàn)在來講相機(jī)的原理。相機(jī)最早是根據(jù)小孔成像原理產(chǎn)生的最早相機(jī)。后面又有了加凸透鏡的相機(jī)。

相機(jī)模型：

這個(gè)公式表示了世界坐標(biāo)Pw到像素坐標(biāo)Puv的轉(zhuǎn)換。表示了從世界坐標(biāo)(Pw)到相機(jī)坐標(biāo)(通過外參R,t)再到像素坐標(biāo)(通過K內(nèi)方位元素)的轉(zhuǎn)換。

對于凸透鏡的鏡頭畸變則采用：

進(jìn)行徑向畸變糾正和切向畸變糾正。其中k1,k2,k3為徑向畸變參數(shù)，而p1,p2為切向畸變參數(shù)。

而對于相機(jī)標(biāo)定，即求解內(nèi)方位參數(shù)K，可以采用OpenCV, Matlab, ROS三種方法求解。參照：鏈接。

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

那么接下來呢，當(dāng)然是將上一步求的相機(jī)內(nèi)參數(shù)應(yīng)用到實(shí)際的圖像點(diǎn)云數(shù)據(jù)處理上。

有了相機(jī)內(nèi)參數(shù)和相片，就可以恢復(fù)像點(diǎn)的像點(diǎn)X,Y,Z坐標(biāo)嗎？不能，由可知，要求像點(diǎn)X,Y,Z除了內(nèi)參數(shù)矩陣外，還需要像點(diǎn)的(x,y,w)，而從像片上只能得到x,y，那么w是距離，怎么獲得呢？通過RGD-D相機(jī)可以獲得深度數(shù)據(jù)，即w。（通過Kinect獲取彩色和深度圖像，保存 顯示）

假設(shè)你已經(jīng)通過Kinect獲得彩色和深度圖像了，那么就可以通過上式恢復(fù)像素的相機(jī)坐標(biāo)點(diǎn)了(即通過彩色圖像(x,y)和深度圖像(w)就可以得到點(diǎn)云了)。然后就可以(通過IMU得到的位姿參數(shù))當(dāng)前點(diǎn)的R,t這些旋轉(zhuǎn)矩陣和平移矩陣(外參)恢復(fù)到它的世界坐標(biāo)Pw(Xw, Yw, Zw)。

以上兩個(gè)文件夾分別為彩色圖像和深度圖像，一一對應(yīng)。

接下來來寫一個(gè)通過兩張彩色圖像和對應(yīng)的兩張深度圖像得到點(diǎn)云數(shù)據(jù)，并且將兩個(gè)點(diǎn)云數(shù)據(jù)融合生成地圖的程序：

程序略。

拼接完成的數(shù)據(jù)保存到map.pcd文件中。可以通過PCL的點(diǎn)云顯示程序來顯示保存的map.pcd。10萬多個(gè)點(diǎn)加載了進(jìn)來，但是顏色在windows程序下怎么沒有顯示出來。在Linux下試一下看看。

pcd_viewer.exe本身有問題，pcl_viewer.exe在Windows下找不到。pcl_visualization可以顯示彩色圖像，但是只是一個(gè)鏈接庫文件。

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

?

?

第6講 非線性優(yōu)化

通過傳感器的運(yùn)動參數(shù)來估計(jì)運(yùn)動方程(位姿x)，通過相機(jī)的照片來估計(jì)物體的位置(地圖y)，都是有噪聲的。因?yàn)檫\(yùn)動參數(shù)和照片都有噪聲，所以需要進(jìn)行優(yōu)化。而過去卡爾曼濾波只關(guān)心當(dāng)前的狀態(tài)估計(jì)，而非線性優(yōu)化則對所有時(shí)刻采集的數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)，被認(rèn)為優(yōu)于卡爾曼濾波。由于要估計(jì)所有的采集數(shù)據(jù)，所以待估計(jì)變量就變成：

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

所以對機(jī)器人狀態(tài)的估計(jì)，就是求已知輸入數(shù)據(jù)u(傳感器參數(shù))和觀測數(shù)據(jù)z(圖像像素)的條件下，計(jì)算狀態(tài)x的條件概率分布（也就是根據(jù)u和z的數(shù)據(jù)事件好壞來估計(jì)x的優(yōu)劣事件概率情況，這其中包含著關(guān)聯(lián)，就好像已知一箱子里面有u和z個(gè)劣質(zhì)的商品，求取出x個(gè)全是好商品的概率，同樣的樣本點(diǎn)，但是從不同角度分析可以得出不同的事件，不同的事件概率之間可以通過某些已知數(shù)據(jù)得出另些事件的概率，通過一系列函數(shù)式計(jì)算得出）：

P(x|z, u)

在不考慮運(yùn)動方程，只考慮觀測照片z的情況下求x（這個(gè)過程也稱SfM運(yùn)動恢復(fù)），那么就變成P(x|z)。這種根據(jù)已知求未知的情況可以通過貝葉斯公式求解，P(x|z)=P(x)P(z|x)/P(z)。

又因?yàn)橹磺髕所以忽略P(z)，所以P(x|z)=P(x)P(z|x)

那么根據(jù)貝葉斯公式的含義，P(x)為先驗(yàn)概率，P(z|x)為似然概率。

而P(x)是不知道，所以主要求P(z|x)也就是最大似然概率。

最大似然的概率用自然語言描述就是在什么狀態(tài)下最有可能產(chǎn)生當(dāng)前的照片結(jié)果！

如何求呢？

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

現(xiàn)在要求x導(dǎo)致z出現(xiàn)的概率，要求z的最大概率，由于v是誤差，符合高斯分布vk~N(0, Qk,j)，所以z也符合高斯分布，即P(zi,k|xk, yj)=N(h(yj, xk), Qk,j)。

所以求多維的概率最大值即為

所以對于所有的運(yùn)動和觀測，有：

從而得到了一個(gè)總體意義下的最小二乘問題。它的最優(yōu)解等于狀態(tài)的最大似然估計(jì)。

它的意義是說P(z,u|x)表明我們把估計(jì)的軌跡和地圖(xk,yj)代入SLAM的運(yùn)動、觀測方程中時(shí)，它們并不會完美的成立。這時(shí)候怎么辦呢？我們把狀態(tài)的估計(jì)值進(jìn)行微調(diào)，使得整體的誤差下降一些。它一般會到極小值。這就是一個(gè)典型的非線性優(yōu)化過程。

因?yàn)樗蔷€性，所以df/dx有時(shí)候不好求，那么可以采用迭代法（有極值的話，那么它收斂，一步步逼近）：

1.給定某個(gè)初始值x0。

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

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

4. 否則，令xk+1=xk+ △xk，返回2。

這樣求導(dǎo)問題就變成了遞歸逼近問題，那么增量△xk如何確定？

這里介紹三種方法：

（1）一階和二階梯度法

將目標(biāo)函數(shù)在x附近進(jìn)行泰勒展開：

其中J為||f(x)||2關(guān)于x的導(dǎo)數(shù)（雅克比矩陣），而H則是二階導(dǎo)數(shù)（海塞(Hessian)矩陣）。我們可以選擇一階或二階，增量不同。但都是為了求最小值，使函數(shù)值最小。

但最速下降法下降太快容易走出鋸齒路線，反而增加了迭代次數(shù)。而牛頓法則需要計(jì)算目標(biāo)函數(shù)的H矩陣，這在問題規(guī)模較大時(shí)非常困難，我們通常為了避免求解H。所以，接下來的兩種最常用的方法：高斯牛頓法和列文伯格-馬夸爾特方法。

（2）高斯牛頓法

將f(x)一階展開：

這里J(x)為f(x)關(guān)于x的導(dǎo)數(shù)，實(shí)際上是一個(gè)m×n的矩陣，也是一個(gè)雅克比矩陣。現(xiàn)在要求△x，使得||f(x+△x)|| 達(dá)到最小。為求△ x，我們需要解一個(gè)線性的最小二乘問題：

這里注意變量是△ x，而非之前的x了。所以是線性函數(shù)，好求。為此，先展開目標(biāo)函數(shù)的平方項(xiàng)：

求導(dǎo)，令其等于零從而得到：

我們稱為高斯牛頓方程。我們把左邊的系數(shù)定義為H，右邊定義為g，那么上式變?yōu)?#xff1a;

H △x=g

跟第（1）種方法對比可以發(fā)現(xiàn)，我們用J(x)^TJ(x)代替H的求法，從而節(jié)省了計(jì)算量。所以高斯牛頓法跟牛頓法相比的優(yōu)點(diǎn)就在于此，步驟列于下：

1.給定初始值x0。

2.對于第k次迭代，求出當(dāng)前的雅克比矩陣J(xk)和誤差f(xk)。

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

4.若△xk足夠小，則停止。否則，令xk+1=xk+△xk，返回2。

但是，還是有缺陷，就是它要求我們所用的近似H矩陣是可逆的（而且是正定的），但實(shí)際數(shù)據(jù)中計(jì)算得到的J^TJ卻只有半正定性。也就是說，在使用Gauss Newton方法時(shí)，可能出現(xiàn)J^TJ為奇異矩陣或者病態(tài)（ill-condition）的情況，此時(shí)增量的穩(wěn)定性較差，導(dǎo)致算法不收斂。更嚴(yán)重的是，就算我們假設(shè)H非奇異也非病態(tài)，如果我們求出來的步長△x太大，也會導(dǎo)致我們采用的局部近似（6.19）不夠準(zhǔn)確，這樣一來我們甚至都無法保證它的迭代收斂，哪怕是讓目標(biāo)函數(shù)變得更大都是可能的。

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

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

由于Gauss-Newton方法中采用的近似二階泰勒展開只能在展開點(diǎn)附近有較好的近似效果，所以我們很自然地想到應(yīng)該給△x添加一個(gè)信賴區(qū)域（Trust Region），不能讓它太大而使得近似不準(zhǔn)確。非線性優(yōu)化中有一系列這類方法，這類方法也被稱之為信賴區(qū)域方法（Trust Region Method）。在信賴區(qū)域里邊，我們認(rèn)為近似是有效的；出了這個(gè)區(qū)域，近似可能會出問題。

那么如何確定這個(gè)信賴區(qū)域的范圍呢？一個(gè)比較好的方法是根據(jù)我們的近似模型跟實(shí)際函數(shù)之間的差異來確定這個(gè)范圍：如果差異小，我們就讓范圍盡可能大；如果差異大，我們就縮小這個(gè)近似范圍。因此，考慮使用：

來判斷泰勒近似是否夠好。ρ的分子是實(shí)際函數(shù)下降的值，分母是近似模型下降的值。如果ρ接近于1，則近似是好的。如果ρ太小，說明實(shí)際減小的值遠(yuǎn)少于近似減小的值，則認(rèn)為近似比較差，需要縮小近似范圍。反之，如果ρ比較大，則說明實(shí)際下降的比預(yù)計(jì)的更大，我們可以放大近似范圍。

于是，我們構(gòu)建一個(gè)改良版的非線性優(yōu)化框架，該框架會比Gauss Newton有更好的效果。

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

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

?

?這里面的限制條件μ是信賴區(qū)域的半徑，D將在后文說明。

3. 計(jì)算ρ。

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

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

6. 如果ρ大于某閾值，認(rèn)為近似可行。令xk+1=xk+△xk。

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

最后化簡得到：

當(dāng)λ比較小時(shí)，接近于牛頓高斯法，當(dāng)λ比較大時(shí)，接近于最速下降法。L-M的求解方式，可在一定程度上避免線性方程組的系數(shù)矩陣的非奇異和病態(tài)問題，提供更穩(wěn)定更準(zhǔn)確的增量△x。

下面是實(shí)踐：

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

編譯glog、gflags和ceres-solver，然后在項(xiàng)目工程中設(shè)置：

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：圖優(yōu)化。

?在視覺slam里更常用的非線性優(yōu)化是圖優(yōu)化，它是將非線性優(yōu)化和圖論結(jié)合在一起的理論。它不是僅僅將幾種類型的誤差加在一起用最小二乘法求解，而是增加了位姿最小二乘和觀測最小二乘之間的關(guān)系（比如位姿方程xk=f(fk-1,uk)+wk和觀測方程zk,j=h(yj,xk)+vk,j都有一個(gè)變量xk聯(lián)系了位姿方程和觀測方程）。所以就用到了圖優(yōu)化。

藍(lán)線表示了位姿最小二乘優(yōu)化結(jié)果（運(yùn)動誤差），而紅線表示觀測最小二乘結(jié)果（觀測誤差），兩者之間用位置變量xk關(guān)聯(lián)，或者說約束。頂點(diǎn)表示優(yōu)化變量（xk處的u導(dǎo)致的位姿參數(shù)誤差和pk,j處的圖像噪聲導(dǎo)致的觀測值誤差），

所以在g2o圖優(yōu)化中，就可以將問題抽象為要優(yōu)化的點(diǎn)和誤差邊，然后求解。

?

第7講 視覺里程計(jì)1

根據(jù)相鄰圖像的信息估算相機(jī)的運(yùn)動稱為視覺里程計(jì)（VO）。一般需要先提取兩幅圖像的特征點(diǎn)，然后進(jìn)行匹配，根據(jù)匹配的特征點(diǎn)估計(jì)相機(jī)運(yùn)動。從而給后端提供較為合理的初始值。

1. 特征點(diǎn)：特征檢測算子SIFT，SURF，ORB等等。

SIFT算子比較“奢侈”，考慮的比較多，對于SLAM算法來說有點(diǎn)太“奢侈”。不太常用目前。

ORB算子是在FAST算子基礎(chǔ)上發(fā)展起來，在特征點(diǎn)數(shù)量上精簡了，而且加上了方向和旋轉(zhuǎn)特性(通過求質(zhì)心)，并改進(jìn)了尺度不變性（通過構(gòu)建圖像金字塔）。從而實(shí)現(xiàn)通過FAST提取特征點(diǎn)并計(jì)算主方向，通過BRIEF計(jì)算描述子的ORB算法。

2. 特征匹配：特征匹配精度將影響到后續(xù)的位姿估計(jì)、優(yōu)化等。

實(shí)踐：特征提取和匹配

略。

根據(jù)提取的匹配點(diǎn)對，估計(jì)相機(jī)的運(yùn)動。由于相機(jī)的不同，情況不同：

1.當(dāng)相機(jī)為單目時(shí)，我們只知道2D的像素坐標(biāo)，因而問題是根據(jù)兩組2D點(diǎn)估計(jì)運(yùn)動。該問題用對極幾何來解決。

2.當(dāng)相機(jī)為雙目、RGB-D時(shí)，或者我們通過某種方法得到了距離信息，那問題就是根據(jù)兩組3D點(diǎn)估計(jì)運(yùn)動。該問題通常用ICP來解決。

3.如果我們有3D點(diǎn)和它們在相機(jī)的投影位置，也能估計(jì)相機(jī)的運(yùn)動。該問題通過PnP求解。

分別來介紹。

1. 對極幾何

假設(shè)我們從兩張圖像中，得到了若干對這樣的匹配點(diǎn)，就可以通過這些二維圖像點(diǎn)的對應(yīng)關(guān)系，恢復(fù)出在兩幀之間攝像機(jī)的運(yùn)動。

根據(jù)小孔成像原理（s₁p₁=KP, s₂p₂=K(RP+t)）可以得到x₂^Tt^Rx₁=0

其中x2,x1為兩個(gè)像素點(diǎn)的歸一化平面上的坐標(biāo)。

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

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

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

它代表了O1,O2,P三點(diǎn)共面。它同時(shí)包含了旋轉(zhuǎn)R和平移t。把中間部分分別記為基礎(chǔ)矩陣F和本質(zhì)矩陣E，可以進(jìn)一步化簡對極約束公式：

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

它給出了兩個(gè)匹配點(diǎn)的空間位置關(guān)系，于是，相機(jī)位姿估計(jì)問題可以變?yōu)閮刹?#xff1a;

1. 根據(jù)配對點(diǎn)的像素位置，求出E或者F；

2.?根據(jù)E或者F，求出R, t。

由于K一般已知，所以一般求E。而E=t^R可知共有6個(gè)自由度，但是由于尺度不變性，所以共有5個(gè)自由度。可以利用八點(diǎn)法采用線性代數(shù)求解。

那么得到本質(zhì)矩陣E之后，如何求出R,t呢？這個(gè)過程是由奇異值分解（SVD）得到的。設(shè)E的SVD分解為：

E=UΣV^T

其中U,V為正交陣，Σ為奇異值矩陣。根據(jù)E的內(nèi)在性質(zhì)，我們知道Σ=diag(δ, δ, 0)。對于任意一個(gè)E，存在兩個(gè)可能的t,R與它對應(yīng)：

其中Rz(π/2)表示繞Z軸旋轉(zhuǎn)90度得到的旋轉(zhuǎn)矩陣。同時(shí)對E來說，E與-E對零等式無影響。所以可以得到4種組合。

但根據(jù)點(diǎn)的深度值可以確定正確的那一組組合。

除了本質(zhì)矩陣，我們還要求單應(yīng)矩陣，單應(yīng)矩陣是指當(dāng)特征點(diǎn)位于同一平面時(shí)可以構(gòu)建單應(yīng)矩陣。它的作用是在相機(jī)不滿足八個(gè)參數(shù)時(shí)，比如只旋轉(zhuǎn)沒有移動，那么就可以通過單應(yīng)矩陣來估計(jì)。求法略。

實(shí)踐：對極約束求解相機(jī)運(yùn)動

略。

求得了相機(jī)運(yùn)動參數(shù)之后，那么需要利用相機(jī)的運(yùn)動參數(shù)估計(jì)特征點(diǎn)的空間位置。由s₁x₁=s₂Rx₂+t可得s₁和s₂的值（通過s₁x₁^x₁=0=s₂x₁^Rx₂+x₁^t）。由R,t以及深度信息可以求得特征像素點(diǎn)的空間點(diǎn)坐標(biāo)。

實(shí)踐：三角測量

略。

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

可以直接采用直接線性變換，即不需要求解內(nèi)外方位元素的變換。直接線性變換即DLT。

而P3P是另一種解PnP的方法。P3P即只利用三對匹配點(diǎn)，它僅使用三對匹配點(diǎn)，對數(shù)據(jù)要求較少。推導(dǎo)過程：

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

?

它存在2個(gè)缺點(diǎn)：（1）P3P只利用三個(gè)點(diǎn)的信息。當(dāng)給定的配對點(diǎn)多于3組時(shí)，難以利用更多的信息。（2）如果3D點(diǎn)或2D點(diǎn)受噪聲影響，或者存在誤匹配，則算法失效。

那么，后續(xù)人們還提出了許多別的方法，如EPnP、UPnP等。它們利用更多的信息，而且用迭代的方式對相機(jī)位姿進(jìn)行優(yōu)化，以盡可能地消除噪聲的影響。

在SLAM中通常做法是先使用P3P/EPnP等方法估計(jì)相機(jī)位姿(R,t)，然后構(gòu)建最小二乘優(yōu)化問題對估計(jì)值(R,t)進(jìn)行調(diào)整（Bundle Adjustment）。下面，介紹一下Bundle Adjustment。

Bundle Adjustment是一種非線性的方式，它是將相機(jī)位姿和空間點(diǎn)位置都看成優(yōu)化變量，放在一起優(yōu)化。可以用它對PnP或ICP給出的結(jié)果進(jìn)行優(yōu)化。在PnP中，這個(gè)Bundle Adjustment問題，是一個(gè)最小化重投影誤差的問題。本節(jié)給出此問題在兩個(gè)視圖下的基本形式，然后在第十講討論較大規(guī)模的BA問題。

考慮n個(gè)三維空間點(diǎn)P和它們的投影p，我們希望計(jì)算相機(jī)的位姿R,t，它的李代數(shù)表示為ζ。假設(shè)某空間點(diǎn)坐標(biāo)為P_i=[X_i, Y_i, Z_i]^T。其投影的像素坐標(biāo)為u_i=[u_i,v_i]。根據(jù)第五章的內(nèi)容，像素位置與空間點(diǎn)位置的關(guān)系如下：

?除了用ζ為李代數(shù)表示的相機(jī)姿態(tài)之外，別的都和前面的定義保持一致。寫成矩陣形式就是：

s_iu_i=Kexp(ζ^{^})Pi

但由于噪聲以及相機(jī)位姿的原因，該等式并不總成立。所以，需要用最小二乘法

這也叫重投影誤差。由于需要考慮很多個(gè)點(diǎn)，所以最后每個(gè)點(diǎn)的誤差通常都不會為零。最小二乘優(yōu)化問題已經(jīng)在第六講介紹過了。使用李代數(shù)，可以構(gòu)建無約束的優(yōu)化問題，很方便地通過G-N，L-M等優(yōu)化算法進(jìn)行求解。不過，在使用G-N和L-M之前，我們需要知道每個(gè)誤差項(xiàng)關(guān)于優(yōu)化變量的導(dǎo)數(shù)，也就是線性化：

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

其中Δx即像素變量坐標(biāo)誤差。e(x+Δx)即偏移后的值。

當(dāng)e為像素坐標(biāo)誤差(2維)，x為相機(jī)位姿(6維,旋轉(zhuǎn)+平移)，J將是一個(gè)2×6的矩陣。推導(dǎo)一下J的形式。

回憶李代數(shù)的內(nèi)容，我們介紹了如何使用擾動模型來求李代數(shù)的導(dǎo)數(shù)。首先，記變換到相機(jī)坐標(biāo)系下的空間點(diǎn)坐標(biāo)為P’，并且把它前三維取出來：

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

那么，相機(jī)投影模型相對于P'則為：

? ? ? ? ? ? ? ? ?su=KP'

展開之：

?

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

這與之前講的相機(jī)模型是一致的。當(dāng)我們求誤差時(shí)，可以把這里的u,v與實(shí)際的測量值比較，求差。在定義了中間變量后，我們對ζ^左乘擾動量δζ，然后考慮e的變化關(guān)于擾動量的導(dǎo)數(shù)。利用鏈?zhǔn)椒▌t，可以列寫如下(這里ζ指的是旋轉(zhuǎn)量和平移量6個(gè)參數(shù))：

? ? ? ? --->?? ??

?

除了擾動量，還有希望優(yōu)化特征點(diǎn)的空間位置。

? ? ??--->? ? ?

?

于是，推導(dǎo)出了觀測相機(jī)方程關(guān)于相機(jī)位姿與特征點(diǎn)的兩個(gè)導(dǎo)數(shù)矩陣。它們特別重要，能夠在優(yōu)化過程中提供重要的梯度方向，指導(dǎo)優(yōu)化的迭代。

實(shí)踐：（1）先使用EPnP程序求解位姿，然后（2）對位姿和點(diǎn)坐標(biāo)進(jìn)行BA非線性優(yōu)化。

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

ICP:SVD法：已有兩個(gè)RGB-D圖像，通過特征匹配獲取兩組3D點(diǎn)，最后用ICP計(jì)算它們的位姿變換。先定義第i對點(diǎn)的誤差項(xiàng)：e_i=p_i-(Rp_i'+t)。然后構(gòu)建最小二乘問題，求使誤差平方和達(dá)到極小的R,t。先求R,再求t。為求R，得到-tr(R∑q_i'q_i^T)，要使R最小從而定義矩陣W=U∑V^T，其中∑為奇異值組成的對角矩陣，對角線元素從大到小排列，而U和V為正交矩陣。當(dāng)W滿秩時(shí)，R為：

R=UV^T.

得到R后，即可求解t。

ICP:非線性方法。跟第四講的內(nèi)容一樣。直接使用。

實(shí)踐：求解ICP，略。

?

第8講 視覺里程計(jì)2

本節(jié)先介紹光流法跟蹤特征點(diǎn)的原理，然后介紹另一種估計(jì)相機(jī)位姿的方法——直接法，現(xiàn)在直接法已經(jīng)一定程度上可以和特征點(diǎn)法估計(jì)相機(jī)位姿平分秋色。

光流即圖像上的某個(gè)灰度值在不同時(shí)刻的圖像上的流動。用圖像表示就是，不同圖像的灰度像素之間的關(guān)聯(lián)

?按照追蹤的像素的多少可以分為稀疏光流和稠密光流，這里主要講稀疏光流中的Lucas-Kanade法，稱為LK光流。

它假設(shè)一個(gè)像素在不同的圖像中是固定不變的，我們知道這并不總是成立的，但我們只是先這樣假設(shè)，不然什么都沒法做。

所以對于t時(shí)刻圖像(x,y)處的像素I(x, y, t)，經(jīng)過dt時(shí)刻后在t+dt它來到了(x+dx, y+dy)處，這是一個(gè)運(yùn)動過程，具體怎么運(yùn)動方向(實(shí)際運(yùn)動是三維反映在圖像上是二維)怎么樣不去管它，那么由假設(shè)可知I(x+dx, y+dy, t+dt)=I(x, y, t)。我們知道一個(gè)像素沿著x,y軸各移動一定的距離，距離上的速度分別為u,v。那么u,v各是多少呢？怎么求呢？求它們有什么用呢？看下圖，假設(shè)前一秒和后一秒相機(jī)運(yùn)動如下

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

可以通過相鄰像素的顏色梯度和自身像素的顏色變化來描述。也就是通過相鄰像素梯度和運(yùn)行速度得到自身該位置的顏色值變化，列出一個(gè)方程，以此來描述圖像的平移。

當(dāng)這個(gè)運(yùn)動足夠小時(shí)可以對左邊的式子進(jìn)行泰勒展開，因?yàn)镮(x,y,t)部分是不變的所以得到：

--->

?兩邊除以dt，得：

?記梯度分別為I_x,I_y，速度分別為u,v，而右側(cè)的灰度對時(shí)間的變化量I_t。

?可以得到：

根據(jù)最小二乘法可以得到：?通過特征塊求解u,v。

得到了像素在圖像間的運(yùn)行速度u,v，就可以估計(jì)特征點(diǎn)在不同圖像中的位置了。當(dāng)t取離散的時(shí)刻而不是連續(xù)時(shí)間時(shí)，我們可以估計(jì)某塊像素在若干個(gè)圖像中出現(xiàn)的位置。由于像素梯度僅在局部有效，所以如果一次迭代不夠好的話，我們會多迭代幾次這個(gè)方程。

下面通過實(shí)踐來體會一下LK光流在跟蹤角點(diǎn)上的應(yīng)用。

例：使用LK光流法對第一張圖像提取FAST角點(diǎn)，然后用LK光流跟蹤它們，畫在圖中。

slambook/ch8/useLK/useLK.cpp

?

可以看到特征點(diǎn)在逐漸減少，需要不斷添加新的特征點(diǎn)進(jìn)去。光流法的優(yōu)點(diǎn)是可以避免描述子的計(jì)算，這可以避免誤匹配，但是如果運(yùn)動太快時(shí)就容易追蹤錯誤還是描述子好些。

然后就可以根據(jù)光流法跟蹤的特征點(diǎn)通過PnP、ICP或?qū)O幾何來估計(jì)相機(jī)運(yùn)動，這之前已講過，不再贅述。

而直接法是直接連特征點(diǎn)都不提取了，直接用類似光流法的梯度迭代完成。計(jì)算得到位姿估計(jì)。它的思想是光流法+迭代近似。從而在光流法的基礎(chǔ)上繼續(xù)減少特征提取的時(shí)間。

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

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

利用最小二乘法

當(dāng)有多個(gè)點(diǎn)時(shí)，，ei為第i像素的像素差

我們要求的是ζ的優(yōu)化值，假設(shè)ei有偏差，那么J(ζ)有如何的變動呢？我們需要求兩者之間的關(guān)系

我們假設(shè)ζ出現(xiàn)了一點(diǎn)偏差，考慮ei的變化，道理是一樣的，求兩者的關(guān)系，得到：

最后經(jīng)過一系列變換得到一個(gè)e與ζ的關(guān)系式：

其中，而[X, Y, Z]為三維點(diǎn)坐標(biāo)。

對于RGBD相機(jī)來說像素對應(yīng)的XYZ比較好確定(我們不知道第二個(gè)像素是什么我們不獲取第二個(gè)像素對應(yīng)的XYZ，假設(shè)第二個(gè)像素是由第一個(gè)像素和第一個(gè)像素對應(yīng)的XYZ經(jīng)過姿態(tài)影射得到的)，但對于沒有深度的純單目相機(jī)來說要麻煩，因?yàn)樯疃炔缓么_定，那只能假設(shè)其有一個(gè)深度，經(jīng)過姿態(tài)后投影到第二個(gè)像素上，詳細(xì)深度估計(jì)放到第13講。

下面進(jìn)行直接法的實(shí)踐。[這里Eigen要使用Eigen3以上的包，因?yàn)橐褂肊igen::Map函數(shù)。]

（1）稀疏直接法。基于特征點(diǎn)的深度恢復(fù)上一講介紹過了，基于塊匹配的深度恢復(fù)將在后面章節(jié)中介紹，所以本節(jié)我們來考慮RGB-D上的稀疏直接法VO。求解直接法最后等價(jià)于求解一個(gè)優(yōu)化問題，因此可以使用g2o或Ceres這些優(yōu)化庫來求解。稀疏怎么個(gè)稀疏嗎？求特征點(diǎn)。稀疏直接法跟光流法的區(qū)別在于光流法是通過平面圖像的運(yùn)動來估計(jì)第二個(gè)圖像上的關(guān)鍵點(diǎn)像素，而稀疏直接法是通過位姿估計(jì)來實(shí)現(xiàn)。

（2）半稠密直接法。把特征點(diǎn)改為凡是像素梯度較大的點(diǎn)就是半稠密直接法。

?

第9講 實(shí)踐章：設(shè)計(jì)前端

前面兩節(jié)我們學(xué)習(xí)了視覺里程計(jì)（VO）的原理，現(xiàn)在設(shè)計(jì)一個(gè)VO。VO也就是前端，與后端優(yōu)化相對應(yīng)。這節(jié)我們利用前面所學(xué)的知識實(shí)際設(shè)計(jì)一個(gè)前端框架。你將會發(fā)現(xiàn)VO前端會遇到很多突發(fā)狀況，畢竟我們前面所學(xué)的只是完美假設(shè)情況下的理論，但實(shí)際情況是總會有意外的情況如相機(jī)運(yùn)動過快、圖像模糊、誤匹配等。可以看到這節(jié)主要是前面的理論的實(shí)踐，你將實(shí)際動手制作一個(gè)視覺里程計(jì)程序。你會管理局部的機(jī)器人軌跡與路標(biāo)點(diǎn)(參考論文)，并體驗(yàn)一下一個(gè)軟件框架是如何組成的。

我們知道光知道原理跟建造起偉大的建筑作品之間是有很大的區(qū)別的，可以說這里面是還有長長的路要走的。就像你知道三維建模的原理，知道各個(gè)部件的建造原理知道怎么搭建，這跟真正建造起美輪美奐的建筑物之間是有區(qū)別的，這需要發(fā)揮你的創(chuàng)造力和毅力來建造起復(fù)雜的建筑模型，光知道各個(gè)部分原理是不夠的，你既需要統(tǒng)籌全局又需要優(yōu)化部分的能力，這是需要廣與小都具備才行。這跟SLAM相似，SLAM是個(gè)很偉大的建筑物，而各個(gè)柱子和屋頂之間如何搭建需要考慮之間的關(guān)聯(lián)，而各個(gè)柱子、各個(gè)瓦片又需要再細(xì)細(xì)優(yōu)化打磨，所以說，從局部到整體都需要考慮到，整體需要局部以及局部之間的關(guān)系。

這是一個(gè)從簡到繁的過程，我們會從簡單的數(shù)據(jù)結(jié)構(gòu)開始，慢慢從簡單到復(fù)雜建立起整個(gè)SLAM。在project文件夾下，從0.1到0.4你可以感受到版本的變化，從簡單到復(fù)雜的整個(gè)過程。

現(xiàn)在我們要寫一個(gè)SLAM庫文件，目標(biāo)是幫讀者將本書用到的各種算法融會貫通，書寫自己的SLAM程序。

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

基本的數(shù)據(jù)結(jié)構(gòu)：

數(shù)據(jù)是基礎(chǔ)，數(shù)據(jù)和算法構(gòu)成程序。那么，有哪些基本的數(shù)據(jù)結(jié)構(gòu)呢？

1. 圖像幀。一幀是相機(jī)采集到的圖像單位。它主要包含一個(gè)圖像。此外還有特征點(diǎn)、位姿、內(nèi)參等信息。而且圖像一般要選擇關(guān)鍵幀，不會都存儲那樣成本太高。

2. 路標(biāo)。路標(biāo)點(diǎn)即圖像中的特征點(diǎn)。

3. 配置文件。將各種配置參數(shù)單獨(dú)存儲到一個(gè)文件中，方便讀取。

4. 坐標(biāo)變換類。你需要經(jīng)常進(jìn)行坐標(biāo)系間的坐標(biāo)變換。

所以我們建立五個(gè)類：Frame為幀，Camera為相機(jī)模型，MapPoint為特征點(diǎn)/路標(biāo)點(diǎn)，Map管理特征點(diǎn)，Config提供配置參數(shù)。

其中的包含關(guān)系。一幀圖像有一個(gè)Camera模型和多個(gè)特征點(diǎn)。而一個(gè)地圖有多個(gè)特征點(diǎn)組成。

?1. Camera類

#ifndef CAMERA_H #define CAMERA_H #include "common_include.h"namespace myslam {//Pinhole RGB-D camera modelclass 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, pixelVector3d 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是為了避免兩次重復(fù)引用導(dǎo)致出錯，在頭文件中必須要加上(這點(diǎn)很重要切記，不然A->B,A->C,那么B,C->D時(shí)，D類中就引用了兩次A)，而這樣還不夠，有時(shí)候我們總喜歡用同樣的類名，為了盡量避免這種情況發(fā)生，我們也必須加上自己的namespace命名空間，一般是以自己的個(gè)性化來命名區(qū)分。而且為了避免發(fā)生內(nèi)存泄露，我們應(yīng)該為我們的類加上智能指針shared_ptr，以后在傳遞參數(shù)時(shí)只需要使用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 framedouble time_stamp_; // when it is recordedSE3 T_c_w_; // transform from world to cameraCamera::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 functionstatic Frame::Ptr createFrame(); // find the depth in depth mapdouble findDepth( const cv::KeyPoint& kp );// Get Camera CenterVector3d 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_Hnamespace myslam {class Frame; class MapPoint { public:typedef shared_ptr<MapPoint> Ptr;unsigned long id_; // IDVector3d pos_; // Position in worldVector3d 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 functionstatic 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 landmarksunordered_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 singletonpublic:~Config(); //close the file when deconstructing//set a new config filestatic void setParameterFile(const std::string& filename);//access the parameter valuestemplate<typename T>static T get(const std::string& key){return T(Config::config_->file_[key]);}}; }#endif

數(shù)據(jù)結(jié)構(gòu)設(shè)計(jì)完之后就可以設(shè)計(jì)程序了，先進(jìn)行最基本的VO：特征提取和匹配

第一種是兩兩幀的視覺里程計(jì)，不進(jìn)行優(yōu)化，不計(jì)算特征點(diǎn)位置，只進(jìn)行兩兩幀的運(yùn)動估計(jì)。

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 statusMap::Ptr map_; //map with all frames and map pointsFrame::Ptr ref_; //reference frameFrame::Ptr curr_; //current frame cv::Ptr<cv::ORB> orb_; //orb detector and computervector<cv::Point3f> pts_3d_ref_; //3d points in reference framevector<cv::KeyPoint> keypoints_curr_; //keypoints in current frameMat descriptors_curr_; //descriptor in current frameMat descriptors_ref_; //descriptor in reference framevector<cv::DMatch> feature_matches_;SE3 T_c_r_estimated_; //the estimated pose of current frameint num_inliers_; //number of inlier features in icpint num_lost_; //number of lost times//parametersint num_of_features_; //number of featuresdouble scale_factor_; //scale in image pyramidint level_pyramid_; //number of pyramid levelsfloat match_ratio_; //ratio for selecting good matchesint max_num_lost_; //max number of continuos lost timesint min_inliers_; //minimum inliersdouble key_frame_min_rot; //minimal rotation of tow key-framesdouble key_frame_min_trans; //minimal translation of two key-framespublic: //functions VisualOdometry();~VisualOdometry();bool addFrame(Frame::Ptr frame); //add a new frameprotected://inner operationvoid extractKeyPoints();void computeDescriptors();void featureMatching();void poseEstimationPnP();void setRef3DPoints();void addKeyFrame();bool checkEstimatedPose();bool checkKeyFrame();}; }#endif

關(guān)于這個(gè)VisualOdometry類，有幾點(diǎn)需要解釋：

1.?addFrame函數(shù)是外部調(diào)用的接口。使用VO時(shí)，將圖像數(shù)據(jù)裝入Frame類后，調(diào)用addFrame估計(jì)其位姿。該函數(shù)根據(jù)VO所處的狀態(tài)實(shí)現(xiàn)不同的操作：

bool VisualOdometry::addFrame(Frame::Ptr frame) { switch(state_) { case INITIALIZING: //初始化，該幀為第一幅 { state_=OK; //修改狀態(tài)為OK curr_=ref_=frame; //將該幀賦給當(dāng)前幀和參考幀 map_->insertKeyFrame(frame); //將該幀插入地圖 //從第一幀中提取特征點(diǎn) extractKeyPoints(); //提取特征點(diǎn) computeDescriptors(); //計(jì)算描述子 //計(jì)算參考幀中的特征點(diǎn)的三維坐標(biāo)點(diǎn) 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; }

?值得一提的是，由于各種原因，我們設(shè)計(jì)的上述VO算法，每一步都有可能失敗。例如圖片中不易提特征、特征點(diǎn)缺少深度值、誤匹配、運(yùn)動估計(jì)出錯等等。因此，要設(shè)計(jì)一個(gè)魯棒的VO，必須（最好是顯式地）考慮到上述所有可能出錯的地方——那自然會使程序變得復(fù)雜。我們在checkEstimatedPose中，根據(jù)內(nèi)點(diǎn)（inlier）的數(shù)量以及運(yùn)動的大小做一個(gè)簡單的檢測：認(rèn)為內(nèi)點(diǎn)不可太少，而運(yùn)動不可能過大。當(dāng)然，讀者也可以思考其他檢測問題的手段，嘗試一下效果。

VisualOdometry的其余部分略，下面進(jìn)行一下測試Test。

slambook/project/0.2/test/run_vo.cpp

程序略，結(jié)果如下圖所示。

注意：這里使用的OpenCV3的viz模塊顯示估計(jì)位姿，請確保你安裝的是OpenCV3，并且viz模塊也編譯安裝了。準(zhǔn)備tum數(shù)據(jù)集中的其中一個(gè)。簡單起見，我推薦fr1_xyz那一個(gè)。請使用associate.py生成一個(gè)配對文件associate.txt。關(guān)于tum數(shù)據(jù)集格式我們已經(jīng)在8.3節(jié)中介紹過了。在config/defualt.yaml中填寫你的數(shù)據(jù)集所在路徑，參照我的寫法即可。然后用bin/run_vo config/default.yaml執(zhí)行程序，就可以看到實(shí)時(shí)的演示了。

改進(jìn)：優(yōu)化PnP的結(jié)果。沿著之前的內(nèi)容，嘗試一些VO的改進(jìn)。嘗試RANSAC PnP加上迭代優(yōu)化的方式估計(jì)相機(jī)位姿。非線性優(yōu)化之前已經(jīng)介紹過了，此處略。

改進(jìn)：局部地圖。將VO匹配到的特征點(diǎn)放到地圖中，并將當(dāng)前幀跟地圖點(diǎn)進(jìn)行匹配，計(jì)算位姿。地圖又分為全局地圖和局部地圖，全局地圖太奢侈了，主要用于回環(huán)檢測和地圖表達(dá)。局部地圖是指隨著相機(jī)運(yùn)動，往地圖里添加新的特征點(diǎn)，并將視野之外的特征點(diǎn)刪掉，并且統(tǒng)計(jì)每個(gè)地圖點(diǎn)被觀測到的次數(shù)等等。

?

第10講 后端1

本節(jié)我們開始轉(zhuǎn)入SLAM系統(tǒng)的另一個(gè)重要模塊：后端優(yōu)化。前面的視覺里程計(jì)可以給出一個(gè)短時(shí)間內(nèi)的軌跡和地圖，但由于不可避免的誤差累積，如果時(shí)間長了這個(gè)地圖是不準(zhǔn)確的。所以我們希望構(gòu)建一個(gè)尺度、規(guī)模更大的優(yōu)化問題，以考慮長時(shí)間內(nèi)的最優(yōu)軌跡和地圖。實(shí)際當(dāng)中考慮到精度與性能的平衡，有許多不同的做法。

1. 什么叫后端？

?之前提到，視覺里程計(jì)只有短暫的記憶，而我們希望整個(gè)運(yùn)動軌跡在較長時(shí)間內(nèi)都能保持最優(yōu)的狀態(tài)。我們可能會用最新的知識，更新較久遠(yuǎn)的狀態(tài)——站在“久遠(yuǎn)的狀態(tài)”的角度上看，仿佛是未來的信息告訴它“你應(yīng)該在哪里”。所以，在后端優(yōu)化中，我們通常考慮一段更長時(shí)間內(nèi)（或所有時(shí)間內(nèi)）的狀態(tài)估計(jì)問題，而且不僅使用過去的信息更新自己的狀態(tài)，也會用未來的信息來更新自己，這種處理方式不妨稱為“批量的”（Batch）。否則，如果當(dāng)前的狀態(tài)只由過去的時(shí)刻決定，甚至只由前一個(gè)時(shí)刻決定，那不妨稱為“漸進(jìn)的”（Incremental）。

所以這是一個(gè)假設(shè)檢驗(yàn)的過程。先驗(yàn)和后驗(yàn)。先由以前的位姿和觀測方程預(yù)測下一步，然后利用下一步的內(nèi)容預(yù)測這一步的最大可能。是概率統(tǒng)計(jì)中的知識。

2. 狀態(tài)估計(jì)的概率解釋

我們已經(jīng)知道SLAM過程可以由運(yùn)動方程和觀測方程來描述。那么，假設(shè)在t=0到t=N的時(shí)間內(nèi)，有位姿x₀到x_N，并且有路標(biāo)y₁,…,y_M。按照之前的寫法，運(yùn)動和觀測方程為：

?注意以下幾點(diǎn)：

（1）在觀測方程中，只有當(dāng)x_k看到了y_j時(shí)，才會產(chǎn)生觀測數(shù)據(jù)。

（2）我們可能沒有測量運(yùn)動的裝置，所以也可能沒有運(yùn)動方程。在這個(gè)情況下，有若干種處理方式：認(rèn)為確實(shí)沒有運(yùn)動方程，或假設(shè)相機(jī)不動，或假設(shè)相機(jī)勻速運(yùn)動。這幾種方式都是可行的。在沒有運(yùn)動方程的情況下，整個(gè)優(yōu)化問題就只由許多個(gè)觀測方程組成。這就非常類似于StM(Structure from Motion)問題，由一組圖像恢復(fù)運(yùn)動和結(jié)構(gòu)。與StM中不同的是，SLAM中的圖像有時(shí)間上的先后順序，而StM中允許使用完全無關(guān)的圖像。

我們知道每個(gè)方程都受噪聲影響，所以要把這里的位姿x和路標(biāo)y看成服從某種概率分布的隨機(jī)變量，而不是單獨(dú)的一個(gè)數(shù)。所以當(dāng)存在一些運(yùn)動數(shù)據(jù)u和觀測數(shù)據(jù)z時(shí)，如何去估計(jì)狀態(tài)量的高斯分布？

當(dāng)只有位姿數(shù)據(jù)時(shí)，誤差累積會越來越大，不確定性增高，而如果加上路標(biāo)則會將不確定性減小。如下圖所示。

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

我們希望用過去0到k中的數(shù)據(jù)來估計(jì)現(xiàn)在的狀態(tài)分布：

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

其中xk為第k時(shí)刻的未知量。上式表示根據(jù)初始位置、1到k的運(yùn)動參數(shù)、1到k的圖像來估計(jì)第k時(shí)刻的未知量。

由x_k和z_k的相互依賴關(guān)系，可以得到如下近似關(guān)系：

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結(jié)果是一樣的概率上來說。

上式第一項(xiàng)為似然，第二項(xiàng)為先驗(yàn)。似然由觀測方程給定，而先驗(yàn)部分我們明白當(dāng)前狀態(tài)x_k是基于過去所有的狀態(tài)估計(jì)得來的。至少它會受x_k-1影響，于是按照x_k-1時(shí)刻為條件概率展開：

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.

如果考慮更久之前的狀態(tài)，也可以繼續(xù)對此式進(jìn)行展開，但現(xiàn)在我們只關(guān)心k時(shí)刻和k-1時(shí)刻的情況。至此，我們給出了貝葉斯估計(jì)，雖然上式還沒有具體的概率分布形式，所以還沒法實(shí)際地操作它。對這一步的后續(xù)處理，方法上產(chǎn)生了一些分歧。大體上講，存在若干種選擇：其一是假設(shè)馬爾可夫性，簡單的一階馬氏性認(rèn)為，k時(shí)刻狀態(tài)只與k-1時(shí)刻狀態(tài)有關(guān)，而與再之前的無關(guān)。如果做出這樣的假設(shè)，我們就會得到以擴(kuò)展卡爾曼濾波（EKF）為代表的濾波器方法。時(shí)刻注意這種方法與里程計(jì)的區(qū)別，在于通過概率統(tǒng)計(jì)的方法來消除誤差累積導(dǎo)致的軌跡偏移。在濾波方法中，我們會從某時(shí)刻的狀態(tài)估計(jì)，推導(dǎo)到下一個(gè)時(shí)刻。另外一種方法是依然考慮k時(shí)刻狀態(tài)與之前所有狀態(tài)的關(guān)系，此時(shí)將得到非線性優(yōu)化為主體的優(yōu)化框架。這也能消除誤差累積的影響。非線性優(yōu)化的基本知識已在前文介紹過。目前視覺SLAM的主流為非線性優(yōu)化方法。不過，為了讓本書更全面，我們要先介紹一下卡爾曼濾波器和EKF的原理。

卡爾曼濾波器的推導(dǎo)方法有若干種，比如從概率角度出發(fā)的最大后驗(yàn)概率估計(jì)的形式（貝葉斯），從最小均方差為目的推導(dǎo)的遞推等式（鏈接）。總之，卡爾曼濾波就是推導(dǎo)出先驗(yàn)和后驗(yàn)之間的關(guān)系式，也就是增益K。通過預(yù)測（先驗(yàn)）和測量反饋（后驗(yàn)）來提高準(zhǔn)確度。

但是EKF存在一些缺點(diǎn)，所以現(xiàn)在在SLAM中一般采用BA與圖優(yōu)化，這個(gè)之前提到過，這里詳細(xì)闡述它的原理。

?

第11講 后端2

雖然BA有各種優(yōu)點(diǎn)，但是速度太慢，尤其是隨著特征點(diǎn)越來越多它的效率會越來越慢。所以引出了位姿圖。即將特征點(diǎn)視為輸入值，只考慮位姿的優(yōu)化。

?

第12講 回環(huán)檢測

回環(huán)檢測的優(yōu)點(diǎn)，可以充分利用每一個(gè)環(huán)來調(diào)整地圖。

如上圖所示第一個(gè)圖表示真實(shí)的軌跡，第二個(gè)圖紅色表示位姿的偏移，而如果有回環(huán)檢測的話則可以將后面偏離的軌跡拉回到重合處。這就是回環(huán)檢測的好處。

回環(huán)檢測如果真的檢測到真實(shí)的回環(huán)則有很大的優(yōu)化作用，但錯誤的檢測則會造成很大的損失，還不如不檢測，所以這里對準(zhǔn)確率要求較高，而對召回率要求小一些（召回率即有些真實(shí)的回環(huán)沒有檢測到的概率），也就是說對于模棱兩可不確定的回環(huán)，寧可不回環(huán)也不要冒險(xiǎn)認(rèn)定為回環(huán)。

那么回環(huán)檢測用什么模型呢？這里用詞袋模型，是特征點(diǎn)法的聚類模型。

聚類問題在無監(jiān)督機(jī)器學(xué)習(xí)（Unsupervised ML）中特別常見，用于讓機(jī)器自行尋找數(shù)據(jù)中的規(guī)律，無人工干預(yù)。詞袋的字典生成亦屬于其中之一。先對大量的圖像提取了特征點(diǎn)，比如說有N個(gè)。現(xiàn)在，我們想找一個(gè)有k個(gè)單詞的字典，每個(gè)單詞是由特征點(diǎn)組成的，那么特征點(diǎn)該如何組合成有效的單詞呢？我們?nèi)艘话銜詣泳酆蠟椤皶薄ⅰ氨印薄ⅰ白雷印边@樣的單詞。每個(gè)單詞可以看作局部相鄰特征點(diǎn)的集合，應(yīng)該怎么做呢？這可以用經(jīng)典的K-means（K均值）算法解決。

?

第13講 建圖

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

?

轉(zhuǎn)載于:https://www.cnblogs.com/2008nmj/p/6344828.html

總結(jié)

以上是生活随笔為你收集整理的高翔《视觉SLAM十四讲》从理论到实践的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。