最近在做基于激光信息的機(jī)器人行人跟蹤發(fā)現(xiàn)如果單獨(dú)利用激光信息很難完成機(jī)器人對(duì)行人的識(shí)別、跟蹤等功能，因此考慮與視覺(jué)融合的方法，這樣便可以充分利用激光雷達(dá)提供的精確位置信息及視覺(jué)提供的豐富紋理、顏色等場(chǎng)景信息。以下是最近調(diào)研視覺(jué)SLAM中的實(shí)現(xiàn)方法的總結(jié)，包括三方面內(nèi)容：姿態(tài)計(jì)算、閉環(huán)檢測(cè)、BA優(yōu)化。

姿態(tài)計(jì)算

一、通過(guò)提取圖像的特征描述子，如ORB、SURF和SIFT等特征描述子，然后通過(guò)RANSAC算法進(jìn)行圖像匹配去除匹配點(diǎn)中的外點(diǎn)，再通過(guò)將二維點(diǎn)對(duì)映射到三維之后，便可以利用PnP或ICP算法計(jì)算相機(jī)位姿。基于特征提取的位姿計(jì)算算法對(duì)場(chǎng)景有一定的要求，在無(wú)紋理場(chǎng)景會(huì)出現(xiàn)位姿計(jì)算失敗的情形。

二、直接圖像匹配方法：直接圖像匹配并不對(duì)圖片進(jìn)行特征提取，核心思想是在旋轉(zhuǎn)坐標(biāo)系下，基于相機(jī)一致性的假設(shè)，在相機(jī)的剛體變換已知的情況下，利用相機(jī)變換矩陣將目標(biāo)圖片投影到當(dāng)前圖片上，其像素之間的差異應(yīng)該最小，將姿態(tài)計(jì)算轉(zhuǎn)換為加權(quán)最小二乘問(wèn)題。直接圖像匹配算法的計(jì)算效率很高，不依賴GPU，具有很高的理論和商用價(jià)值。

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

閉環(huán)檢測(cè)算法指的是通過(guò)檢測(cè)算法檢測(cè)出之前訪問(wèn)過(guò)的場(chǎng)景。如圖1所示，當(dāng)機(jī)器人在移動(dòng)過(guò)程中，特別是在探索大面積的場(chǎng)景時(shí)，由于模型的不確定性以及設(shè)備的噪聲，不確定性會(huì)逐漸增長(zhǎng)。通過(guò)引入閉環(huán)檢測(cè)技術(shù)，識(shí)別出歷史訪問(wèn)過(guò)的場(chǎng)景以增加位姿之間的約束，可以很好的減少這種不確定性。

閉環(huán)檢測(cè)前后對(duì)比(a) 累計(jì)誤差導(dǎo)致的構(gòu)圖錯(cuò)誤，(b)閉環(huán)約束后的地圖

閉環(huán)檢測(cè)方法有：

一、最簡(jiǎn)單的閉環(huán)檢測(cè)算法是將新檢測(cè)出來(lái)的關(guān)鍵幀和過(guò)去所有的關(guān)鍵幀一一進(jìn)行比較，雖然這種方法能比較好的檢測(cè)出當(dāng)前場(chǎng)景是否在之前出現(xiàn)過(guò)，但是在大規(guī)模場(chǎng)景下，機(jī)器人往往有成千上萬(wàn)個(gè)關(guān)鍵幀，這種方法檢測(cè)效率及其底下，不能再實(shí)際場(chǎng)景中使用。

二、通過(guò)將圖像中的特征和整個(gè)地圖中的路標(biāo)對(duì)應(yīng)起來(lái)，然后建立一個(gè)所有路標(biāo)的數(shù)據(jù)庫(kù)，通過(guò)使用路標(biāo)分類器來(lái)加快場(chǎng)景識(shí)別速度。比如講所有路標(biāo)構(gòu)建成一棵KD-Tree，當(dāng)新添加一幀關(guān)鍵幀時(shí)，將關(guān)鍵幀在KD-Tree中進(jìn)行檢索，從中檢索之前出現(xiàn)過(guò)的場(chǎng)景。

三、基于視覺(jué)詞袋的閉環(huán)檢測(cè)方法，這種方法通過(guò)將特征描述子抽象成詞匯，通過(guò)TF-IDF方法識(shí)別出現(xiàn)過(guò)的場(chǎng)景。如圖2所示，是閉環(huán)檢測(cè)中基于詞袋模型流程圖。使用視覺(jué)詞袋的方法效率很高，可用在大規(guī)模地圖的創(chuàng)建上。

BA優(yōu)化

一、問(wèn)題闡述：同時(shí)對(duì)三維點(diǎn)位置和相機(jī)參數(shù)進(jìn)行非線性優(yōu)化。

BA原理

二、LM法的原理與優(yōu)勢(shì)：

原理：是一種“信賴域”的方法，當(dāng)收斂速度較快時(shí)，增大信賴域使算法趨向于高斯牛頓法；當(dāng)收斂速度較慢時(shí)，減小信賴域使算法趨向于最速下降法。優(yōu)勢(shì)：速度快；可以在距離初始值較遠(yuǎn)處得到最優(yōu)解。

SLAM優(yōu)化算法對(duì)比