SLAM的歷史、理論以及實(shí)現(xiàn)的方式

到目前為止，室內(nèi)的視覺(jué)SLAM仍處于研究階段，遠(yuǎn)未到實(shí)際應(yīng)用的程度。一方面，編寫(xiě)和使用視覺(jué)SLAM需要大量的專(zhuān)業(yè)知識(shí)，算法的實(shí)時(shí)性未達(dá)到實(shí)用要求；另一方面，視覺(jué)SLAM生成的地圖（多數(shù)是點(diǎn)云）還不能用來(lái)做機(jī)器人的路徑規(guī)劃，需要科研人員進(jìn)一步的探索和研究。以下，我會(huì)介紹SLAM的歷史、理論以及實(shí)現(xiàn)的方式，且主要介紹視覺(jué)（Kinect）的實(shí)現(xiàn)方式。

??

1.前言

開(kāi)始做SLAM（機(jī)器人同時(shí)定位與建圖）研究已經(jīng)近一年了。從一年級(jí)開(kāi)始對(duì)這個(gè)方向產(chǎn)生興趣，到現(xiàn)在為止，也算是對(duì)這個(gè)領(lǐng)域有了大致的了解。然而越了解，越覺(jué)得這個(gè)方向難度很大?？傮w來(lái)講有以下幾個(gè)原因：

入門(mén)資料很少。雖然國(guó)內(nèi)也有不少人在做，但這方面現(xiàn)在沒(méi)有太好的入門(mén)教程。《SLAM for dummies》可以算是一篇。中文資料幾乎沒(méi)有。

SLAM研究已進(jìn)行了三十多年，從上世紀(jì)的九十年代開(kāi)始。其中又有若干歷史分枝和爭(zhēng)論，要把握它的走向就很費(fèi)工夫。

難以實(shí)現(xiàn)。SLAM是一個(gè)完整的系統(tǒng)，由許多個(gè)分支模塊組成?，F(xiàn)在經(jīng)典的方案是“圖像前端，優(yōu)化后端，閉環(huán)檢測(cè)”的三部曲，很多文獻(xiàn)看完了自己實(shí)現(xiàn)不出來(lái)。

自己動(dòng)手編程需要學(xué)習(xí)大量的先決知識(shí)。首先你要會(huì)C和C++，網(wǎng)上很多代碼還用了11標(biāo)準(zhǔn)的C++。第二要會(huì)用Linux。第三要會(huì)cmake，vim/emacs及一些編程工具。第四要會(huì)用openCV, PCL, Eigen等第三方庫(kù)。只有學(xué)會(huì)了這些東西之后，你才能真正上手編一個(gè)SLAM系統(tǒng)。如果你要跑實(shí)際機(jī)器人，還要會(huì)ROS。

當(dāng)然，困難多意味著收獲也多，坎坷的道路才能鍛煉人（比如說(shuō)走著走著才發(fā)現(xiàn)Linux和C++才是我的真愛(ài)之類(lèi)的。）鑒于目前網(wǎng)上關(guān)于視覺(jué)SLAM的資料極少，我于是想把自己這一年多的經(jīng)驗(yàn)與大家分享一下。說(shuō)的不對(duì)的地方請(qǐng)大家批評(píng)指正。

這篇文章關(guān)注視覺(jué)SLAM，專(zhuān)指用攝像機(jī)，Kinect等深度像機(jī)來(lái)做導(dǎo)航和探索，且主要關(guān)心室內(nèi)部分。到目前為止，室內(nèi)的視覺(jué)SLAM仍處于研究階段，遠(yuǎn)未到實(shí)際應(yīng)用的程度。一方面，編寫(xiě)和使用視覺(jué)SLAM需要大量的專(zhuān)業(yè)知識(shí)，算法的實(shí)時(shí)性未達(dá)到實(shí)用要求；另一方面，視覺(jué)SLAM生成的地圖（多數(shù)是點(diǎn)云）還不能用來(lái)做機(jī)器人的路徑規(guī)劃，需要科研人員進(jìn)一步的探索和研究。以下，我會(huì)介紹SLAM的歷史、理論以及實(shí)現(xiàn)的方式，且主要介紹視覺(jué)（Kinect）的實(shí)現(xiàn)方式。

2.SLAM問(wèn)題

SLAM，全稱(chēng)叫做Simultaneous Localization and Mapping，中文叫做同時(shí)定位與建圖。啊不行，這么講下去，這篇文章肯定沒(méi)有人讀，所以我們換一個(gè)講法。

3.小蘿卜的故事

從前，有一個(gè)機(jī)器人叫“小蘿卜”。它長(zhǎng)著一雙烏黑發(fā)亮的大眼睛，叫做Kinect。有一天，它被邪惡的科學(xué)家關(guān)進(jìn)了一間空屋子，里面放滿(mǎn)了雜七雜八的東西。

小蘿卜感到很害怕，因?yàn)檫@個(gè)地方他從來(lái)沒(méi)來(lái)過(guò)，一點(diǎn)兒也不了解。讓他感到害怕的主要是三個(gè)問(wèn)題：

1.自己在哪里？

2.這是什么地方？

3.怎么離開(kāi)這個(gè)地方？

在SLAM理論中，第一個(gè)問(wèn)題稱(chēng)為定位(Localization)，第二個(gè)稱(chēng)為建圖(Mapping)，第三個(gè)則是隨后的路徑規(guī)劃。我們希望借助Kinect工具，幫小蘿卜解決這個(gè)難題。各位同學(xué)有什么思路呢？

4.Kinect數(shù)據(jù)

要打敗敵人，首先要了解你的武器。不錯(cuò)，我們先介紹一下Kinect。眾所周知這是一款深度相機(jī)，你或許還聽(tīng)說(shuō)過(guò)別的牌子，但Kinect的價(jià)格便宜，測(cè)量范圍在3m-12m之間，精度約3cm，較適合于小蘿卜這樣的室內(nèi)機(jī)器人。它采到的圖像是這個(gè)樣子的（從左往右依次為rgb圖，深度圖與點(diǎn)云圖）：

Kinect的一大優(yōu)勢(shì)在于能比較廉價(jià)地獲得每個(gè)像素的深度值，不管是從時(shí)間上還是從經(jīng)濟(jì)上來(lái)說(shuō)。OK，有了這些信息，小蘿卜事實(shí)上可以知道它采集到的圖片中，每一個(gè)點(diǎn)的3d位置。只要我們事先標(biāo)定了Kinect，或者采用出廠的標(biāo)定值。

我們把坐標(biāo)系設(shè)成這個(gè)樣子，這也是openCV中采用的默認(rèn)坐標(biāo)系。

o’-uv是圖片坐標(biāo)系，o-xyz是Kinect的坐標(biāo)系。假設(shè)圖片中的點(diǎn)為(u,v)，對(duì)應(yīng)的三維點(diǎn)位置在(x,y,z)，那么它們之間的轉(zhuǎn)換關(guān)系是這樣的：

左側(cè)的s為尺度因子，表示從相機(jī)光心出去的射線(xiàn)都會(huì)落在成像平面的同一個(gè)點(diǎn)上。如果我們不知道該點(diǎn)的距離，那么s就是一個(gè)自由變量。但在RGB-D相機(jī)中，我們?cè)贒epth圖中知道了這個(gè)距離，它的讀數(shù)dep(u,v)與真實(shí)距離相差一個(gè)倍數(shù)。如果也記作s，那么：

后一個(gè)公式給出了計(jì)算三維點(diǎn)的方法。先從深度圖中讀取深度數(shù)據(jù)（Kinect給的是16位無(wú)符號(hào)整數(shù)），除掉z方向的縮放因子，這樣你就把一個(gè)整數(shù)變到了以米為單位的數(shù)據(jù)。然后，x,y用上面的公式算出。一點(diǎn)都不難，就是一個(gè)中心點(diǎn)位置和一個(gè)焦距而已。f代表焦距，c代表中心。如果你沒(méi)有自己標(biāo)定你的Kinect，也可以采用默認(rèn)的值：s=5000, cx = 320, cy=240, fx=fy=525。實(shí)際值會(huì)有一點(diǎn)偏差，但不會(huì)太大。

5.定位問(wèn)題

知道了Kinect中每個(gè)點(diǎn)的位置后，接下來(lái)我們要做的，就是根據(jù)兩幀圖像間的差別計(jì)算小蘿卜的位移。比如下面兩張圖，后一張是在前一張之后1秒采集到的：

你肯定可以看出，小蘿卜往右轉(zhuǎn)過(guò)了一定的角度。但究竟轉(zhuǎn)過(guò)多少度呢？這就要靠計(jì)算機(jī)來(lái)求解了。這個(gè)問(wèn)題稱(chēng)為相機(jī)相對(duì)姿態(tài)估計(jì)，經(jīng)典的算法是ICP（Iterative Closest Point，迭代最近點(diǎn)）。這個(gè)算法要求知道這兩個(gè)圖像間的一組匹配點(diǎn)，說(shuō)的通俗點(diǎn)，就是左邊圖像哪些點(diǎn)和右邊是一樣的。你當(dāng)然看見(jiàn)那塊黑白相間的板子同時(shí)出現(xiàn)在兩張圖像中。在小蘿卜看來(lái)，這里牽涉到兩個(gè)簡(jiǎn)單的問(wèn)題：特征點(diǎn)的提取和匹配。

如果你熟悉計(jì)算機(jī)視覺(jué)，那你應(yīng)該聽(tīng)說(shuō)過(guò)SIFT, SURF之類(lèi)的特征。不錯(cuò)，要解決定位問(wèn)題，首先要得到兩張圖像的一個(gè)匹配。匹配的基礎(chǔ)是圖像的特征，下圖就是SIFT提取的關(guān)鍵點(diǎn)與匹配結(jié)果：

對(duì)實(shí)現(xiàn)代碼感興趣的同學(xué)請(qǐng)Google“opencv匹配”即可，在openCV的教程上也有很明白的例子。上面的例子可以看出，我們找到了一些匹配，但其中有些是對(duì)的（基本平等的匹配線(xiàn)），有些是錯(cuò)的。這是由于圖像中存在周期性出現(xiàn)的紋理（黑白塊），所以容易搞錯(cuò)。但這并不是問(wèn)題，在接下來(lái)的處理中我們會(huì)將這些影響消去。

得到了一組匹配點(diǎn)后，我們就可以計(jì)算兩個(gè)圖像間的轉(zhuǎn)換關(guān)系，也叫PnP問(wèn)題。它的模型是這樣的：

[uv]=C(Rp+t)(1)(1)[uv]=C(Rp+t)

R為相機(jī)的姿態(tài)，C為相機(jī)的標(biāo)定矩陣。R是不斷運(yùn)動(dòng)的，而C則是隨著相機(jī)做死的。ICP的模型稍有不同，但原理上也是計(jì)算相機(jī)的姿態(tài)矩陣。原則上，只要有四組匹配點(diǎn)，就可以算這個(gè)矩陣。你可以調(diào)用openCV的SolvePnPRANSAC函數(shù)或者PCL的ICP算法來(lái)求解。openCV提供的算法是RANSAC（Random Sample Consensus，隨機(jī)采樣一致性）架構(gòu)，可以剔除錯(cuò)誤匹配。所以代碼實(shí)際運(yùn)行時(shí)，可以很好地找到匹配點(diǎn)。

有同學(xué)會(huì)說(shuō)，那只要不斷匹配下去，定位問(wèn)題不就解決了嗎？表面上看來(lái)，的確是這樣的，只要我們引入一個(gè)關(guān)鍵幀的結(jié)構(gòu)（發(fā)現(xiàn)位移超過(guò)一個(gè)固定值時(shí)，定義成一個(gè)關(guān)鍵幀）。然后，把新的圖像與關(guān)鍵幀比較就行了。至于建圖，就是把這些關(guān)鍵幀的點(diǎn)云拼起來(lái)，看著還有模有樣，煞有介事的：

1－200幀的匹配結(jié)果

然而，如果事情真這么簡(jiǎn)單，SLAM理論就不用那么多人研究三十多年了（它是從上世紀(jì)90年代開(kāi)始研究的）（上面講的那些東西簡(jiǎn)直隨便哪里找個(gè)小碩士就能做出來(lái)……）。那么，問(wèn)題難在什么地方呢？

6.SLAM端優(yōu)化理論

最麻煩的問(wèn)題，就是“噪聲”。這種漸近式的匹配方式，和那些慣性測(cè)量設(shè)備一樣，存在著累積噪聲。因?yàn)槲覀冊(cè)诓粩嗟馗玛P(guān)鍵幀，把新圖像與最近的關(guān)鍵幀比較，從而獲得機(jī)器人的位移信息。但是你要想到，如果有一個(gè)關(guān)鍵幀出現(xiàn)了偏移，那么剩下的位移估計(jì)都會(huì)多出一個(gè)誤差。這個(gè)誤差還會(huì)累積，因?yàn)楹竺娴墓烙?jì)都基于前面的機(jī)器人位置……哇！這后果簡(jiǎn)直不堪設(shè)想?。ɡ?，你的機(jī)器人往右轉(zhuǎn)了30度，再往左轉(zhuǎn)了30度回到原來(lái)的位置。然而由于誤差，你算成了向右轉(zhuǎn)29度，再向左轉(zhuǎn)31度，這樣你構(gòu)建的地圖中，會(huì)出現(xiàn)初始位置的兩個(gè)“重影”）。我們能不能想辦法消除這個(gè)該死的誤差呢？

朋友們，這才是SLAM的研究，前面的可以說(shuō)是“圖像前端”的處理方法。我們的解決思路是：如果你和最近的關(guān)鍵幀相比，會(huì)導(dǎo)致累計(jì)誤差。那么，我們最好是和更前面的關(guān)鍵幀相比，而且多比較幾個(gè)幀，不要只比較一次。

我們用數(shù)學(xué)來(lái)描述這個(gè)問(wèn)題。設(shè)：

不要怕，只有借助數(shù)學(xué)才能把這個(gè)問(wèn)題講清楚。上面的公式中，xp是機(jī)器人小蘿卜的位置，我們假定由n個(gè)幀組成。xL則是路標(biāo)，在我們的圖像處理過(guò)程中就是指SIFT提出來(lái)的關(guān)鍵點(diǎn)。如果你做2D SLAM，那么機(jī)器人位置就是x, y加一個(gè)轉(zhuǎn)角theta。如果是3D SLAM，就是x,y,z加一個(gè)四元數(shù)姿態(tài)（或者rpy姿態(tài)）。這個(gè)過(guò)程叫做參數(shù)化（Parameterization）。

不管你用哪種參數(shù)，后面兩個(gè)方程你都需要知道。前一個(gè)叫運(yùn)動(dòng)方程，描述機(jī)器人怎樣運(yùn)動(dòng)。u是機(jī)器人的輸入，w是噪聲。這個(gè)方程最簡(jiǎn)單的形式，就是你能通過(guò)什么方式（碼盤(pán)等）獲得兩幀間的位移差，那么這個(gè)方程就直接是上一幀與u相加即得。另外，你也可以完全不用慣性測(cè)量設(shè)備，這樣我們就只依靠圖像設(shè)備來(lái)估計(jì)，這也是可以的。

后一個(gè)方程叫觀測(cè)方程，描述那些路標(biāo)是怎么來(lái)的。你在第i幀看到了第j個(gè)路標(biāo)，產(chǎn)生了一個(gè)測(cè)量值，就是圖像中的橫縱坐標(biāo)。最后一項(xiàng)是噪聲。偷偷告訴你，這個(gè)方程形式上和上一頁(yè)的那個(gè)方程是一模一樣的。

在求解SLAM問(wèn)題前，我們要看到，我們擁有的數(shù)據(jù)是什么？在上面的模型里，我們知道的是運(yùn)動(dòng)信息u以及觀測(cè)z。用示意圖表示出來(lái)是這樣的：

我們要求解的，就是根據(jù)這些u和z，確定所有的xp和xL。這就是SLAM問(wèn)題的理論。從SLAM誕生開(kāi)始科學(xué)家們就一直在解決這個(gè)問(wèn)題。最初，我們用Kalman濾波器，所以上面的模型（運(yùn)動(dòng)方程和觀測(cè)方程）被建成這個(gè)樣子。直到21世紀(jì)初，卡爾曼濾波器仍在SLAM系統(tǒng)占據(jù)最主要的地位，Davison經(jīng)典的單目SLAM就是用EKF做的。但是后來(lái)，出現(xiàn)了基于圖優(yōu)化的SLAM方法，漸漸有取而代之的地位[1]。我們?cè)谶@里不介紹卡爾曼濾波器，有興趣的同學(xué)可以在wiki上找卡爾曼濾波器，另有一篇中文的《卡爾曼濾波器介紹》也很棒。由于濾波器方法存儲(chǔ)n個(gè)路標(biāo)要消耗n平方的空間，在計(jì)算量上有點(diǎn)對(duì)不住大家。盡管08年有人提出分治法的濾波器能把復(fù)雜度弄到O(n) [2]，但實(shí)現(xiàn)手段比較復(fù)雜。我們要介紹那種新興的方法:Graph-based SLAM。

圖優(yōu)化方法把SLAM問(wèn)題做成了一個(gè)優(yōu)化問(wèn)題。學(xué)過(guò)運(yùn)籌學(xué)的同學(xué)應(yīng)該明白，優(yōu)化問(wèn)題對(duì)我們有多么重要。我們不是要求解機(jī)器人的位置和路標(biāo)位置嗎？我們可以先做一個(gè)猜測(cè)，猜想它們大概在什么地方。這其實(shí)是不難的。然后呢，將猜測(cè)值與運(yùn)動(dòng)模型／觀測(cè)模型給出的值相比較，可以算出誤差：

通俗一點(diǎn)地講，例如，我猜機(jī)器人第一幀在(0,0,0)，第二幀在(0,0,1)。但是u1告訴我機(jī)器人往z方向（前方）走了0.9米，那么運(yùn)動(dòng)方程就出現(xiàn)了0.1m的誤差。同時(shí)，第一幀中機(jī)器人發(fā)現(xiàn)了路標(biāo)1，它在該機(jī)器人圖像的正中間；第二幀卻發(fā)現(xiàn)它在中間偏右的位置。這時(shí)我們猜測(cè)機(jī)器人只是往前走，也是存在誤差的。至于這個(gè)誤差是多少，可以根據(jù)觀測(cè)方程算出來(lái)。

我們得到了一堆誤差，把這些誤差平方后加起來(lái)（因?yàn)閱渭兊恼`差有正有負(fù)，然而平方誤差可以改成其他的范數(shù)，只是平方更常用），就得到了平方誤差和。我們把這個(gè)和記作phi，就是我們優(yōu)化問(wèn)題的目標(biāo)函數(shù)。而優(yōu)化變量就是那些個(gè)xp, xL。

改變優(yōu)化變量，誤差平方和（目標(biāo)函數(shù)）就會(huì)相應(yīng)地變大或變小，我們可以用數(shù)值方法求它們的梯度和二階梯度矩陣，然后用梯度下降法求最優(yōu)值。這些東西學(xué)過(guò)優(yōu)化的同學(xué)都懂的。

注意到，一次機(jī)器人SLAM過(guò)程中，往往會(huì)有成千上萬(wàn)幀。而每一幀我們都有幾百個(gè)關(guān)鍵點(diǎn)，一乘就是幾百萬(wàn)個(gè)優(yōu)化變量。這個(gè)規(guī)模的優(yōu)化問(wèn)題放到小蘿卜的機(jī)載小破本上可解嗎？是的，過(guò)去的同學(xué)都以為，Graph-based SLAM是無(wú)法計(jì)算的。但就在21世紀(jì)06，07年后，有些同學(xué)發(fā)現(xiàn)了，這個(gè)問(wèn)題規(guī)模沒(méi)有想象的那么大。上面的J和H兩個(gè)矩陣是“稀疏矩陣”，于是呢，我們可以用稀疏代數(shù)的方法來(lái)解這個(gè)問(wèn)題?！跋∈琛钡脑?，在于每一個(gè)路標(biāo)，往往不可能出現(xiàn)在所有運(yùn)動(dòng)過(guò)程中，通常只出現(xiàn)在一小部分圖像里。正是這個(gè)稀疏性，使得優(yōu)化思路成為了現(xiàn)實(shí)。

優(yōu)化方法利用了所有可以用到的信息（稱(chēng)為full-SLAM, global SLAM），其精確度要比我們一開(kāi)始講的幀間匹配高很多。當(dāng)然計(jì)算量也要高一些。

由于優(yōu)化的稀疏性，人們喜歡用“圖”來(lái)表達(dá)這個(gè)問(wèn)題。所謂圖，就是由節(jié)點(diǎn)和邊組成的東西。我寫(xiě)成G={V,E}，大家就明白了。V是優(yōu)化變量節(jié)點(diǎn)，E表示運(yùn)動(dòng)/觀測(cè)方程的約束。什么，更糊涂了嗎？那我就上一張圖，來(lái)自[3]。

圖有點(diǎn)模糊，而且數(shù)學(xué)符號(hào)和我用的不太一樣，我用它來(lái)給大家一個(gè)圖優(yōu)化的直觀形象。上圖中，p是機(jī)器人位置，l是路標(biāo)，z是觀測(cè)，t是位移。其中呢，p, l是優(yōu)化變量，而z,t是優(yōu)化的約束?？雌饋?lái)是不是像一些彈簧連接了一些質(zhì)點(diǎn)呢？因?yàn)槊總€(gè)路標(biāo)不可能出現(xiàn)在每一幀中，所以這個(gè)圖是蠻稀疏的。不過(guò)，“圖”優(yōu)化只是優(yōu)化問(wèn)題的一個(gè)表達(dá)形式，并不影響優(yōu)化的含義。實(shí)際解起來(lái)時(shí)還是要用數(shù)值法找梯度的。這種思路在計(jì)算機(jī)視覺(jué)里，也叫做Bundle Adjustment。

不過(guò)，BA的實(shí)現(xiàn)方法太復(fù)雜，不太建議同學(xué)們拿C來(lái)寫(xiě)。好在2010年的ICRA上，其他的同學(xué)們提供了一個(gè)通用的開(kāi)發(fā)包：g2o [5]。它是有圖優(yōu)化通用求解器，很好用，我改天再詳細(xì)介紹這個(gè)軟件包?？傊覀冎灰延^測(cè)和運(yùn)動(dòng)信息丟到求解器里就行。這個(gè)優(yōu)化器會(huì)為我們求出機(jī)器人的軌跡和路標(biāo)位置。如下圖，紅點(diǎn)是路標(biāo)，藍(lán)色箭頭是機(jī)器人的位置和轉(zhuǎn)角（2D SLAM）。細(xì)心的同學(xué)會(huì)發(fā)現(xiàn)它往右偏轉(zhuǎn)了一些。：

7.閉環(huán)檢測(cè)

上面提到，僅用幀間匹配最大的問(wèn)題在于誤差累積，圖優(yōu)化的方法可以有效地減少累計(jì)誤差。然而，如果把所有測(cè)量都丟進(jìn)g2o，計(jì)算量還是有點(diǎn)兒大的。根據(jù)我自己測(cè)試，約10000多條邊，g2o跑起來(lái)就有些吃力了。這樣，就有同學(xué)說(shuō)，能把這個(gè)圖構(gòu)造地簡(jiǎn)潔一些嗎？我們用不著所有的信息，只需要把有用的拿出來(lái)就行了。

事實(shí)上，小蘿卜在探索房間時(shí)，經(jīng)常會(huì)左轉(zhuǎn)一下，右轉(zhuǎn)一下。如果在某個(gè)時(shí)刻他回到了以前去過(guò)的地方，我們就直接與那時(shí)候采集的關(guān)鍵幀做比較，可以嗎？我們說(shuō)，可以，而且那是最好的方法。這個(gè)問(wèn)題叫做閉環(huán)檢測(cè)。

閉環(huán)檢測(cè)是說(shuō)，新來(lái)一張圖像時(shí)，如何判斷它以前是否在圖像序列中出現(xiàn)過(guò)？有兩種思路：一是根據(jù)我們估計(jì)的機(jī)器人位置，看是否與以前某個(gè)位置鄰近；二是根據(jù)圖像的外觀，看它是否和以前關(guān)鍵幀相似。目前主流方法是后一種，因?yàn)楹芏嗫茖W(xué)家認(rèn)為前一種依靠有噪聲的位置來(lái)減少位置的噪聲，有點(diǎn)循環(huán)論證的意思。后一種方法呢，本質(zhì)上是個(gè)模式識(shí)別問(wèn)題（非監(jiān)督聚類(lèi)，分類(lèi)），常用的是Bag-of-Words (BOW)。但是BOW需要事先對(duì)字典進(jìn)行訓(xùn)練，因此SLAM研究者仍在探討有沒(méi)有更合適的方法。

在Kinect SLAM經(jīng)典大作中[6]，作者采用了比較簡(jiǎn)單的閉環(huán)方法：在前面n個(gè)關(guān)鍵幀中隨機(jī)采k個(gè)，與當(dāng)前幀兩兩匹配。匹配上后認(rèn)為出現(xiàn)閉環(huán)。這個(gè)真是相當(dāng)?shù)暮?jiǎn)單實(shí)用，效率也過(guò)得去。

高效的閉環(huán)檢測(cè)是SLAM精確求解的基礎(chǔ)。這方面還有很多工作可以做。