日本无码免费高清在线|成人日本在线观看高清|A级片免费视频操逼欧美|全裸美女搞黄色大片网站|免费成人a片视频|久久无码福利成人激情久久|国产视频一二国产在线v|av女主播在线观看|五月激情影音先锋|亚洲一区天堂av

  • 手機(jī)站
  • 小程序

    汽車(chē)測(cè)試網(wǎng)

  • 公眾號(hào)

    • 汽車(chē)測(cè)試網(wǎng)

    • 在線課堂

    • 電車(chē)測(cè)試

FAST-LIO2: 快速直接的激光雷達(dá)-慣性里程計(jì)

2021-09-12 21:53:49·  來(lái)源：同濟(jì)智能汽車(chē)研究所  
　
收斂后，最優(yōu)狀態(tài)和協(xié)方差估計(jì)為:
 
 
隨著狀態(tài)更新
，第k次掃描中的每個(gè)激光雷達(dá)點(diǎn)(
)通過(guò)以下方式轉(zhuǎn)換為全局坐標(biāo)系：
 
 
轉(zhuǎn)換后的激光雷達(dá)點(diǎn){
}被插入到由ikd-Tree表示的地圖中（參見(jiàn)第V章）。我們的狀態(tài)估計(jì)總結(jié)在算法1中，如下。
 
 
 
5 建圖
在本節(jié)中，我們將描述如何增量維護(hù)地圖（即插入和刪除），并通過(guò)ikd-Tree對(duì)其執(zhí)行k-nearest搜索。為了從理論上證明ikd-Tree的時(shí)間效率，我們對(duì)時(shí)間復(fù)雜度進(jìn)行了全面的分析。
A.地圖管理
地圖點(diǎn)被組織成一個(gè)ikd-Tree，它通過(guò)以里程計(jì)速率合并新的一幀點(diǎn)云而動(dòng)態(tài)增長(zhǎng)。為了防止地圖的大小不受約束，在ikd樹(shù)上僅保留激光雷達(dá)當(dāng)前位置周?chē)L(zhǎng)度為L(zhǎng)的大局部區(qū)域中的地圖點(diǎn)。2D演示如圖3所示。地圖區(qū)域被初始化為一個(gè)邊長(zhǎng)為L(zhǎng)的立方體，它以初始激光雷達(dá)位置p0為中心。假設(shè)激光雷達(dá)的檢測(cè)區(qū)域是一個(gè)以從(15)中得到的激光雷達(dá)當(dāng)前位置為球心的檢測(cè)球。假設(shè)探測(cè)球的半徑為r=γR，其中R是激光雷達(dá)視場(chǎng)角范圍，γ是大于1的松參數(shù)。當(dāng)激光雷達(dá)移動(dòng)到新位置p'時(shí)（在p’處探測(cè)球接觸到地圖的邊界），地圖區(qū)域沿激光雷達(dá)檢測(cè)區(qū)域與觸摸邊界之間距離增加的方向移動(dòng)。地圖區(qū)域移動(dòng)的距離設(shè)置為常數(shù)d=(γ-1)R。新地圖區(qū)域和舊地圖區(qū)域之間的減法區(qū)域中的所有點(diǎn)都將通過(guò)V-C小節(jié)中詳述的框式刪除操作從ikd-Tree中被刪除。
 
 
圖2 地圖區(qū)域管理的2D演示。在（a）中，藍(lán)色矩形是長(zhǎng)度為L(zhǎng)的初始地圖區(qū)域。紅色圓圈是以初始LiDAR位置p0為圓心的初始檢測(cè)區(qū)域。在（b）中，檢測(cè)區(qū)域（紅色虛線圓圈）移動(dòng)到地圖邊界被觸摸的新位置p'（紅色實(shí)線圓圈）。地圖區(qū)域移動(dòng)到新位置（綠色矩形）距離d。減法區(qū)域（橙色區(qū)域）中的點(diǎn)從地圖（即ikd-Tree）中移除。
B.樹(shù)狀結(jié)構(gòu)和構(gòu)造
1)數(shù)據(jù)結(jié)構(gòu)：ikd-Tree是一個(gè)二叉搜索樹(shù)。ikd-Tree中樹(shù)節(jié)點(diǎn)的屬性在Data Structure中呈現(xiàn)。
 
 
k-d樹(shù)的許多現(xiàn)有實(shí)現(xiàn)只在葉節(jié)點(diǎn)[43]–[45，53，54]上存儲(chǔ)點(diǎn)的“桶”，與此不同，我們的ikd-Tree在葉節(jié)點(diǎn)和內(nèi)部節(jié)點(diǎn)上都存儲(chǔ)點(diǎn),以更好地支持動(dòng)態(tài)點(diǎn)插入和樹(shù)的重平衡。當(dāng)使用單個(gè)k-d樹(shù)時(shí)，這種存儲(chǔ)模式在kNN搜索中也表現(xiàn)出更高的效率[41]，我們的ikd-Tree就是這種情況。由于一個(gè)點(diǎn)對(duì)應(yīng)于ikd-Tree上的單個(gè)節(jié)點(diǎn)，我們將交替使用點(diǎn)和節(jié)點(diǎn)。點(diǎn)信息（例如，點(diǎn)坐標(biāo)、強(qiáng)度）存儲(chǔ)在point中.屬性leftchild和rightchild分別是指向其左右子節(jié)點(diǎn)的指針。分割空間的分割軸記錄在axis中。以當(dāng)前節(jié)點(diǎn)為根的(子)樹(shù)的樹(shù)節(jié)點(diǎn)數(shù)(包括有效和無(wú)效節(jié)點(diǎn))保持在屬性treesize中。當(dāng)點(diǎn)從地圖中刪除時(shí)，ikd-Tree不會(huì)立即從樹(shù)中刪除節(jié)點(diǎn)，而只是將布爾變量detected置為真（詳見(jiàn)第V-C2節(jié)）。如果刪除以當(dāng)前節(jié)點(diǎn)為根的整個(gè)（子）樹(shù)，則treedeleted設(shè)置為true。從（子）樹(shù)中刪除的點(diǎn)數(shù)匯總到屬性invalidnum中。屬性range記錄了（子）樹(shù)上點(diǎn)的范圍信息，它被解釋為一個(gè)包含所有點(diǎn)的外接軸對(duì)齊長(zhǎng)方體。外接長(zhǎng)方體由其每個(gè)維度上分別具有最小和最大坐標(biāo)的兩個(gè)對(duì)角頂點(diǎn)表示。
2）構(gòu)建：構(gòu)建ikd-Tree類(lèi)似于[40]中構(gòu)建靜態(tài)k-d樹(shù)。ikd-Tree沿著最長(zhǎng)維度遞歸地在中間點(diǎn)分割空間，直到子空間中只有一個(gè)點(diǎn)。Data Structure中的屬性在構(gòu)造過(guò)程中被初始化，包括計(jì)算樹(shù)的大小和(子)樹(shù)的范圍信息。
C.增量更新
ikd-Tree上的增量更新指的在動(dòng)態(tài)重新平衡之后而進(jìn)行的增量操作，其中動(dòng)態(tài)重新平衡詳見(jiàn)第V-D節(jié)。ikd-Tree支持兩種類(lèi)型的增量操作：點(diǎn)式操作和框式操作。點(diǎn)式操作在k-d樹(shù)中插入、刪除或重新插入單個(gè)點(diǎn)，而框式操作在給定的軸對(duì)齊長(zhǎng)方體中插入、刪除或重新插入所有點(diǎn)。在這兩種情況下，點(diǎn)插入都進(jìn)一步與樹(shù)上的下采樣相結(jié)合，從而將地圖保持在預(yù)定的分辨率。在本文中，我們只解釋了FAST-LIO2的地圖管理所需的點(diǎn)式插入和框式刪除。讀者可以參考我們?cè)贕ithub存儲(chǔ)庫(kù)中ikd-Tree的開(kāi)源完整實(shí)現(xiàn)以及其中包含的技術(shù)文檔以了解更多詳細(xì)信息。
1)使用樹(shù)上的下采樣進(jìn)行點(diǎn)插入：考慮到機(jī)器人的應(yīng)用，我們的ikd-Tree支持同時(shí)進(jìn)行點(diǎn)插入和地圖下采樣。該算法在算法2中有詳細(xì)說(shuō)明。
 
 
對(duì)于來(lái)自狀態(tài)估計(jì)模塊（見(jiàn)算法1）的中的給定點(diǎn)p和下采樣分辨率l，算法將空間均勻地劃分為邊長(zhǎng)為l的立方體，然后是找到包含點(diǎn)p的立方體CD（第2行）。該算法僅保留最接近CD中心點(diǎn)pcenter的點(diǎn)（第3行）。這是通過(guò)首先在k-d樹(shù)上搜索CD中包含的所有點(diǎn)，并將它們與新點(diǎn)p一起存儲(chǔ)在點(diǎn)數(shù)組V中來(lái)實(shí)現(xiàn)的（第4-5行）。最近的點(diǎn)pnearest是通過(guò)比較V中每個(gè)點(diǎn)到中心pcenter的距離來(lái)獲得的（第6行）。然后刪除CD中的現(xiàn)有點(diǎn)（第7行），然后將最近的點(diǎn)pnearest插入k-d樹(shù)（第8行）。框式搜尋的實(shí)現(xiàn)類(lèi)似于第V-C2節(jié)中介紹的框式刪除。
ikd-Tree上的點(diǎn)插入（第11-24行）是遞歸實(shí)現(xiàn)的。該算法從根節(jié)點(diǎn)開(kāi)始向下搜索，直到找到一個(gè)空節(jié)點(diǎn)以追加一個(gè)新節(jié)點(diǎn)（第12-14行）。新葉節(jié)點(diǎn)的屬性初始化為表1。在每個(gè)非空節(jié)點(diǎn)上，新的點(diǎn)與存儲(chǔ)在樹(shù)節(jié)點(diǎn)上的點(diǎn)沿分割軸進(jìn)行比較，以便進(jìn)一步遞歸（第15-20行）。這些被訪問(wèn)過(guò)的節(jié)點(diǎn)的屬性（例如，treesize、range）用第V-C3節(jié)中介紹的最新信息（第21行）進(jìn)行更新。算法檢查并維護(hù)用新點(diǎn)更新的子樹(shù)的平衡標(biāo)準(zhǔn)，以保持ikd-Tree（第22行）的平衡特性，詳見(jiàn)第V-D節(jié)。
表1 要插入的新樹(shù)節(jié)點(diǎn)的屬性初始化
 
 
2)使用懶惰標(biāo)簽進(jìn)行框式刪除：在刪除操作中，我們使用了惰性刪除策略。也就是說(shuō)，這些點(diǎn)不會(huì)立即從樹(shù)中刪除，而只是通過(guò)將屬性detected置為true來(lái)將點(diǎn)標(biāo)記為“已刪除”（參見(jiàn)Data Structure，第6行）。如果以節(jié)點(diǎn)T為根的子樹(shù)上的所有節(jié)點(diǎn)都已被刪除，則T的屬性treedeleted被設(shè)置為true。因此detected和treedeleted的屬性稱(chēng)為惰性標(biāo)簽。標(biāo)記為“已刪除”的點(diǎn)將在重建過(guò)程中從樹(shù)中刪除（參見(jiàn)第V-D節(jié)）。
框式刪除是利用屬性range中的范圍信息和樹(shù)節(jié)點(diǎn)上的惰性標(biāo)簽實(shí)現(xiàn)的。如V-B中所述，屬性range由外接長(zhǎng)方體CT表示。偽代碼如算法3所示。
 
 
給定要從以T為根的（子）樹(shù)中刪除的長(zhǎng)方體CO，該算法遞歸搜索樹(shù)并將外接長(zhǎng)方體CT與給定的長(zhǎng)方體CO進(jìn)行比較。如果CT和CO之間沒(méi)有交集，遞歸直接返回而不更新樹(shù)（第2行）。如果外接長(zhǎng)方體CT完全包含在給定的長(zhǎng)方體CO中，則框式刪除將屬性detected和treedeleted都置為真（第5行）。當(dāng)（子）樹(shù)上的所有點(diǎn)都被刪除時(shí)，屬性invalidnum等于treesize（第6行）。對(duì)于CT與CO相交但不包含在CO中的情況，如果當(dāng)前點(diǎn)p包含在CO中，則首先將其從樹(shù)中刪除（第9行），然后算法遞歸查找子節(jié)點(diǎn)（第10-11行）。當(dāng)前節(jié)點(diǎn)T的屬性更新和平衡維護(hù)在框式刪除操作之后進(jìn)行（第12-13行）。
3)屬性更新：在每次增量操作后，使用AttributeUpdate函數(shù)，用最新的信息更新被訪問(wèn)節(jié)點(diǎn)的屬性。該函數(shù)通過(guò)匯總其兩個(gè)子節(jié)點(diǎn)上的對(duì)應(yīng)屬性和自身的點(diǎn)信息來(lái)計(jì)算屬性treesize和invalidnum；通過(guò)合并兩個(gè)子節(jié)點(diǎn)的范圍信息和存儲(chǔ)在其上的點(diǎn)信息來(lái)確定屬性range；如果兩個(gè)子節(jié)點(diǎn)的treedeleted都為真，并且節(jié)點(diǎn)本身將被刪除，則treedeleted被設(shè)置為真。
D.重平衡
ikd-Tree會(huì)在每次增量操作后主動(dòng)監(jiān)控balance特性，并通過(guò)僅重新構(gòu)建相關(guān)子樹(shù)來(lái)動(dòng)態(tài)地重平衡自身。
1)平衡準(zhǔn)則：平衡準(zhǔn)則由兩個(gè)子準(zhǔn)則組成：α-平衡準(zhǔn)則和α-刪除準(zhǔn)則。假設(shè)ikd-Tree的一個(gè)子樹(shù)以T為根。當(dāng)且僅當(dāng)它滿(mǎn)足以下條件時(shí)，該子樹(shù)是α-平衡的：
 
 
其中
，S(T)是節(jié)點(diǎn)T的treesize屬性。
以T為根的一個(gè)子樹(shù)的α-刪除準(zhǔn)則是：
 
 
其中
，I(T)表示子樹(shù)上無(wú)效節(jié)點(diǎn)的數(shù)量（即節(jié)點(diǎn)T的屬性invalidnum）。
如果ikd-Tree的一個(gè)子樹(shù)同時(shí)滿(mǎn)足這兩個(gè)準(zhǔn)則，則該子樹(shù)是平衡的。如果所有子樹(shù)都是平衡的，則整個(gè)樹(shù)是平衡的。違反任一準(zhǔn)則將觸發(fā)重建過(guò)程以重新平衡該子樹(shù)：α-平衡準(zhǔn)則保持樹(shù)的最大高度。很容易證明，α-平衡樹(shù)的最大高度是
，其中n是樹(shù)的大??；α-刪除準(zhǔn)則確保刪除子樹(shù)上的無(wú)效節(jié)點(diǎn)（即，標(biāo)記為“已刪除”）以減小樹(shù)的大小。降低k-d樹(shù)的高度和大小允許在未來(lái)進(jìn)行高效的增量操作和查詢(xún)。
2）重建和并行重建：假設(shè)在子樹(shù)T上觸發(fā)重建（見(jiàn)圖4），首先將子樹(shù)展平為點(diǎn)存儲(chǔ)陣列V。標(biāo)記為“已刪除”的樹(shù)節(jié)點(diǎn)在展平期間被丟棄。然后如第V-B節(jié)所述，用V中所有的點(diǎn)構(gòu)建一個(gè)新的完美平衡的k-d樹(shù)。當(dāng)在ikd-Tree上重新構(gòu)建大型子樹(shù)時(shí)，可能會(huì)出現(xiàn)相當(dāng)大的延遲并破壞FAST-LIO2的實(shí)時(shí)性能。為了保持較高的實(shí)時(shí)性，我們?cè)O(shè)計(jì)了一種雙線程重建方法。我們提出的方法不是簡(jiǎn)單地在第二個(gè)線程中重新構(gòu)建，而是通過(guò)一個(gè)操作記錄器避免兩個(gè)線程中的信息丟失和內(nèi)存沖突，從而始終保持k-最近鄰搜索的完全準(zhǔn)確性。
 
 
圖4 重建一個(gè)不平衡的子樹(shù)
分享到：
 
反對(duì) 0 舉報(bào) 0 收藏 0 評(píng)論 0
滬ICP備11026917號(hào)-25