我原來(lái)總結(jié)過(guò)LOAM_Livox，這篇文章主要是解決LOAM在長(zhǎng)時(shí)間運(yùn)行的時(shí)累計(jì)誤差的問(wèn)題。本文提出的方法計(jì)算關(guān)鍵幀的2D直方圖，局部地圖patch，并使用2D直方圖的歸一化互相關(guān)(normalized cross-correlation)作為當(dāng)前關(guān)鍵幀與地圖中關(guān)鍵幀之間的相似性度量。這個(gè)方法快速且具有旋轉(zhuǎn)不變性，魯棒性高。

注1：文末附【VSLAM】交流群

注2：整理不易，請(qǐng)點(diǎn)贊支持！

作者：Liam| 來(lái)源：3D視覺(jué)工坊微信公眾號(hào)

介紹

常用的閉環(huán)檢測(cè)方法是詞袋模型，把特的描述子聚類成單詞，可以在詞空間中計(jì)算觀察值之間的相似度。不像視覺(jué)閉環(huán)的研究，基于雷達(dá)的閉環(huán)檢測(cè)的工就很少，到現(xiàn)在也沒(méi)有統(tǒng)一的開(kāi)源的工程。主要的原因有雷達(dá)的價(jià)格太高，沒(méi)有廣泛被使用；基于點(diǎn)云的場(chǎng)景識(shí)別是很難的，不像圖片中有豐富的紋理和顏色信息，點(diǎn)云里只有結(jié)構(gòu)信息。本文重要介紹閉環(huán)檢測(cè)，地圖配準(zhǔn)和位姿圖優(yōu)化。

相關(guān)工作

• “Place recognition using keypoint voting in large 3d lidar datasets"直接利用點(diǎn)云中的3D特征點(diǎn)把他們描述成人工制作的3DGestalt描述子，然后利用投票的方法找到距離關(guān)鍵點(diǎn)最近的3D點(diǎn)。

• “Structure-based vision-laser matching”(2016 IROS)利用正態(tài)分布變(NDT)來(lái)描述3D點(diǎn)云的外觀。利用NDT描述子的直方圖計(jì)算兩次掃描的相似度。

• “Segmatch：Segment based place recognition in 3d point clouds”通過(guò)匹配諸如建筑物，樹(shù)等語(yǔ)義信息進(jìn)行閉環(huán)檢測(cè)。

但是到目前為止，針對(duì)于LOAM并沒(méi)有開(kāi)源的代碼數(shù)據(jù)集。本文的主貢獻(xiàn)是：

• 研究出來(lái)一個(gè)快速的閉環(huán)檢測(cè)的方法來(lái)檢測(cè)兩個(gè)關(guān)鍵幀的相似度

• 把閉環(huán)檢測(cè)，地圖對(duì)齊，位姿優(yōu)化集成到LOAM中。

• 通過(guò)在Github上開(kāi)放數(shù)據(jù)集和源代碼，我們?yōu)榛邳c(diǎn)云的閉環(huán)提供了可用的解決方案和范例。( https：//github。com/hku-mars/loam_livox

系統(tǒng)概述

系統(tǒng)的整體過(guò)程如下所示：每一個(gè)傳入的新幀和和全局的雷達(dá)地圖配準(zhǔn)(LOAM algorithm)，如果已接收到指定數(shù)量的幀（例如100幀），則會(huì)創(chuàng)建一個(gè)關(guān)鍵幀，該關(guān)鍵幀會(huì)形成一個(gè)小的本地地圖補(bǔ)丁。通過(guò)LOAM將與新關(guān)鍵幀相對(duì)應(yīng)的原始點(diǎn)云配準(zhǔn)到全局地圖中，以計(jì)算其2D直方圖。將計(jì)算的2D直方圖與數(shù)據(jù)庫(kù)進(jìn)行比較，該數(shù)據(jù)庫(kù)包含由所有過(guò)去的關(guān)鍵幀組成的全局地圖的2D直方圖，以檢測(cè)可能的閉環(huán)。同時(shí)，將新的關(guān)鍵幀2D直方圖添加到數(shù)據(jù)庫(kù)中以供下一個(gè)關(guān)鍵幀使用。一旦檢測(cè)到閉環(huán)，就將關(guān)鍵幀與全局地圖對(duì)齊，并執(zhí)行位姿圖優(yōu)化以校正全局地圖中的漂移。

地圖和立方體

小胞體是有合適尺寸的小立方體(邊長(zhǎng)為Sx，Sy，Sz)，中心坐標(biāo)為立方體中第一個(gè)點(diǎn)坐標(biāo)。然后計(jì)算立方體中所有點(diǎn)的均值和協(xié)方差。需要注意的是，該單元格是3D空間的固定分區(qū)，并不斷填充新點(diǎn)。為了加快均值和協(xié)方差的計(jì)算，我們可以通過(guò)小立方體存在的N個(gè)點(diǎn)推導(dǎo)出來(lái)加入來(lái)的點(diǎn)以后的新的均值和協(xié)方差。地圖是所有的保存在小胞體中的點(diǎn)的總和，地圖點(diǎn)是用哈希表和八叉樹(shù)表示的。利用哈希表可以通過(guò)立方體的中心快速找到胞體。通過(guò)八叉樹(shù)可以快速找到給定范圍內(nèi)的所有的胞體。這兩個(gè)策略對(duì)于地圖對(duì)齊都很重要。對(duì)于新來(lái)的胞體，通過(guò)其各個(gè)組成部分(中心坐標(biāo))的XOR操作來(lái)得到其哈希索引值。然后將計(jì)算出的哈希索引添加到地圖H的哈希表中。由于該單元格是3D空間的固定分區(qū)，因此其中心位置是靜態(tài)的，不需要更新哈希表中的現(xiàn)有條目（哈希表 雖然正在動(dòng)態(tài)增長(zhǎng)）。新插入的立方體根據(jù)中心坐標(biāo)也被添加到八叉樹(shù)地圖中。

算法一：新幀配準(zhǔn)

輸入：第k幀的點(diǎn)云，當(dāng)前的地圖，利用LOAM估計(jì)出來(lái)的相機(jī)位姿(R_k，T_k)對(duì)于新幀中的每個(gè)點(diǎn)：

• 把每個(gè)點(diǎn)利用位姿轉(zhuǎn)換到全局坐標(biāo)系

• 利用公式1計(jì)算cell的中心

• 計(jì)算中心點(diǎn)的哈希值索引

• 如果這個(gè)哈希值不在哈希表

• 利用中心值創(chuàng)建一個(gè)新的cell

• 把地圖的哈希索引的值插入到哈希表中

• 把中心值插入到地圖的八叉樹(shù)中

• 把這個(gè)點(diǎn)添加到cell中

• 更新cell的平均值

• 更新協(xié)方差矩陣

2D直方圖的旋轉(zhuǎn)不變性

快速閉檢測(cè)主要思想是：我們使用類似于2D圖的直方圖粗略描述關(guān)鍵幀。2D直方圖描述了關(guān)鍵幀中特征方向的Eulerangles的分布。

cell中特征的類型和方向

每個(gè)關(guān)鍵幀由一百次掃描的結(jié)果組成，對(duì)于每個(gè)cell，我們利用每個(gè)特征點(diǎn)和相關(guān)的特征的方向來(lái)確定他的形狀。第一步首先進(jìn)行特征值分解，依據(jù)Appearance-based loop detection from 3d laser data using the normal distributions transform，根據(jù)分解出的特征值：

• 如果λ2遠(yuǎn)大于λ3，則將此cell視為平面特征，并將平面的法向量作為特征的方向；

• 如果cell不是平面而且λ1遠(yuǎn)大于λ2，則將此cell視為線特征，并將線的方向視為特征方向；

旋轉(zhuǎn)不變性

為了保證每個(gè)特征都具有旋轉(zhuǎn)不變性，我們利用一個(gè)額外的旋轉(zhuǎn)矩陣旋轉(zhuǎn)每個(gè)特征的方向，并以此保大多數(shù)特征都在x軸的方向。第二多的是在Y軸的方向上。因?yàn)槠矫嫣卣鞲煽?，所以我們利用平特征的方向?lái)確定旋轉(zhuǎn)矩陣。

• 首先利用平面特征的方向向量來(lái)計(jì)算協(xié)方差矩陣

• 利用特征值分解協(xié)方差矩陣

• 利用特征向量得到旋轉(zhuǎn)矩陣

算法二：計(jì)算關(guān)鍵幀的2D分布

• 輸入關(guān)鍵幀F(xiàn)

• 輸入2D線特征的分布和面特征的分布H_L，H_P

• 開(kāi)始設(shè)置H_L,H_P為0，同時(shí)計(jì)算旋轉(zhuǎn)矩

對(duì)于關(guān)鍵幀中的每個(gè)cell

• 如果cell是線特征

• 把特征×旋轉(zhuǎn)矩陣

• 計(jì)算旋轉(zhuǎn)后特征的pitch和yaw

• ?H_L[round(pitch/3)，round(yaw/3)]+=1

• 如果cell是面特征

• 把特征×旋轉(zhuǎn)矩陣

• 計(jì)算旋轉(zhuǎn)后特征的pitch和yaw

• H_P[round(pitch/3)，round(yaw/3)]+=1

對(duì)H_P和H_L進(jìn)行濾波每個(gè)關(guān)鍵幀包100次掃描結(jié)果，其包含了多個(gè)cell，每個(gè)關(guān)鍵幀由2個(gè)2D直方圖組成：線分布和面分布。利用旋轉(zhuǎn)不變的cell特征的方向我們計(jì)算2D直方圖：

• 選擇X的分量為正的，計(jì)算特征分量的pitch和yaw

• 利用60*60的矩陣表示2D的直方圖（每個(gè)pitch和yaw都有3度的分辨率）

• 利用pitch和yaw確定這個(gè)cell在矩中的位置

• 對(duì)每個(gè)2D直方圖進(jìn)行高斯濾波以提升魯棒性

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

通過(guò)計(jì)算新幀的2D直方圖和其他所有幀的相似度來(lái)檢測(cè)閉環(huán)，這個(gè)關(guān)鍵幀和地圖匹配然后地圖利用位姿圖優(yōu)化的方進(jìn)行更新。

（1）兩幀相似度計(jì)算使用2D直方圖的歸一化互相關(guān)來(lái)計(jì)算它們的相似性，論文中給出了相似度計(jì)算公式：如果平面相似度計(jì)算值大于0.9或線段相似度計(jì)算值大于0.65，就可以認(rèn)為檢測(cè)出了閉環(huán)。

（2）地圖對(duì)齊及優(yōu)化成功檢測(cè)到閉環(huán)后，執(zhí)行地圖對(duì)齊以計(jì)算兩個(gè)關(guān)鍵幀之間的相對(duì)位姿。地圖對(duì)齊問(wèn)題可以看作是目標(biāo)點(diǎn)云和源點(diǎn)云之間的配準(zhǔn)。由于LOAM算法中對(duì)線性形狀和平面形狀的像元進(jìn)行了分類，因此我們使用邊緣到邊緣和平面到平面的特征來(lái)迭代求解相對(duì)姿勢(shì)。對(duì)齊后，如果邊緣/平面特征上的點(diǎn)的平均距離足夠接近邊緣/平面特征（距離小于0。1m）則我們將這兩張地圖對(duì)齊。（3） 位姿圖優(yōu)化 一旦兩個(gè)關(guān)鍵幀對(duì)齊，執(zhí)行位姿圖優(yōu)化。我們使用Google ceres-solver實(shí)現(xiàn)圖優(yōu)化。優(yōu)化位姿圖后，我們通過(guò)重新計(jì)算包含的點(diǎn)，點(diǎn)的均值和協(xié)方差來(lái)更新整個(gè)地圖中的所有像元。

參考文獻(xiàn)：

[1] Lin J , Zhang F . A fast, complete, point cloud based loop closure for LiDAR odometry and mapping[J]. 2019.

[2]?https://zhuanlan.zhihu.com/p/93600130

備注：作者也是我們「3D視覺(jué)從入門到精通」特邀嘉賓：一個(gè)超干貨的3D視覺(jué)學(xué)習(xí)社區(qū)

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

3D視覺(jué)工坊-VSLAM交流群

已建立3D視覺(jué)工坊-VSLAM微信交流群！想要進(jìn)VSLAM學(xué)習(xí)交流群的同學(xué)，可以直接加微信號(hào)：CV_LAB。加的時(shí)候備注一下：VSLAM+學(xué)校+昵稱，即可。然后就可以拉你進(jìn)群了。

強(qiáng)烈推薦大家關(guān)注3D視覺(jué)工坊知乎賬號(hào)和3D視覺(jué)工坊微信公眾號(hào)，可以快速了解到最新優(yōu)質(zhì)的3D視覺(jué)與SLAM論文。