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

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

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

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

閉環(huán)檢測

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

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

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

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

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

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

BA優(yōu)化

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

BA原理

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

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

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