?

這篇文章即是Felix Endres等人12年完成的RGB-D SLAM V2，是最早的為kinect風(fēng)格傳感器設(shè)計(jì)的SLAM系統(tǒng)之一

沒(méi)看到有相關(guān)的論文解析

在Github上可找到開(kāi)源代碼，工程配置與運(yùn)行參考http://www.cnblogs.com/voyagee/p/6898278.html

?

系統(tǒng)流程

?

?

?

??????? 系統(tǒng)分為前后端。前端就是視覺(jué)里程記。從每一幀的RGB圖像提取特征，計(jì)算描述符，RANSAC+ICP計(jì)算兩幀之間的motion estimation，

　　并提出了一個(gè)EMM（Environment Measurement Model）模型來(lái)判斷estimate motion是否可接受。后端回環(huán)檢測(cè)，基于g2o優(yōu)化庫(kù)的位姿圖（pose graph）優(yōu)化，

????? 得到優(yōu)化后的軌跡，用于建立地?? 圖。建圖采用八叉樹(shù)地圖octomap的方式。

?

特征提取

在源碼中可選擇 SIFT \ SURF \ORB 特征，其中SIFT要在GPU上運(yùn)行，ORB和SURF都在CPU上用Opencv實(shí)現(xiàn)

不同特征比較：

（?其中，ATE為Absolute Trajectory Error，軌跡的誤差，就是系統(tǒng)估計(jì)的軌跡和真實(shí)軌跡（GroundTruth）之間的歐式距離，RMSE就是均方根）

可以看出，有GPU時(shí)SIFT綜合表現(xiàn)最好。 綜合實(shí)時(shí)性、硬件成本和準(zhǔn)確率來(lái)看，ORB較好。

?

運(yùn)動(dòng)估計(jì)

三對(duì)特征快速計(jì)算RANSAC的初始值，在每一輪迭代中，最小二乘correspondences之間的馬氏距離 。

馬氏距離與歐式距離的差別：http://www.cnblogs.com/likai198981/p/3167928.html，簡(jiǎn)單說(shuō)就是計(jì)算距離時(shí)考慮到了各項(xiàng)異性，多乘了一個(gè)協(xié)方差矩陣

?

EMM：Environment Measurement Model

?一個(gè)傳統(tǒng)的判斷motion estimate是否可接受的方法就是看inlier的比例，小于設(shè)定閾值就reject motion estimate。

然而，motion blur(運(yùn)動(dòng)模糊)，缺少紋理信息的環(huán)境都很容易出現(xiàn)inlier較少的情況。

并且有一些點(diǎn)，在一幀中可以看到，另一幀可能就被其他點(diǎn)擋住了。? 作者提出使用這個(gè)EMM來(lái)更魯棒的判斷是否reject estimate?

?

先看一個(gè)假設(shè)：實(shí)施transformation之后，空間上對(duì)應(yīng)的深度測(cè)量值應(yīng)該來(lái)自于同一個(gè)表面位置之下:

after applying transformation estimate,spatially corresponding depth measurement stem from the same underlying surface location.

?

?論文中作者證明了觀測(cè)yi和yj（不同與下圖的yi,yj）之間的差滿足高斯分布，方差為表示噪音的協(xié)方差矩陣（計(jì)算方法由論文 Accuracy and Resolution of Kinect Depth Data給出）

這樣，就可以用概率學(xué)里的p值檢驗(yàn)來(lái)判斷是否reject estimate，然而，發(fā)現(xiàn)p值檢驗(yàn) 有點(diǎn)神經(jīng)衰弱 ，對(duì)于微小的誤差太過(guò)敏感，

因此使用另一種方法

?

將相機(jī)A觀測(cè)到的點(diǎn)投影到相機(jī)B，在observed points中找出inlier,outlier,occluded：

?上圖中yi和yj應(yīng)該是同一個(gè)點(diǎn)，算作inlier。yk不再A的視場(chǎng)內(nèi)，所以被忽視，不看作“observed points”。

投影之后的yq在相機(jī)B 的市場(chǎng)中被yk擋住，看不到，因此算作occluded。

至于yp,落在了yq和相機(jī)A光心的連線上，算作outlier（注意，yp與yk不同，yp在相機(jī)A的視場(chǎng)內(nèi)，但是相機(jī)A在這里觀測(cè)到的是yq的深度）

因此，在上圖中，inlier有倆，outlier一個(gè)，occluded一個(gè)

?

?而在代碼中（misc.cpp 913行之后有兩個(gè)函數(shù)，其中一個(gè)是用于p值檢驗(yàn)方法的），作者是這么計(jì)算inlier,outlier,occluded，并判斷是否reject的

?

?

?

?

?

?

?

?

?

?

?

?

?

?

?

?

observedPoints=inliers + outliers + occluded

I=inlier數(shù)量，O=outlier數(shù)量，C=occluded數(shù)量，如果I/（I+O+C）<25%,直接reject

否則，對(duì)I/（I+O）設(shè)一個(gè)閾值，小于閾值就reject

?? 回環(huán)檢測(cè)

　　現(xiàn)在的回環(huán)檢測(cè)大多數(shù)是基于Bag-of-Words，深度學(xué)習(xí)興起后也有用deep learning 的，或者用語(yǔ)義信息

??? 此文回環(huán)是基于 最小生成樹(shù) ，隨機(jī)森林的隨機(jī)回環(huán)

??? 使用描述符之間的距離（直接相減），生成一棵深度圖像（limited depth）的最小生成樹(shù)，去掉n個(gè)時(shí)間上最近的（避免重復(fù)）

??? 再隨機(jī)從樹(shù)上選k幀(偏向于更早的幀)來(lái)找閉環(huán)

?

?

圖優(yōu)化

只優(yōu)化位姿圖，不優(yōu)化三維點(diǎn)。

使用g2o圖優(yōu)化庫(kù)。g2o的使用http://www.cnblogs.com/gaoxiang12/p/3776107.html

沒(méi)有被EMM拒絕的motion，將兩幀相機(jī)位姿作為優(yōu)化頂點(diǎn)，motion作為約束，加入優(yōu)化邊

檢測(cè)到的回環(huán)，也加入優(yōu)化頂點(diǎn)和邊

g2o優(yōu)化邊的誤差函數(shù)：

?

???

????? 這里的xi,xj為優(yōu)化變量（頂點(diǎn)位姿的estimate），Zij是約束，也就是xi和xj之間的變換。e（）是how well xi and xj satisfy the constraint ，

???? 中間的Ω是約束（優(yōu)化邊）的信息矩陣（協(xié)方差矩陣的逆）， 表示對(duì)邊的精度的估計(jì)。在代碼中，是這么計(jì)算的：

?

odom_edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity() / time_delta_sec;//e-9;

?

??? 單位矩陣除以兩幀之間的時(shí)間間隔

?

????

???? 看到這里，依然沒(méi)有明白這個(gè)誤差究竟咋算的～

????? 查看g2o::SE3edge.cpp, 這個(gè)e其實(shí)是這么算的：

????

　　

?

_inverseMeasurement * from->estimate().inverse() * to->estimate()

?

解釋一下這個(gè)誤差函數(shù)：

我們認(rèn)為 幀2（ from）的位姿態(tài) Tj 是幀1（to）的位姿 Ti 經(jīng)過(guò) Tij 得到的 ，也就是

Ti * Tij = Tj ??

? 也就是

Tij = Ti ^-1 *?? Tj

即一個(gè)位姿的逆乘另一個(gè)位姿可以表示兩個(gè)位姿之間的變化，那么，我們想表達(dá)邊的measurement與Tij之間的差距，也可以這么玩兒

?

delta ： ? ? ?? Δ = measurement^-1? * Ti^-1 * Tj? ,?

?

這就得到g2o代碼里的式子了， 后面的toVectorMQT是啥我也不清楚。。應(yīng)該就是矩陣轉(zhuǎn)化為向量

?

注意，Ti其實(shí)完整寫法是Twi,? w代表世界坐標(biāo)系

這里看不懂的可以參考高翔大大《視覺(jué)SLAM十四講》11.1.2

?

這個(gè)樣子的回環(huán)檢測(cè)，難免會(huì)出現(xiàn)錯(cuò)誤的回環(huán)，這種回環(huán)約束加入到圖優(yōu)化中去，勢(shì)必會(huì)把地圖拉的扭曲

所以，在圖優(yōu)化第一次收斂之后，作者就把錯(cuò)誤回環(huán)對(duì)應(yīng)的優(yōu)化邊? 從圖中刪除。

怎么判斷回環(huán)是錯(cuò)誤的呢？ -----? 給上面說(shuō)的誤差函數(shù)設(shè)一個(gè)閾值（又又又又是閾值。。） ，誤差大于這個(gè)閾值的就是錯(cuò)誤的

?

octoMap:

http://www.cnblogs.com/gaoxiang12/p/5041142.html

把一個(gè)立方體平均分成八個(gè)小立方體，小立方體又可以再分，子子孫孫無(wú)窮盡也

每個(gè)小立方體對(duì)應(yīng)八叉樹(shù)的一個(gè)節(jié)點(diǎn)，因此成為八叉樹(shù)地圖

每個(gè)小立方體存儲(chǔ) 被占據(jù)的概率的logist回歸，被觀測(cè)到占據(jù)次數(shù)越多，概率越大，而如果沒(méi)有一次觀測(cè)到被占據(jù)，則不用展開(kāi)該節(jié)點(diǎn)

?

OctoMap的優(yōu)點(diǎn)： 利于導(dǎo)航 ; 易于更新 ;存儲(chǔ)方式比較省空間

?

?

歡迎交流，歡迎補(bǔ)充，歡迎糾錯(cuò)

轉(zhuǎn)載于:https://www.cnblogs.com/voyagee/p/7027076.html

總結(jié)

以上是生活随笔為你收集整理的3D Mapping with an RGB-D Camera（RGBD SLAM V2 ）论文笔记的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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