|
泡泡點(diǎn)云時(shí)空，帶你精讀點(diǎn)云領(lǐng)域頂級(jí)會(huì)議文章 標(biāo)題：BALM: Bundle Adjustment for Lidar Mapping 作者：Zheng Liu and Fu Zhang 來(lái)源：ICRA 2021 編譯：廖邦彥 審核：lionheart 這是泡泡點(diǎn)云時(shí)空推送的第217篇文章，歡迎個(gè)人轉(zhuǎn)發(fā)朋友圈；其他機(jī)構(gòu)或自媒體如需轉(zhuǎn)載，后臺(tái)留言申請(qǐng)授權(quán)  摘要  對(duì)于包含關(guān)鍵幀的滑窗進(jìn)行局部BA（集束調(diào)整）被廣泛的應(yīng)用于視覺(jué)SLAM，并且已經(jīng)證明BA可以有效的降低累計(jì)飄移。但是在激光SLAM中，BA受限于難以對(duì)稀疏的特征點(diǎn)（例如邊沿以及平面）進(jìn)行精確匹配，從而很少被使用。在這篇文章中，我們將激光BA建模成最小化特征點(diǎn)到對(duì)應(yīng)匹配的邊緣或者平面特征的距離。不像視覺(jué)SLAM（以及雷達(dá)SLAM中的先驗(yàn)平面優(yōu)化方法）需要特征點(diǎn)與位姿共同優(yōu)化確定，我們表明特征點(diǎn)可以通過(guò)解析方法求解并且從BA中去除，剩下的BA只取決于關(guān)鍵幀的位姿。這個(gè)方法顯著的降低了優(yōu)化的尺度并且使得大規(guī)模的稠密邊緣以及平面特征能夠被應(yīng)用于優(yōu)化中。為了加速整體優(yōu)化，我們閉式推導(dǎo)了誤差函數(shù)導(dǎo)數(shù)的解析式，一直到二階。同時(shí)，我們提出了一個(gè)新穎的自適應(yīng)體素化方法來(lái)高效的搜索對(duì)應(yīng)的特征。提出的方法被嵌入到LOAM后端來(lái)實(shí)現(xiàn)地圖優(yōu)化。結(jié)果表明，即使在后端，局部BA也能夠高效的求解，即使在實(shí)時(shí)環(huán)境也能以10Hz運(yùn)行同時(shí)優(yōu)化20幀點(diǎn)云。局部BA同時(shí)也能在一定程度上降低LOAM漂移。我們的BA優(yōu)化以及相關(guān)LOAM實(shí)現(xiàn)已經(jīng)開(kāi)源。  主要貢獻(xiàn)
 結(jié)果顯示局部BA能夠有效的降低漂移。雖然它被設(shè)計(jì)為后端系統(tǒng)，局部BA任然運(yùn)行的非?？?，當(dāng)優(yōu)化一個(gè)大小為20幀的滑窗時(shí)，它能夠幾乎以10Hz實(shí)時(shí)運(yùn)行。BA求解公式，優(yōu)化庫(kù)以及LOAM的具體實(shí)現(xiàn)都已經(jīng)在社區(qū)開(kāi)源。 方法概述 BA FORMULATION AND DERIVATIVES
定義稀疏特征點(diǎn)群  屬于不同的M幀，并且都對(duì)應(yīng)著統(tǒng)一特征。 我們定義第i個(gè)特征點(diǎn)來(lái)自于  幀，并且定義M幀的位姿集合  所以在世界坐標(biāo)系下的特征點(diǎn)定義為 圖三展示了稀疏特征點(diǎn)到平面以及邊緣的距離定義 對(duì)于每個(gè)平面特征，有以下公式 其中 表示的是依據(jù)矩陣A的特征值從大到小第k個(gè) 是對(duì)應(yīng)的特征向量，其它定義如下 對(duì)于每個(gè)邊緣特征，也有以下公式 其中 表示的是矩陣A的跡。從上式我們可以看到，最優(yōu)點(diǎn)q*對(duì)于之后的代價(jià)函數(shù)并沒(méi)有其它作用。此外，公式2以及4表明平面或者邊緣的最佳特征變量可以被解析的得到在執(zhí)行BA之前，以及剩下的BA問(wèn)題只取決于位姿T。最后的優(yōu)化問(wèn)題就變?yōu)樽钚』仃嘇的特征值。
定理一 矩陣A對(duì)于的特征值對(duì)特質(zhì)點(diǎn)有如下公式 定理二 對(duì)于不同的特征值 有如下公式
有了上文的第一二階導(dǎo)數(shù)，我們就可以對(duì)公式5進(jìn)行二階近似 下面導(dǎo)出位姿擾動(dòng) 對(duì)特征點(diǎn)的導(dǎo)數(shù) 定義位姿擾動(dòng)對(duì)位姿的影響 最后將公式12導(dǎo)入公式8 得到 使用LM方法迭代優(yōu)化 ADAPTIVE VOXELIZATION 假設(shè)我們獲得了不同幀的大概初始位姿（例如，通過(guò)LOAM里程計(jì)），我們不斷的將3D空間依據(jù)默認(rèn)尺寸（例如，1m）進(jìn)行柵格化。如果發(fā)現(xiàn)在當(dāng)前柵格中的所有特征點(diǎn)都與同一個(gè)特征相對(duì)應(yīng)（通過(guò)檢查協(xié)方差矩陣的特征值），我們就將其與包含的特征點(diǎn)保存在內(nèi)存中，如果不是這樣，當(dāng)前的柵格就會(huì)分裂成八叉樹(shù)，并且對(duì)每個(gè)子?xùn)鸥褡鐾瑯訖z查一直到柵格到了最小的尺寸（例如，0.125m）。最后可以得到一個(gè)有不同柵格尺寸的地圖，并且每個(gè)柵格都對(duì)應(yīng)著一個(gè)特征，因此也只有一個(gè)損失函數(shù)。圖四是一個(gè)示例。 a）不同的顏色對(duì)應(yīng)著不同的柵格。在圖片右下角的是對(duì)于樹(shù)枝平面點(diǎn)的聚焦圖像，包含了3個(gè)尺寸為0.125米的柵格 b）實(shí)際環(huán)境圖片 c）所有的樹(shù)節(jié)點(diǎn)都在哈希表中建立了索引 自適應(yīng)柵格擁有以下的優(yōu)點(diǎn) 1）有現(xiàn)存的數(shù)據(jù)結(jié)構(gòu)天然兼容，例如八叉樹(shù)，因此可以促進(jìn)它的實(shí)施與效率。 2）它通常比直接在特征點(diǎn)上構(gòu)造一個(gè)完整的Kd-樹(shù)更有效，因?yàn)楫?dāng)包含的特征點(diǎn)位于同一平面或邊緣上時(shí)，可能會(huì)更早終止繼續(xù)構(gòu)建。這種優(yōu)點(diǎn)在一個(gè)擁有大尺度平面以及長(zhǎng)邊緣特征時(shí)更加明顯。 3）具有自適應(yīng)體素的地圖將降低在激光雷達(dá)里程計(jì)中搜索對(duì)應(yīng)特征點(diǎn)的時(shí)間。只需要搜索特征點(diǎn)被包含或接近的對(duì)應(yīng)柵格，而不需要更耗時(shí)的搜索的最近鄰點(diǎn)。在我們的實(shí)際實(shí)現(xiàn)中，我們使用了兩個(gè)柵格地圖，分別存儲(chǔ)平面以及邊緣特征。為了降低樹(shù)的層數(shù)，我們使用哈希鍵值來(lái)存儲(chǔ)。 備注一 如果柵格地圖包含點(diǎn)過(guò)多，我們將同一特征上的同一幀的特征點(diǎn)進(jìn)行平均。這樣既能夠減少計(jì)算量也能夠保證不會(huì)降低建圖的一致性。 備注二 因?yàn)樵诙ɡ矶幸髮?duì)于λk具有代數(shù)多重性的體素，我們只需跳過(guò)它。 備注三 該方法自然地?cái)U(kuò)展到非平面特征（例如曲面)，通過(guò)在更精細(xì)的構(gòu)造體素映射，并在檢查所包含的點(diǎn)是否位于同一平面上時(shí)允許更大的方差。 備注四 我們?cè)O(shè)置了兩個(gè)條件來(lái)停止遞歸細(xì)分：一個(gè)是樹(shù)的最大深度，另一個(gè)是體素中的最小點(diǎn)數(shù)。 LOAM WITH LOCAL BA 當(dāng)構(gòu)造體素映射時(shí)，我們計(jì)算體素中平面（或邊緣)的中心點(diǎn)和法線(xiàn)(或方向）向量。然后，對(duì)于新來(lái)的一幀中的一個(gè)特征點(diǎn)，我們通過(guò)計(jì)算體素中的點(diǎn)與平面或邊緣特征之間的距離來(lái)搜索最近的體素（由它的中心點(diǎn)表示）。當(dāng)新的一幀加入到柵格地圖中后，地圖優(yōu)化被觸發(fā)，對(duì)一個(gè)滑窗中的全部雷達(dá)位姿進(jìn)行局部BA，優(yōu)化過(guò)后的位姿進(jìn)一步更新所有涉及的柵格的中心點(diǎn)以及法向量。當(dāng)一個(gè)滑窗滿(mǎn)了，屬于舊的幀的特征點(diǎn)就會(huì)被融合到Pfix中去，特征點(diǎn)點(diǎn)協(xié)方差矩陣的一個(gè)很好的性質(zhì)是存在遞歸形式，允許滑動(dòng)窗口外的所有特征點(diǎn)組成一些緊湊的矩陣和向量中，而不用保存原始數(shù)據(jù)點(diǎn)。合并后的Pfix將保留在體素圖中，用于里程計(jì)和地圖優(yōu)化。 實(shí)驗(yàn)結(jié)果 戶(hù)外行走數(shù)據(jù) a）使用BALM構(gòu)建全局地圖 b）使用LOAM構(gòu)建地圖 c）d）展示在起點(diǎn)/終點(diǎn)分別使用BALM和LOAM的側(cè)視圖 分別使用Livox Horizon雷達(dá)的BALM和LOAM的漂移誤差比較 室內(nèi)建圖以及里程計(jì)結(jié)果 a）整體場(chǎng)景 b）d）展示了LOAM的結(jié)果 c）e）展示了BALM的結(jié)果 展示了戶(hù)外數(shù)據(jù)集的結(jié)果 使用velodyne-16線(xiàn)雷達(dá)得到的漂移誤差以及與LOAM運(yùn)行時(shí)間建立柵格數(shù)目比較。 Abstract A local Bundle Adjustment (BA) on a sliding window of keyframes has been widely used in visual SLAM and proved to be very effective in lowering the drift. But in lidar SLAM, BA method is hardly used because the sparse feature points (e.g., edge and plane) make the exact point matching impossible. In this paper, we formulate the lidar BA as minimizing the distance from a feature point to its matched edge or plane. Unlike the visual SLAM (and prior plane adjustment method in lidarSLAM) where the feature has to be co-determined along with the pose, we show that the feature can be analytically solved and removed from the BA, the resultant BA is only dependent on the scan poses. This greatly reduces the optimization scale and allows large-scale dense plane and edge features to be used. To speedup the optimization, we derive the analytical derivatives of the cost function, up to second order, in closed form. Moreover, we propose a novel adaptive voxelization method to search feature correspondence efficiently. The proposed formulations are incorporated into a LOAM back-end for map refinement. Results show that, although as a back-end, the local BA can be solved very efficiently, even in real-time at 10Hz when optimizing 20 scans of point-cloud. The local BA also considerably lowers the LOAM drift. Our implementation of the BA optimization and LOAM are open-sourced to benefit the community. 如果你對(duì)本文感興趣，想要下載完整文章進(jìn)行閱讀，可以關(guān)注【泡泡機(jī)器人SLAM】公眾號(hào)。 歡迎來(lái)到泡泡論壇，這里有大牛為你解答關(guān)于SLAM的任何疑惑。 有想問(wèn)的問(wèn)題，或者想刷帖回答問(wèn)題，泡泡論壇歡迎你！ 泡泡網(wǎng)站：www. 泡泡論壇：http:///bbs/ |
|
|
來(lái)自： taotao_2016 > 《視覺(jué)》
