在我們所熟知經(jīng)典SLAM系統(tǒng)，以O(shè)RB-SLAM為代表的通過特征點(diǎn)法在相機(jī)位姿估計(jì)方面有很好的表現(xiàn)，但在一些人為構(gòu)造的弱紋理環(huán)境下，由于缺少可靠的特征點(diǎn)的緣故，導(dǎo)致表現(xiàn)性能下降。

針對此種問題，作者通過根據(jù)周圍環(huán)境的幾何特征來提升結(jié)構(gòu)化環(huán)境下的SLAM追蹤和建圖準(zhǔn)確性。除特征點(diǎn)以外，我們還可以利用結(jié)構(gòu)化環(huán)境提供的大量線和面的特征來進(jìn)行SLAM系統(tǒng)的追蹤與建圖。在SLAM的追蹤模塊中，作者對點(diǎn)、線、面做了解耦合的優(yōu)化處理，同時(shí)利用曼哈頓假設(shè)對位姿進(jìn)行優(yōu)化。對于建圖部分，從稀疏到稠密不同等級的地圖可以以較低的計(jì)算量進(jìn)行重建。作者使用實(shí)例化網(wǎng)格的策略，根據(jù)平面網(wǎng)格實(shí)例的獨(dú)立性來建立稠密地圖。

1.主要工作

·?基于曼哈頓假設(shè)原理，對位姿進(jìn)行解耦。通過聯(lián)合平面上的點(diǎn)、線特征以及附加的基于曼哈頓約束的位姿優(yōu)化過程來提升位姿估計(jì)的準(zhǔn)確性。

·?提出平面實(shí)例化網(wǎng)格方法，將稀疏點(diǎn)云中生成一個(gè)輕量化環(huán)境表征。

·?提出在結(jié)構(gòu)環(huán)境下具有高精度的實(shí)時(shí)RGB-D SLAM系統(tǒng)。

作者結(jié)合點(diǎn)、線、面這三者的特征來對提高系統(tǒng)在結(jié)構(gòu)化弱紋理場景下的穩(wěn)定性，點(diǎn)特征采用ORB特征點(diǎn)，特征點(diǎn)的提取通過LSD方法[A Fast Line Segment Detector with a False Detection Control]，由于RGB-D的緣故，我們可以直接得到稠密點(diǎn)云，通過 Efficient Organized Point Cloud Segmentation with Connected Components 這篇文章的思想來根據(jù)有序點(diǎn)云進(jìn)行平面分割，平面方程用以下公式進(jìn)行表示。

在點(diǎn)線提取環(huán)節(jié)，由于RGB-D相機(jī)得到的深度圖在物體邊界處會(huì)出現(xiàn)深度不連續(xù)的先天性弱勢，當(dāng)我們將特征根據(jù)深度值和相機(jī)內(nèi)參進(jìn)行投影至相機(jī)坐標(biāo)系下時(shí)，勢必會(huì)存在由于深度錯(cuò)誤導(dǎo)致投影的不準(zhǔn)確情況，為克服這種情況對系統(tǒng)的影響，作者采用啦一個(gè)小trick：對3D線進(jìn)行濾波，即通過RANSAC的方法對線段上的外點(diǎn)進(jìn)行剔除。

平面的法向量提取這塊，作者采用法向量平滑的方式進(jìn)行處理，即我取一個(gè)10×10大小的patch法向量的平均值來對當(dāng)前法向量進(jìn)行平滑。而后將我們提取的平面采用Point-Plane SLAM Using Supposed Planes for Indoor Environments 所介紹的數(shù)據(jù)關(guān)聯(lián)的方法與我們已有平面進(jìn)行匹配。大致思想就是說我們先對比它們之間的法向量角度的差異，如果小于一定閾值10度，我們可以再將它們的點(diǎn)到面的距離進(jìn)行對比，如果小于0.1m的話，我們將其插入已觀測地圖中，同時(shí)在地圖中加入平面平行和垂直關(guān)系為后續(xù)的追蹤線程提供約束。

位姿解耦與優(yōu)化

在追蹤線程中包含相機(jī)的旋轉(zhuǎn)、平移的估計(jì)以及最后的位姿優(yōu)化過程。位姿的接耦這部分作者推薦三篇論文供大家學(xué)習(xí)。

·?Divide and Conquer: Efficient ensity-based Tracking of 3D Sensors in Manhattan Worlds

·?Globally Optimal Manhattan Frame Estimation in Real-time

·?Visual Odometry with Drift-free Rotation Estimation Using Indoor Scene Regularities

后續(xù)我會(huì)針對這三篇論文進(jìn)行整理，分享一下我們?nèi)绾位诼D大世界的位姿解耦問題，在這里大家只要清楚作者通過在曼哈頓坐標(biāo)系下求得了我們當(dāng)前幀與曼哈頓坐標(biāo)系的旋轉(zhuǎn)矩陣

與

，基于此我們就可以求得當(dāng)前幀到世界坐標(biāo)系下的旋轉(zhuǎn)矩陣：

通過這種方式我可以先單獨(dú)將旋轉(zhuǎn)矩陣分解出來，這樣的做法區(qū)別于之前根據(jù)我們典型的根據(jù)相鄰幀的對極幾何約束所求的本質(zhì)矩陣或基礎(chǔ)矩陣不同，每一幀是在我們的曼哈頓坐標(biāo)下對旋轉(zhuǎn)矩陣進(jìn)行求解，這的好處是我們可以杜絕根據(jù)由于相鄰幀的位姿估計(jì)所導(dǎo)致的誤差的累計(jì)，當(dāng)然我們通過在曼哈頓世界坐標(biāo)系下求解的旋轉(zhuǎn)矩陣也仍然存在誤差，只不過沒有累積誤差這一說。

平移向量的估計(jì)我們通過構(gòu)建殘差方程，通過最小二乘來實(shí)現(xiàn)對平移向量的求解，作者根據(jù)點(diǎn)、線、面的約束構(gòu)建了三個(gè)子殘差方程，根據(jù)特征點(diǎn)的重投影誤差所構(gòu)建的殘差方程為：

對于線特征，作者在文章中介紹過于簡短，有必要參考論文Accurate and Linear Time Pose Estimation from Points and Lines 在此進(jìn)行簡短的介紹，對線特征的殘差構(gòu)之前，我們使用線特征的歸一化法向量來對我們的線特征進(jìn)行表征。

將線特征的端點(diǎn)P、Q投影至齊次坐標(biāo)系下與觀測量點(diǎn)乘得其殘差

關(guān)于線段的殘差方程為：

面特征的殘差方程為：

我們假設(shè)我們的觀測量服從高斯分布，那最終的非線性最小二乘可以寫為：

位姿優(yōu)化

在進(jìn)行位姿求解過程中，我們均在理想的曼哈頓假設(shè)進(jìn)行位姿的接偶與估計(jì)。在實(shí)際環(huán)境中難免會(huì)出現(xiàn)不嚴(yán)格遵循曼哈頓假設(shè)的情況是我們的位姿估計(jì)存在偏差，所以在獲取到初始化位姿后我們需要通過優(yōu)化模型對其偏差進(jìn)行補(bǔ)償。將根據(jù)關(guān)鍵幀建立起局部地圖投影到當(dāng)前幀進(jìn)行匹配搜索，另為作者加入平行和垂直的關(guān)系對平面進(jìn)行約束。

建圖

在建圖部分包含稀疏地圖以及平面實(shí)例網(wǎng)格地圖。其中稀疏地圖有點(diǎn)、線以及面的特征組成。建圖的策略跟其他經(jīng)典SLAM類似，即將新觀測到的點(diǎn)線面特征保存在局部地圖中，然后將質(zhì)量較高的特征放入全局地圖中。這里注意一點(diǎn)就是不同于點(diǎn)、線特征，對于面特征來說我們對于新檢測到的面特征在插入到全局地圖之前我們需要先檢查一下當(dāng)前面特征是否與已觀測到的平面相關(guān)聯(lián)，若關(guān)聯(lián)則將其加入到相關(guān)聯(lián)平面中，否則就以新的平面插入到地圖中。

2.實(shí)驗(yàn)驗(yàn)證

作者在與其他SLAM系統(tǒng)進(jìn)行對比時(shí)，選擇將回環(huán)檢測模塊關(guān)閉來進(jìn)行對比，取得了不錯(cuò)的實(shí)驗(yàn)效果，下面我將在ICL-NUM數(shù)據(jù)集上同ORB-SLAM2進(jìn)行對比實(shí)驗(yàn)。

誤差對比分析針對ICL-NUM的living＿room＿traj0＿frei＿png以及traj3＿frei＿png這兩個(gè)數(shù)據(jù)集進(jìn)行了測試，做了絕對軌跡誤差(ape)分析，結(jié)果如下所示：

表的左半部分為living＿room＿traj0＿frei＿png分析結(jié)果，右半部分為traj3＿frei＿png分析結(jié)果。

上圖為在traj3＿frei＿png數(shù)據(jù)集上測試的相機(jī)軌跡。

通過在數(shù)據(jù)集上的實(shí)驗(yàn)對比發(fā)現(xiàn)，在弱紋理、結(jié)構(gòu)化場景中，通過將線、面特征加入到ORB-SLAM2的框架中，可以有效的為SLAM位姿提供強(qiáng)有力的約束，提升了SLAM系統(tǒng)的穩(wěn)定性及精度，但同時(shí)由于加入面和線特征的緣故，會(huì)增加系統(tǒng)計(jì)算資源的投入。由于沒有真值進(jìn)行對比，僅將小車的運(yùn)動(dòng)軌跡顯示出來。

由于作者的程序僅提供在數(shù)據(jù)集下運(yùn)行的接口，為與實(shí)驗(yàn)室的D435相匹配，我將其封裝在ROS環(huán)境下運(yùn)作的程序，ros版本的程序分享在github：

https://github.com/WangWen-Believer/PlanarSLAM ros?

下面為在走廊環(huán)境下的實(shí)驗(yàn)效果：

通過實(shí)際測試效果來看，Planar在走廊基本實(shí)現(xiàn)對自身位姿的準(zhǔn)確估計(jì)，但在測試過程中需要注意，雖然PlanarSLAM有效的提升了在結(jié)構(gòu)化弱紋理場景下的魯棒性和精度。但在實(shí)際運(yùn)行中應(yīng)該避免相機(jī)直接面對一堵白墻，此時(shí)對相機(jī)來說無紋可言，無法有效提升點(diǎn)、線特征導(dǎo)致位姿估計(jì)錯(cuò)誤。