參考Hydra、ChatGPTHydra 教程一為什么機器人需要三維場景圖SLAMSimultaneous Localization and Mapping同時定位與建圖可以估計機器人走過的軌跡也可以重建周圍幾何但機器人若要執(zhí)行“去會議室找打印機”這樣的任務還必須知道哪些表面屬于同一個物體、哪些自由空間彼此連通、物體屬于哪個房間以及地圖在回環(huán)校正后怎樣保持這些關系一致。Hydra 的目標就是在度量地圖之上實時建立并維護這種多層空間表示。本篇只回答三個基礎問題為什么 SLAM 地圖不能直接承擔全部空間推理動態(tài)三維場景圖究竟保存什么Hydra 怎樣把局部傳感器觀測逐步變成可查詢、可校正的全局結構讀完本篇后讀者應能說清 Hydra 在機器人系統(tǒng)中的職責邊界并知道后續(xù)每篇文章解決哪一個尚未解決的問題。1. SLAM 解決了定位和幾何但沒有自動得到任務語義典型 SLAM 系統(tǒng)主要估計兩類結果機器人軌跡 {TWB(t)}和環(huán)境地圖 M \text{機器人軌跡 } \{\mathbf{T}_{WB}(t)\} \quad\text{和}\quad \text{環(huán)境地圖 } \mathcal{M}機器人軌跡{TWB?(t)}和環(huán)境地圖M其中TWB(t)\mathbf{T}_{WB}(t)TWB?(t)表示時刻ttt機器人機體坐標系BBB到世界坐標系WWW的位姿M\mathcal{M}M可以是特征點、點云、體素、網(wǎng)格或占據(jù)柵格。這些結果對定位和避障很重要卻不會自動回答下面的問題。下表中的 Object 指可獨立引用的物體實例Place 指自由空間骨架上的稀疏代表位置Room 指由一組 Places 組織出的房間實例這些表示將在后文正式定義。任務問題僅有軌跡和幾何時還缺少什么“打印機在哪里”哪些表面屬于同一個打印機實例“打印機在哪個房間”O(jiān)bject、Place 和 Room 之間的歸屬關系“從辦公室能否到會議室”自由空間的稀疏連通結構而不只是表面點“前方還有哪里沒探索”已知可達空間和未知空間的邊界“回到起點后為什么出現(xiàn)兩份辦公室”回環(huán)約束以及幾何、實體和層級的聯(lián)合校正這不是 SLAM 做得不夠好而是問題層次不同。SLAM 首先解決“機器人和幾何在哪里”任務規(guī)劃還需要“這里有什么、彼此什么關系、應該通過哪些空間到達”。Hydra 消費上游定位結果和傳感器觀測在其上構建面向空間推理的表示。這種把抽象概念連接到實際物理實體的過程稱為符號落地。例如命令中的“打印機”只是一個符號機器人必須把它對應到地圖中的某個三維實例、位置和所屬房間這個符號才真正能夠指導行動。2. 同一個環(huán)境為什么需要多種表示假設機器人進入一間辦公室看到墻、地面、桌子、椅子和打印機。不同表示回答的問題不同。2.1 度量表示保存精確幾何度量表示中的位置和距離具有真實物理尺度。例如體素邊長是0.1 m0.1\,\text{m}0.1m網(wǎng)格頂點具有世界坐標機器人可以計算自己離墻還有多遠。Hydra 使用的底層度量表示包括TSDFTruncated Signed Distance Field截斷有符號距離場融合多幀深度并描述觀測表面附近的截斷有符號距離。語義體素把類別證據(jù)與體素幾何共同融合。三角網(wǎng)格把隱式距離場轉成可顯示、可分割的顯式表面。ESDFEuclidean Signed Distance Field歐氏有符號距離場給自由空間位置提供到障礙的歐氏距離。GVDGeneralized Voronoi Diagram廣義 Voronoi 圖在 ESDF 中找出同時受多個障礙表面限制的位置形成自由空間中軸骨架。這些表示精確卻非常密集。機器人若要查詢“辦公室里有哪些對象”不應該每次都掃描數(shù)百萬體素或網(wǎng)格頂點。2.2 語義地圖告訴機器人“這是什么”語義地圖在幾何位置上附加類別信息例如墻、地面、椅子或打印機。它把“這里存在表面”推進到“這里可能是某一類表面”。但逐點或逐體素標簽仍然是扁平表示。它可能有許多chair頂點卻沒有明確說明這些頂點屬于一把椅子還是兩把椅子也沒有表示椅子和房間的關系。2.3 實例與關系讓語義能夠被查詢?yōu)榱俗寵C器人引用一個具體實體需要把許多底層證據(jù)聚合成一個實例節(jié)點許多 chair 網(wǎng)格頂點 - Object O7 許多自由空間骨架體素 - Place p12 一組相互連通的 Places - Room R2節(jié)點解決“有哪些實體”邊解決“實體之間有什么關系”。例如O7 -- p12 對象以哪個自由空間位置作為空間錨點 p12 -- p13 兩個自由空間代表位置之間存在連接 R2 -- p12 這個 Place 被分配到哪個 Room2.4 拓撲表示壓縮“怎樣連通”這里的拓撲主要指連通關系。兩條走廊即使長度略有誤差只要連接結構沒有改變拓撲仍可以保持一致。Hydra 的 Places 圖把連續(xù)自由空間壓縮成稀疏節(jié)點和邊節(jié)點代表自由空間骨架上的位置邊代表底層幾何證據(jù)支持的連接。它不取代精確地圖而是讓路徑搜索、房間發(fā)現(xiàn)和回環(huán)描述不必直接處理全部體素。所以 Hydra 并不是在幾種地圖中選擇一種而是讓它們分工體素和 Mesh保留幾何精度 Objects表達可引用實體 Places表達自由空間拓撲 Rooms / Building表達更高層空間組織 Agents表達機器人經(jīng)過這些空間的時間軌跡3. 動態(tài)三維場景圖到底是什么在時刻ttt可以把 Hydra 的動態(tài)場景圖寫成Gt(Vt,Et,At,Mt) \mathcal{G}_t(\mathcal{V}_t,\mathcal{E}_t,\mathcal{A}_t,\mathcal{M}_t)Gt?(Vt?,Et?,At?,Mt?)其中Vt\mathcal{V}_tVt?是節(jié)點集合例如 Objects、Places、Agents、Rooms 和 Building。Et\mathcal{E}_tEt?是邊集合表示同層鄰接或跨層歸屬。At\mathcal{A}_tAt?是節(jié)點和邊的屬性例如位置、語義類別、清障距離和時間戳。Mt\mathcal{M}_tMt?是掛在圖上的全局三角網(wǎng)格它提供稠密幾何證據(jù)但網(wǎng)格頂點不需要全部成為普通圖節(jié)點。下標ttt不表示每一幀保存一份完整圖副本而表示同一張圖會隨著新觀測、歸檔、節(jié)點合并和后端優(yōu)化持續(xù)變化。3.1 DSG 是分層圖但不是一棵樹DSG 是Dynamic Scene Graph的縮寫中文可譯為動態(tài)場景圖。它具有層級關系卻不能簡單畫成一棵樹因為每一層內部還可能有橫向邊和回路。例如Building B0 | Room R0 ----- Room R1 / \ | p0 -- p1 ------ p2 | | O3 O4B0 - R0是高層到低層的包含關系。R0 -- R1是兩個房間的鄰接關系。p0 -- p1 -- p2是自由空間拓撲。p0 - O3表示 Object 與附近 Place 的跨層錨定。只要存在p0--p1--p2--p0一類同層回路整張圖就不是樹。層次只規(guī)定不同抽象尺度怎樣組織不要求刪除同層拓撲。3.2 Layer 和 partition 分別解決什么問題Layer按實體的抽象層次組織節(jié)點partition則允許同一層號中保存生成方式或所有者不同的子圖。當前 C 倉庫的默認命名層可以概括為名稱默認層鍵一個節(jié)點表示什么主要證據(jù)來源OBJECTS(2,0)(2,0)(2,0)一個三維語義對象實例語義 Mesh 頂點聚類AGENTS層號 2、按機器人前綴分區(qū)機器人在某個時刻的離散位姿上游里程計或位姿圖PLACES(3,0)(3,0)(3,0)三維自由空間骨架代表位置ESDF、GVD 和圖壓縮MESH_PLACES(3,1)(3,1)(3,1)指定語義表面的二維區(qū)域Mesh 分割、邊界和橢圓近似ROOMS(4,0)(4,0)(4,0)一組 Places 形成的幾何房間Places 圖的多尺度聚類BUILDINGS(5,0)(5,0)(5,0)當前 Rooms 的頂層聚合Room 集合OBJECTS和AGENTS共享層號 2卻位于不同分區(qū)PLACES和MESH_PLACES共享層號 3也位于不同分區(qū)。因此層號相同不表示它們是同一種節(jié)點partition 也不表示父子高低關系。Frontier前沿表示已知可達空間旁邊、值得繼續(xù)觀測的未知邊界候選。當前實現(xiàn)把 Frontier 節(jié)點放入PLACES層但用屬性區(qū)分它不是由 GVD 產(chǎn)生的真實 Place。這樣探索模塊可以把未知邊界候選連接到真實 Places 圖而不必再建立一套完全獨立的導航骨架。3.3 同層邊與跨層邊不能按同一種語義解釋同層邊連接同一個層鍵中的節(jié)點Place–Place 邊表示自由空間連接并可攜帶路徑瓶頸清障值。Agent–Agent 邊表示軌跡狀態(tài)之間的圖連接。Room–Room 邊表示底層 Places 存在跨房間鄰接。跨層邊連接不同抽象尺度Room–Place 表示幾何房間成員關系。Place–Object 表示對象的空間錨點。Building–Room 表示頂層包含關系。同一個通用EdgeAttributes類型可以在不同層承載不同意義所以不能看到weight就假定它一定是歐氏距離也不能把所有跨層邊都當作傳感器直接測量。每類邊都有自己的證據(jù)來源和接受條件。4. Hydra 怎樣從觀測逐層得到這些節(jié)點高層節(jié)點不是由一個神經(jīng)網(wǎng)絡一次性輸出。Hydra 同時維護幾何鏈、語義鏈和軌跡鏈再通過圖邊把它們連接起來。Object--Room 圖Room 鄰接跨層錨定可配置錨定上游位姿 / 里程計局部重建深度圖或點云像素或點語義TSDF / 語義體素度量-語義 MeshESDF / GVDObjectsMesh PlacesPlacesAgentsFrontiersRoomsBuildingNeural Tree 房間類別這張圖中有幾個必須從一開始就明確的邊界。**第一機器人位姿通常來自上游定位系統(tǒng)。**Hydra 可以接收視覺慣性里程計、激光里程計、融合定位或數(shù)據(jù)集真值提供的位姿再用它把觀測放進共同坐標系。Hydra 的核心職責不是替代所有前端定位算法。**第二Objects 依賴語義輸入。**深度只能說明表面在哪里不能自己產(chǎn)生chair、table等類別。語義通常來自二維或三維感知網(wǎng)絡再由 Hydra 融合和聚類。**第三Rooms 的實例邊界和語義類別是兩步。**Room Finder幾何房間提取器根據(jù) Places 形成 Room 實例2024 論文再用 Neural Tree神經(jīng)樹推斷office、bedroom等類別。Neural Tree 先把 Object–Room 圖組織成 H-tree 計算結構再進行可學習的圖消息傳遞。當前核心 C 倉庫創(chuàng)建幾何 Room 時先寫占位標簽房間分類網(wǎng)絡另在 Hydra-GNN 發(fā)布。**第四Building 在當前 C 路徑中是單一聚合根。**它把當前 Rooms 連接到B0不是一套能夠在室外地圖中自動分割多棟建筑的算法。5. 在線流水線為什么分成多個模塊如果所有步驟都在每一幀串行處理整張地圖計算量會隨探索范圍不斷增長。Hydra 把快速局部更新和較慢全局推理分開并通過增量消息和圖快照銜接?；丨h(huán)檢測Loop Closure DetectionLCD負責識別當前區(qū)域是否與歷史區(qū)域重合。輸入與位姿同步Active Window / ReconstructionGraph BuilderBackend回環(huán)檢測 LCD優(yōu)化后的全局 DSG 與 Mesh5.1 輸入與局部重建回答“這一幀改變了哪里”輸入端把帶時間戳的位姿、深度或點云、顏色和語義組織成觀測包。局部重建只更新當前觀測影響的體素 block一個 block 是按固定尺寸組合在一起、可整體分配和更新的一組體素。模塊隨后輸出發(fā)生變化或離開活動范圍的增量。這里的active window活動窗口不是一張獨立地圖也不等于相機當前視錐。它是一種生命周期和計算預算策略機器人附近的稠密塊繼續(xù)參與實時融合較遠的歷史塊可以歸檔也就是停止局部高頻更新但保留必要歷史結果稀疏高層節(jié)點仍可留在全局場景圖中。5.2 Graph Builder 回答“這些變化對應哪些圖實體”前端 Graph Builder 消費局部重建輸出增量更新 Mesh、Objects、Places、Frontiers、Mesh Places 和 Agents并建立必要的跨層關系。它追求低延遲因此主要處理本輪變化而不是反復從空圖重建全部層。一輪前端提交帶有時間戳和遞增序號。序號表示“這是第幾版完整前端更新”時間戳表示“觀測發(fā)生在什么時候”兩者用途不同。5.3 Loop Closure 回答“這里以前是否來過”回環(huán)檢測模塊從已歸檔的歷史子圖和當前查詢位置生成描述子先檢索候選再通過三維對應和魯棒配準驗證幾何一致性。描述子相似只能說明“可能是同一地點”只有通過幾何驗證的結果才適合成為后端約束。5.4 Backend 回答“怎樣讓過去和現(xiàn)在重新一致”后端接收前端位姿圖、Mesh 形變圖、回環(huán)約束和共享 DSG 快照。它優(yōu)化 Agent 位姿和形變控制點再校正 Mesh、Places、Objects 等狀態(tài)并合并回環(huán)后重合的重復節(jié)點。Rooms 和 Building 依賴更新后的低層結構重新生成。前端、回環(huán)和后端可以以不同頻率運行因此代碼維護分別服務后端和回環(huán)檢測的共享圖快照。這樣較慢的全局計算不會阻塞每一幀局部重建但消息中的序號必須與圖快照對應避免把某輪增量應用到另一版圖上。6. 為什么“增量”和“層次化”必須同時存在這兩個詞解決的是不同問題。6.1 增量更新控制每輪計算量機器人新觀察到一段走廊時真正變化的通常只是附近一小片體素、Mesh 和圖節(jié)點。增量系統(tǒng)傳播哪些 block 更新了 哪些 Mesh 頂點新增或歸檔了 哪些 GVD 體素成立或失效了 哪些節(jié)點、邊和屬性發(fā)生變化它避免每一幀重新掃描整棟建筑。6.2 層次化表示控制長期存儲和查詢成本即使每輪只處理局部變化機器人長期運行后仍會積累大量幾何。層次化表示把不同任務需要的信息壓縮到合適尺度碰撞與表面檢查使用體素或 Mesh。局部連通查詢使用 Places。對象查詢使用 Objects?？鐓^(qū)域推理使用 Rooms 和 Building。高層不是低層的無損副本。例如一個 Place 只能摘要一簇自由空間骨架證據(jù)不能恢復所有原始深度像素。這個有意的抽象損失換來了更快的搜索和更清晰的關系結構需要精確幾何時系統(tǒng)仍然可以回到底層 Mesh 或體素。7. “Dynamic” 不等于自動跟蹤所有運動物體Hydra 中的“動態(tài)”首先表示地圖狀態(tài)會變化新觀測會新增或更新節(jié)點與邊。節(jié)點可以從 active 變?yōu)?archived也可以因證據(jù)失效而刪除?；丨h(huán)會改變歷史軌跡和地圖位置。重合實體可以合并Rooms 可以按最新 Places 重建。因此archived ! deleted 圖會動態(tài)更新 ! 系統(tǒng)自動完成多目標運動跟蹤當前對象提取主要從累計語義 Mesh 中形成實例并不等同于帶速度狀態(tài)、運動模型和數(shù)據(jù)關聯(lián)的完整動態(tài)物體跟蹤器。動態(tài)類別是否進入靜態(tài)重建還取決于上游語義、掩碼和系統(tǒng)集成策略。8. 場景圖怎樣服務一個具體機器人任務考慮任務去會議室找到打印機并停在打印機旁邊的可通行位置。完整推理可以分為四個尺度。第一步找到目標類別和實例。Neural Tree 或外部房間語義模塊給 Room 提供conference_room類別Object 層提供printer實例。若系統(tǒng)只有幾何 Room 而沒有類別模型就無法僅憑 Room ID 回答“哪個是會議室”。第二步沿跨層關系完成空間落地。printer Object - 附近的 Place - 所屬 Room這把“打印機”從一個類別詞變成世界坐標中的具體實例和可接近空間。第三步在高層和低層分別搜索。Room 圖可以先給出要經(jīng)過的區(qū)域序列Places 圖再提供區(qū)域內部的稀疏連通骨架并用清障屬性幫助過濾過窄連接。第四步交給導航系統(tǒng)生成可執(zhí)行軌跡。Hydra 提供空間知識和拓撲證據(jù)但它不是完整運動規(guī)劃器。機器人半徑、地面坡度、動力學、局部避障和控制約束仍應由導航棧處理。Place 邊存在也不等于任意機器人都能無條件通過。這個例子說明 Hydra 的價值不在于替代 SLAM 或 Navigation而在于把二者之間缺少的實體、語義、拓撲和層級關系組織起來。9. 閱讀當前項目時必須區(qū)分的系統(tǒng)邊界能力完整研究系統(tǒng)中的位置當前倉庫閱讀結論機器人定位上游視覺慣性里程計VIO、激光慣性里程計LIO、融合定位或真值軌跡Hydra 接收位姿不應默認它在本倉庫內完成全部定位語義分割上游語義網(wǎng)絡Hydra 融合語義結果真實模型可由外部semantic_inference等提供在線 ROS 接口Hydra-ROS當前核心倉庫主要提供算法模塊和接口房間幾何實例Hydra Room Finder當前 C 路徑已實現(xiàn)房間語義類別2024 論文 Neural Tree分類網(wǎng)絡單獨在 Hydra-GNN 發(fā)布Room Finder 本身不是分類器回環(huán)后全局校正Hydra Backend 外部形變圖優(yōu)化庫 Kimera-PGMO當前倉庫封裝并使用形變圖與場景圖更新邏輯最終運動規(guī)劃和控制下游 NavigationPlaces 和 Rooms 提供圖結構不直接輸出完整控制軌跡動態(tài)物體跟蹤可擴展的上游/下游能力“Dynamic Scene Graph” 不能直接等同于多目標跟蹤器這張邊界表會貫穿后續(xù)文章。若一項機制來自論文或外部倉庫正文會明確標記而不會因為它屬于“Hydra 系統(tǒng)”就暗示當前目錄內一定存在對應實現(xiàn)。10. 建立一個不會混亂的心智模型可以把 Hydra 記成連續(xù)的三次壓縮和一次全局校正第一次壓縮多幀觀測 - 穩(wěn)定的度量—語義地圖 第二次壓縮稠密幾何 - Objects 和 Places 等稀疏實體 第三次壓縮局部實體 - Rooms 和 Building 等層級結構 全局校正回環(huán)約束 - 軌跡、Mesh 和場景圖重新一致每次壓縮都會丟掉一部分低層細節(jié)卻保留下一類任務真正需要的狀態(tài)。正因為底層幾何和高層圖同時存在機器人才能在“精確到厘米的表面”和“可以用語言查詢的房間與對象”之間切換。下一篇將從第一步開始上游位姿、深度、點云和語義具體怎樣進入同一個坐標系并逐幀形成 TSDF、語義體素、Mesh 和 active window。實現(xiàn)證據(jù)索引正文按概念組織下面只列支撐本篇關鍵結論的實現(xiàn)位置。結論證據(jù)層級位置本倉庫用于實時增量構建三維場景圖當前倉庫說明README.md默認層名和LayerKey當前代碼include/hydra/common/shared_dsg_info.h::SharedDsgInfo::Config前端、后端和 LCD 使用獨立共享圖狀態(tài)當前代碼include/hydra/common/hydra_pipeline.h、src/common/hydra_pipeline.cpp模塊間使用 Backend/LCD/回環(huán)隊列當前代碼include/hydra/common/pipeline_queues.hGraph Builder 更新 Mesh、Places、Frontiers、Objects、Mesh Places 和 Pose Graph當前代碼src/frontend/graph_builder.cpp::GraphBuilder后端和 LCD 接收同一輪前端序號對應的圖快照當前代碼src/frontend/graph_builder.cpp::GraphBuilder::spinOnceRoom 分類使用 Neural Tree2024 論文設計doc/paper_translation/hydra_foundations_2024/hydra_foundations_2024_zh.html第 3.4.2 節(jié)Room 分類網(wǎng)絡獨立發(fā)布當前倉庫說明README.md中 2024-06-26 更新與 Hydra-GNN 鏈接C Room Finder 創(chuàng)建幾何 Room 時初始化占位標簽當前代碼src/rooms/room_finder.cpp::RoomFinder::makeRoomLayerBuilding 當前創(chuàng)建固定B0聚合根當前代碼src/backend/update_buildings_functor.cpp