景坐標(biāo)回歸：讓定位系統(tǒng)“知道自己不知道”)
自動(dòng)駕駛和機(jī)器人定位一直有一個(gè)很反直覺的現(xiàn)象大多數(shù)系統(tǒng)不是在最差的時(shí)候崩潰的而是在“自我感覺良好”的時(shí)候崩潰的。LiDAR 定位看起來比純視覺穩(wěn)定得多點(diǎn)云不會(huì)受光照變化影響幾何結(jié)構(gòu)也更干凈。但真正在停車場(chǎng)、隧道、重復(fù)結(jié)構(gòu)樓道里跑過定位的人都知道LiDAR 也會(huì)漂也會(huì)匹配錯(cuò)而且錯(cuò)誤往往沒有任何預(yù)警——優(yōu)化器照樣輸出一個(gè)置信度很高的位姿直到下游任務(wù)跟著翻車。UQ-Loc 這個(gè)工作值得關(guān)注的真正原因不在于它又提出了一種更準(zhǔn)的 LiDAR 定位方法而在于它把“不確定性估計(jì)”這件事放到了 LiDAR 場(chǎng)景坐標(biāo)回歸Scene Coordinate RegressionSCR的框架里。換句話說它讓每一次定位輸出都能附帶一個(gè)可靠度評(píng)分系統(tǒng)終于可以知道自己“什么時(shí)候不知道”了。這篇文章會(huì)從 SCR 的前世今生講起拆解 UQ-Loc 的思路、不確定性建模的兩種來源、訓(xùn)練與驗(yàn)證的完整流程以及要想把這套方法真正用進(jìn)生產(chǎn)系統(tǒng)你會(huì)踩到的那些坑。什么樣的讀者適合讀這篇文章正在做 LiDAR SLAM 或重定位的算法工程師研究視覺/點(diǎn)云場(chǎng)景坐標(biāo)回歸的碩博生以及想給多傳感器定位系統(tǒng)增加安全邊界的開發(fā)同學(xué)。你不需要提前讀過原論文只需要對(duì)點(diǎn)位姿估計(jì)、相機(jī)/雷達(dá)投影變換有基本概念剩下的我會(huì)逐步展開。1. 這篇文章真正要解決的問題先拋開論文本身聊一個(gè)更寬泛的問題為什么我們其實(shí)很需要“帶不確定性的定位”傳統(tǒng) LiDAR 定位的主流做法是 ICP、NDT 或者基于特征匹配的配準(zhǔn)。這類方法有一個(gè)共同點(diǎn)它們輸出一個(gè)位姿同時(shí)也會(huì)輸出一個(gè)“分?jǐn)?shù)”比如 ICP 的 fitness score、NDT 的變換協(xié)方差。麻煩在于這個(gè)分?jǐn)?shù)和真正的定位誤差之間并不總是單調(diào)相關(guān)。一個(gè)在對(duì)稱走廊里匹配錯(cuò)誤的位姿可能 fitness score 看起來相當(dāng)不錯(cuò)因?yàn)閮蛇叺膲υ趲缀紊暇褪菐缀跸嗤?。再往深處想一步。定位系統(tǒng)往往不是孤立工作的。下游的路徑規(guī)劃、避障、地圖更新、傳感器融合都要拿這個(gè)位姿當(dāng)輸入。如果定位模塊每次輸出位姿的同時(shí)還能輸出一個(gè)誠實(shí)的不確定性估計(jì)——這次定位的誤差可能有多大——下游系統(tǒng)就可以做很多事在不確定性過高時(shí)降低速度、切換定位模式、觸發(fā)重新初始化、或者在后端融合時(shí)自動(dòng)降低該位姿的權(quán)重。這就是 UQ-Loc 這類工作的意義它不一定讓你在 benchmark 上刷出驚人的精度提升但它能讓定位系統(tǒng)從“自信地犯錯(cuò)”變成“知道自己可能犯錯(cuò)”。從論文標(biāo)題來看UQ-Loc 的三個(gè)關(guān)鍵詞可以這樣拆LiDAR輸入是激光雷達(dá)點(diǎn)云而不是圖像。Scene Coordinate Regression不直接回歸位姿而是先為每個(gè)點(diǎn)回歸三維場(chǎng)景坐標(biāo)再求解位姿。Uncertainty-Aware在回歸過程中同時(shí)估計(jì)不確定性并用不確定性來優(yōu)化定位結(jié)果。這類工作的價(jià)值不能只看位姿精度指標(biāo)還要看它能不能真正提升系統(tǒng)的安全性和魯棒性。2. 場(chǎng)景坐標(biāo)回歸從視覺定位到 LiDAR 重定位2.1 什么是場(chǎng)景坐標(biāo)回歸場(chǎng)景坐標(biāo)回歸Scene Coordinate RegressionSCR最早是在視覺定位領(lǐng)域發(fā)展起來的。它的核心思想很樸素給定一幀圖像對(duì)圖像中的每個(gè)像素直接回歸出該像素對(duì)應(yīng)表面點(diǎn)在世界坐標(biāo)系下的三維坐標(biāo)。有了足夠多這樣的 2D-3D 對(duì)應(yīng)關(guān)系就可以用 PnP 或 RANSAC 求解出相機(jī)在世界坐標(biāo)系中的位姿。通俗解釋傳統(tǒng)特征匹配需要先在圖像和三維地圖之間建立特征描述子對(duì)應(yīng)再做 PnPSCR 則把這個(gè)過程壓縮成一個(gè)端到端的回歸問題——我不管這個(gè)像素長什么樣我直接“猜”它在世界里的位置。這個(gè)思路相比傳統(tǒng)方法有幾個(gè)明顯優(yōu)勢(shì)不需要存儲(chǔ)大量特征描述子地圖可以做得更緊湊。端到端訓(xùn)練特征學(xué)習(xí)和位姿求解可以聯(lián)合優(yōu)化。在紋理缺失或重復(fù)紋理場(chǎng)景中比基于局部特征的方法更魯棒。DSAC* 等一系列工作把 SCR 推到了視覺定位的第一梯隊(duì)。這個(gè)路線的基本范式是卷積網(wǎng)絡(luò)回歸場(chǎng)景坐標(biāo) 可微 PnP/RANSAC 模塊求位姿 端到端損失傳播。2.2 從視覺轉(zhuǎn)到 LiDAR難點(diǎn)在哪視覺 SCR 的輸入是 RGB 圖像每個(gè)像素都有明確的規(guī)則網(wǎng)格結(jié)構(gòu)。LiDAR 點(diǎn)云則完全不同稀疏性一幀 64 線雷達(dá)大約十多萬個(gè)點(diǎn)但分布極不均勻近處密遠(yuǎn)處稀。無序性點(diǎn)云沒有固定的排列順序。幾何退化在隧道或長走廊中某些維度如沿走廊方向的平移在幾何上不可觀。外觀信息少激光雷達(dá)沒有豐富的紋理信息雖然強(qiáng)度值可以提供一些幫助但遠(yuǎn)不如 RGB 信息豐富。所以 LiDAR SCR 不能直接把視覺的網(wǎng)絡(luò)結(jié)構(gòu)搬過來用。它需要處理點(diǎn)云的稀疏與無序特性常見的做法包括體素化、PointNet 這類集合提取結(jié)構(gòu)、或者將點(diǎn)云投影到距離圖range image上做稠密預(yù)測(cè)。UQ-Loc 的賣點(diǎn)在于它不光做這個(gè)回歸還在回歸的同時(shí)估計(jì)每個(gè)點(diǎn)的不確定性并把不確定性信息用于最終的位姿優(yōu)化——這正是從視覺方法轉(zhuǎn)向 LiDAR 場(chǎng)景時(shí)容易被忽略的問題。2.3 SCR 與傳統(tǒng)定位方法的對(duì)比方法類型核心思想優(yōu)勢(shì)劣勢(shì)ICP / NDT 配準(zhǔn)迭代優(yōu)化源點(diǎn)云與目標(biāo)點(diǎn)云的幾何對(duì)齊精度高通用性強(qiáng)需要較好的初始位姿對(duì)稱場(chǎng)景易退化特征匹配 PnP提取關(guān)鍵點(diǎn)描述子匹配地圖點(diǎn)計(jì)算位姿可處理大范圍重定位依賴特征質(zhì)量弱紋理場(chǎng)景易失敗位姿回歸網(wǎng)絡(luò)直接回歸 6-DoF 位姿輕量推理快精度低泛化能力弱場(chǎng)景坐標(biāo)回歸回歸每個(gè)點(diǎn)的全局三維坐標(biāo)再求解位姿精度較高地圖緊湊可與 PnP 結(jié)合需要模型訓(xùn)練場(chǎng)景變化需要重新訓(xùn)練不確定性 SCRSCR 不確定性輸出具備誤差預(yù)警能力可輔助失敗恢復(fù)訓(xùn)練復(fù)雜度更高評(píng)估維度更多從這張表能看出來UQ-Loc 不是把上一行全部推翻而是在 SCR 這個(gè)已經(jīng)有較好精度基礎(chǔ)的框架上增加了一個(gè)最關(guān)鍵的維度風(fēng)險(xiǎn)感知。3. UQ-Loc 的核心思路拆解下面開始拆 UQ-Loc 的模型設(shè)計(jì)。需要說明的是不同論文對(duì)“不確定性感知”的具體實(shí)現(xiàn)差異較大這里重點(diǎn)講通用的方法論框架這樣無論你看的是哪篇具體論文都能快速對(duì)上號(hào)。3.1 不確定性從哪里來偶然不確定性與認(rèn)知不確定性幾乎所有帶不確定性估計(jì)的深度學(xué)習(xí)工作都會(huì)把不確定性分成兩類偶然不確定性Aleatoric Uncertainty來自觀測(cè)數(shù)據(jù)本身的噪聲。比如激光雷達(dá)測(cè)距誤差、標(biāo)定誤差、動(dòng)態(tài)物體的遮擋導(dǎo)致的測(cè)量不完整。這類不確定性無法通過增加訓(xùn)練數(shù)據(jù)消除只能估計(jì)它的大小并在推理時(shí)考慮進(jìn)去。認(rèn)知不確定性Epistemic Uncertainty來自模型的“知識(shí)盲區(qū)”。比如你訓(xùn)練集里全是室外道路場(chǎng)景突然來一個(gè)地下車庫模型在這個(gè)分布外輸入上就不知道該怎么辦。增加訓(xùn)練數(shù)據(jù)、增加模型容量、或者使用測(cè)試時(shí)增強(qiáng)可以降低認(rèn)知不確定性。UQ-Loc 這類工作在設(shè)計(jì)時(shí)通常會(huì)把這兩種不確定性分別建模偶然不確定性通過網(wǎng)絡(luò)的第二個(gè)輸出頭直接回歸出來比如為每個(gè)點(diǎn)的場(chǎng)景坐標(biāo)預(yù)測(cè)加上一個(gè)方差。認(rèn)知不確定性常見做法是用 MC Dropout 或 Deep Ensemble 來近似在推理時(shí)多次前向傳播觀察輸出的一致性。如果你只想落地一個(gè)最簡(jiǎn)版本先從偶然不確定性開始就夠用它的訓(xùn)練損失函數(shù)天然支持方差的學(xué)習(xí)工程成本也要小得多。3.2 共享主干 雙分支輸出UQ-Loc 的典型結(jié)構(gòu)可以概括為一個(gè)點(diǎn)云特征提取主干PointNet、稀疏卷積或范圍圖 CNN一個(gè)場(chǎng)景坐標(biāo)回歸分支輸出每個(gè)點(diǎn)/像素的全局三維坐標(biāo)一個(gè)不確定性回歸分支輸出對(duì)應(yīng)每個(gè)坐標(biāo)的不確定性方差或置信度。兩個(gè)分支共享同一個(gè)特征提取網(wǎng)絡(luò)。這樣做的好處是特征提取部分學(xué)習(xí)的幾何和語義信息既為坐標(biāo)回歸服務(wù)也為不確定性估計(jì)服務(wù)。在推理階段不確定性分支并不需要額外的計(jì)算量所以這個(gè)設(shè)計(jì)幾乎是免費(fèi)的。用代碼邏輯表示推理時(shí)的偽代碼如下# 偽代碼UQ-Loc 風(fēng)格推理流程示意 # 實(shí)際網(wǎng)絡(luò)結(jié)構(gòu)以論文實(shí)現(xiàn)為準(zhǔn) def forward(point_cloud): # 輸入一幀 LiDAR 點(diǎn)云形狀 [N, 3C] # N 為點(diǎn)數(shù)C 為額外特征強(qiáng)度、時(shí)間戳等 features backbone(point_cloud) # 點(diǎn)云特征提取 scene_coords coord_head(features) # 每個(gè)點(diǎn)的全局場(chǎng)景坐標(biāo) [N, 3] uncertainty unc_head(features) # 每個(gè)點(diǎn)的不確定性 [N, 1] return scene_coords, uncertainty這里最關(guān)鍵的設(shè)計(jì)決策是不確定性分支的輸出應(yīng)該是什么形式常見選擇有兩種輸出方差variance在損失函數(shù)中參與加權(quán)輸出置信度confidence在位姿求解時(shí)作為 RANSAC 的樣本權(quán)重。前者訓(xùn)練更穩(wěn)定后者和 PnP/RANSAC 的結(jié)合更直覺。UQ-Loc 這種工作一般會(huì)想辦法讓兩者在一個(gè)統(tǒng)一框架里工作。3.3 損失函數(shù)讓不確定性學(xué)會(huì)“誠實(shí)”不確定性估計(jì)最難的一點(diǎn)是它沒有直接的監(jiān)督標(biāo)簽。我們并不知道每個(gè)點(diǎn)真實(shí)的方差是多少那該怎么辦答案是利用似然函數(shù)來間接監(jiān)督。一個(gè)經(jīng)典的思路是假設(shè)場(chǎng)景坐標(biāo)預(yù)測(cè)服從高斯分布均值是預(yù)測(cè)的坐標(biāo)方差是不確定性分支的輸出。那么我們訓(xùn)練的目標(biāo)就是最大化這個(gè)高斯分布的似然等價(jià)于最小化如下所示的負(fù)對(duì)數(shù)似然損失loss 0.5 * ( (y_pred - y_true)^2 / sigma^2 log(sigma^2) )其中sigma^2是不確定性分支輸出的方差。這個(gè)損失函數(shù)的妙處在于當(dāng)預(yù)測(cè)誤差很大時(shí)模型會(huì)增大sigma來降低第一項(xiàng)但第二項(xiàng)會(huì)阻止sigma無限增大當(dāng)預(yù)測(cè)誤差很小時(shí)模型會(huì)減小sigma并保持它以降低第二項(xiàng)。這樣訓(xùn)練出來的不確定性可以理解為模型對(duì)自身預(yù)測(cè)誤差的“誠實(shí)估計(jì)”。誤差大的位置不確定性高誤差小的位置不確定性低。在實(shí)際工程中還需要注意如果不確定性分支輸出的是log(sigma^2)而不是直接輸出sigma^2訓(xùn)練數(shù)值會(huì)更加穩(wěn)定因?yàn)楸苊饬朔讲畋仨毞秦?fù)的約束帶來的優(yōu)化困難。3.4 位姿求解階段如何使用不確定性有了每個(gè)點(diǎn)的不確定性后續(xù)的位姿求解可以有兩種用法。一種是用不確定性作為 RANSAC 的權(quán)重。傳統(tǒng) RANSAC 在采樣時(shí)對(duì)所有點(diǎn)一視同仁但如果我們知道某些點(diǎn)的預(yù)測(cè)很不確定就可以降低它們被采樣的概率從而提高采樣效率減少迭代次數(shù)。另一種是用不確定性做加權(quán) PnP。在求解最小二乘問題時(shí)每個(gè) 2D-3D 對(duì)應(yīng)的殘差乘以一個(gè)權(quán)重權(quán)重由不確定性決定。這樣不確定度高的點(diǎn)在優(yōu)化中的影響被自動(dòng)壓低最終的位姿精度會(huì)更高。定位流程對(duì)比 無不確定性感知 原始點(diǎn)云 → 回歸場(chǎng)景坐標(biāo) → 均勻權(quán)重 PnP/RANSAC → 一次性位姿輸出 不確定性感知 原始點(diǎn)云 → 回歸場(chǎng)景坐標(biāo) 不確定性 → 加權(quán) PnP/RANSAC → 位姿 不確定性估計(jì)這個(gè)差異在表面上看只是多了一個(gè)分支實(shí)際上是把“系統(tǒng)是否可信”這個(gè)原本要靠事后閾值判斷的問題提前合并到了模型的推理過程中。4. 不確定性感知到底能改變什么很多人看到“不確定性”三個(gè)字第一反應(yīng)是“這個(gè)東西又不能直接提升精度有什么實(shí)際價(jià)值”這個(gè)判斷是不完整的。不確定性感知的核心價(jià)值在于讓定位系統(tǒng)具備了自我評(píng)估的能力。拿自動(dòng)駕駛場(chǎng)景來說如果定位模塊給出的位姿不確定度突然升高說明當(dāng)前環(huán)境可能進(jìn)入了結(jié)構(gòu)退化區(qū)域、動(dòng)態(tài)遮擋嚴(yán)重區(qū)域或者傳感器數(shù)據(jù)質(zhì)量在下降。這時(shí)候系統(tǒng)可以選擇降低車速、切換定位策略、請(qǐng)求重新初始化而不是繼續(xù)以高置信度往前沖。具體到工程系統(tǒng)不確定性輸出可以在至少三個(gè)環(huán)節(jié)被使用第一后端融合的權(quán)重調(diào)節(jié)。在多傳感器融合系統(tǒng)中GPS、IMU、輪速計(jì)、LiDAR 定位各自的可靠性是動(dòng)態(tài)變化的。傳統(tǒng)做法是配置一個(gè)固定的噪聲協(xié)方差。如果 LiDAR 定位模塊能輸出逐幀的不確定性就可以將這個(gè)不確定性動(dòng)態(tài)映射到融合權(quán)重中實(shí)現(xiàn)更合理的多傳感器融合。這也正好呼應(yīng)了當(dāng)前業(yè)內(nèi)非常關(guān)注的“針對(duì) camera / lidar / imu / gps 四類傳感器的專屬質(zhì)量評(píng)估指標(biāo)”這一趨勢(shì)——不確定性估計(jì)正是評(píng)估感知模塊輸出質(zhì)量的關(guān)鍵技術(shù)手段之一。第二失敗檢測(cè)與自動(dòng)恢復(fù)。定位系統(tǒng)最怕的不是失敗而是失敗了還不自知。不確定性估計(jì)算法可以提供一個(gè)天然的失敗檢測(cè)信號(hào)。設(shè)定一個(gè)閾值當(dāng)不確定性超過閾值時(shí)觸發(fā)重定位或地圖切換流程就能避免系統(tǒng)帶著錯(cuò)誤位姿長時(shí)間運(yùn)行。第三動(dòng)態(tài)場(chǎng)景過濾。在動(dòng)態(tài)物體行人、車輛占據(jù)的區(qū)域場(chǎng)景坐標(biāo)回歸的誤差通常很大不確定性也會(huì)相應(yīng)升高。利用不確定性信息可以在位姿求解前過濾掉這些不可靠的點(diǎn)相當(dāng)于給點(diǎn)云做了一次面向定位任務(wù)的動(dòng)態(tài)物體去除。從架構(gòu)層面看不確定性感知改變的并不是某一個(gè)模塊的算法而是定位系統(tǒng)對(duì)待自身輸出的態(tài)度從每次輸出一個(gè)唯一答案到輸出答案并且告訴你這個(gè)答案值不值得信賴。5. 實(shí)驗(yàn)設(shè)計(jì)與評(píng)估指標(biāo)怎么證明“不確定性”是有效的一個(gè)很實(shí)際的問題是如果我們要復(fù)現(xiàn)或者評(píng)估這類方法除了看傳統(tǒng)的定位精度還需要關(guān)心什么5.1 定位精度指標(biāo)LiDAR 定位精度通常使用兩類指標(biāo)ATE絕對(duì)軌跡誤差衡量估計(jì)軌跡與真值軌跡的整體偏差。RPE相對(duì)位姿誤差衡量相鄰幀之間的相對(duì)位姿誤差。這兩個(gè)指標(biāo)在任何 SLAM / 定位論文中都是標(biāo)配UQ-Loc 這類工作也會(huì)報(bào)告。但只看這兩個(gè)指標(biāo)無法回答“不確定性估計(jì)到底準(zhǔn)不準(zhǔn)”的問題。5.2 不確定性質(zhì)量指標(biāo)比精度更重要的驗(yàn)證維度假設(shè)一個(gè)模型預(yù)測(cè)出了不確定性sigma那怎么判斷它好不好核心標(biāo)準(zhǔn)是預(yù)測(cè)的不確定性和實(shí)際誤差是不是匹配。常用的指標(biāo)包括NLL負(fù)對(duì)數(shù)似然衡量模型預(yù)測(cè)分布和真實(shí)標(biāo)簽的擬合程度越低越好。ECE期望校準(zhǔn)誤差把預(yù)測(cè)不確定性分桶比較桶內(nèi)平均不確定性和實(shí)際誤差的一致性越低說明校準(zhǔn)越好。AUSE面積下的不確定性誤差對(duì)不同不確定性閾值計(jì)算“錯(cuò)誤被接受的比例”繪制曲線后積分。AURRE面積下的拒絕誤差在不同拒絕比例下計(jì)算平均定位誤差可以直觀看到“過濾掉高不確定性樣本后精度能提升多少”。這里的工程含義是一個(gè)良好的不確定性估計(jì)應(yīng)該能做到“當(dāng)系統(tǒng)說這次定位不可信時(shí)這次定位真的更可能出錯(cuò)”。如果不確定性輸出和真實(shí)誤差完全不相關(guān)那這個(gè)輸出對(duì)下游來說就是一個(gè)噪音。5.3 設(shè)計(jì)評(píng)測(cè)場(chǎng)景的注意事項(xiàng)評(píng)估不確定性算法比評(píng)估普通定位算法更要小心場(chǎng)景設(shè)計(jì)。建議準(zhǔn)備三類測(cè)試集正常場(chǎng)景常規(guī)道路、公開數(shù)據(jù)集上的標(biāo)準(zhǔn)序列。退化場(chǎng)景長走廊、隧道、停車場(chǎng)、空曠區(qū)域。動(dòng)態(tài)場(chǎng)景車流密集、行人混雜的環(huán)境。退化場(chǎng)景和動(dòng)態(tài)場(chǎng)景恰恰是不確定性估計(jì)發(fā)揮最大價(jià)值的地方。如果在這些場(chǎng)景下模型的不確定性輸出能夠顯著高于正常場(chǎng)景并且過濾掉高不確定性幀之后定位誤差明顯下降那基本可以判斷這套不確定性機(jī)制是有效的。6. 一個(gè)最小可驗(yàn)證的實(shí)驗(yàn)流程如果你打算在項(xiàng)目里試著引入類似 UQ-Loc 的方案可以參考下面的思路跑通一個(gè)最小閉環(huán)實(shí)驗(yàn)。6.1 數(shù)據(jù)準(zhǔn)備階段你需要準(zhǔn)備一個(gè)包含 LiDAR 點(diǎn)云、真值位姿和三維地圖的數(shù)據(jù)集。如果是自采數(shù)據(jù)需要先完成 LiDAR 的位姿標(biāo)定也就是通常說的 LiDAR SLAM 或 LiDAR-Inertial 建圖。這一步非常關(guān)鍵因?yàn)閳?chǎng)景坐標(biāo)回歸的訓(xùn)練標(biāo)簽就是“每個(gè)點(diǎn)在地圖坐標(biāo)系下的真實(shí)三維坐標(biāo)”標(biāo)簽不準(zhǔn)整個(gè)模型的上限就被卡死了。這里特別提醒一個(gè)容易出現(xiàn)問題的環(huán)節(jié)LiDAR 外參標(biāo)定。場(chǎng)景坐標(biāo)回歸需要把每幀點(diǎn)云從傳感器坐標(biāo)系變換到地圖坐標(biāo)系如果 LiDAR 相對(duì)車體的外參不準(zhǔn)確點(diǎn)云和地圖的對(duì)齊誤差會(huì)直接變成訓(xùn)練標(biāo)簽的誤差。工程上建議在建圖之前先做一次完整的 LiDAR-IMU-Camera 聯(lián)合標(biāo)定并建立標(biāo)定參數(shù)的版本管理機(jī)制。6.2 網(wǎng)絡(luò)訓(xùn)練流程一個(gè)最小訓(xùn)練流程可以這樣組織# 偽代碼訓(xùn)練循環(huán)核心邏輯 import torch # 假設(shè)已經(jīng)構(gòu)建好模型和數(shù)據(jù)加載器 model UQLocModel( backbonepointnet2, coord_dim3, uncertainty_modeheteroscedastic ) optimizer torch.optim.Adam(model.parameters(), lr1e-4) for epoch in range(args.epochs): for batch_points, batch_coords_true in dataloader: batch_points batch_points.to(device) batch_coords_true batch_coords_true.to(device) coords_pred, log_var model(batch_points) # 負(fù)對(duì)數(shù)似然損失帶不確定性加權(quán) precision torch.exp(-log_var) loss torch.mean(precision * (coords_pred - batch_coords_true) ** 2) loss torch.mean(log_var) # 正則項(xiàng)防止方差無限增大 optimizer.zero_grad() loss.backward() optimizer.step()這個(gè)代碼的核心邏輯就是前文提到的負(fù)對(duì)數(shù)似然損失。注意這里用的是log_var而不是直接輸出方差這是數(shù)值穩(wěn)定性的關(guān)鍵。6.3 后處理與位姿求解訓(xùn)練完場(chǎng)景坐標(biāo)回歸器后在位姿求解階段可以按如下方式結(jié)合不確定性# 偽代碼加權(quán) PnP 流示意 # 省略了 RANSAC 細(xì)節(jié)只展示加權(quán)核心思想 import numpy as np def solve_pose_with_uncertainty(scene_coords, uncertainties, keypoint_indices): # scene_coords: 模型預(yù)測(cè)的全局場(chǎng)景坐標(biāo) # uncertainties: 對(duì)應(yīng)每個(gè)預(yù)測(cè)點(diǎn)的方差 # keypoint_indices: 通過 RANSAC 采樣選中的內(nèi)點(diǎn)下標(biāo) selected_coords scene_coords[keypoint_indices] selected_weights 1.0 / uncertainties[keypoint_indices] # 方差異常處理可加 epsilon # 用加權(quán)最小二乘求解位姿示意 # 這類問題建議使用成熟 PnP 庫或?qū)崿F(xiàn)加權(quán)版本的 P3P/PnP 求解器 pose weighted_pnp( points_2dselected_coords, points_3dselected_coords, weightsselected_weights ) return pose這里面的關(guān)鍵處理是不確定度越高的點(diǎn)其權(quán)重越低?？梢杂行У挚箞?chǎng)景坐標(biāo)回歸中少量外點(diǎn)對(duì)位姿求解的干擾。6.4 運(yùn)行與驗(yàn)證跑完一套流程后建議至少輸出以下統(tǒng)計(jì)信息ATE / RPE驗(yàn)證定位精度。不確定性估計(jì)值和定位誤差之間的相關(guān)系數(shù)。設(shè)置不同不確定度閾值時(shí)的定位誤差變化曲線。如果看到的現(xiàn)象是過濾掉高不確定度的幀之后定位誤差明顯下降那就說明不確定性輸出是有信息量的。這也是 UQ-Loc 這類方法最值得關(guān)注的驗(yàn)證結(jié)果。7. 常見問題與排查思路圍繞不確定性感知 LiDAR 定位結(jié)合我過去定位方向的經(jīng)驗(yàn)整理出下面幾個(gè)高頻問題供大家快速排查。問題現(xiàn)象可能原因排查方式解決方案不確定性輸出全部接近幾乎沒有區(qū)分度訓(xùn)練數(shù)據(jù)場(chǎng)景單一模型沒有見過難度不同的樣本檢查訓(xùn)練集中退化場(chǎng)景和動(dòng)態(tài)場(chǎng)景的占比增加不同難度的場(chǎng)景尤其是長走廊、停車場(chǎng)場(chǎng)景坐標(biāo)回歸誤差很大模型不收斂訓(xùn)練標(biāo)簽地圖坐標(biāo)本身存在錯(cuò)誤可視化一部分點(diǎn)云和地圖的疊加檢查對(duì)齊情況重新做建圖和 LiDAR 外參標(biāo)定校驗(yàn)標(biāo)簽生成流程不確定性估計(jì)與定位誤差不相關(guān)不確定性分支只用 NLL 訓(xùn)練但未參與位姿求解檢查推理時(shí)是否使用了不確定性信息在位姿求解階段引入不確定性權(quán)重對(duì)稱環(huán)境下位姿錯(cuò)配但不確定性不高模型在訓(xùn)練時(shí)沒有見過這種對(duì)稱場(chǎng)景單獨(dú)構(gòu)造對(duì)稱場(chǎng)景測(cè)試集引入結(jié)構(gòu)退化感知的損失項(xiàng)或數(shù)據(jù)增強(qiáng)推理速度較慢不確定性分支增加了額外計(jì)算分析每個(gè)模塊耗時(shí)考慮用輕量級(jí)不確定性頭或只在定位結(jié)果下發(fā)時(shí)計(jì)算不確定性訓(xùn)練時(shí) loss 出現(xiàn) NaN方差出現(xiàn)極小值導(dǎo)致除零檢查 log_var 的輸出范圍和數(shù)值穩(wěn)定性給分母加 epsilon或約束 log_var 的上下界這里的前兩個(gè)問題最容易出現(xiàn)在自采數(shù)據(jù)集上。很多時(shí)候模型訓(xùn)不好不是網(wǎng)絡(luò)結(jié)構(gòu)不夠好而是地圖建得不夠好。8. 工程化落地的幾條建議把這類算法從論文帶到實(shí)際系統(tǒng)有幾點(diǎn)經(jīng)驗(yàn)值得分享。第一地圖資產(chǎn)和傳感器標(biāo)定要分開管理。UQ-Loc 這類方法強(qiáng)依賴三維地圖的質(zhì)量而地圖的質(zhì)量又強(qiáng)依賴 LiDAR 建圖時(shí)的外參、回環(huán)糾正、點(diǎn)云去畸變等環(huán)節(jié)。建議把標(biāo)定參數(shù)、地圖版本、算法權(quán)重三者納入獨(dú)立的版本管理避免某個(gè)環(huán)節(jié)變更后難以回溯。第二別丟掉傳統(tǒng)定位方法。“不確定性 SCR”和傳統(tǒng) ICP/NDT 不是替代關(guān)系而是互補(bǔ)關(guān)系。在工程上更穩(wěn)妥的做法是用 SCR 做全局重定位的初始猜測(cè)再用 ICP/NDT 精配準(zhǔn)最后用 SCR 輸出的不確定性來決定這個(gè)流程是否可信。這樣即兼容了 SCR 的重定位能力和傳統(tǒng)精配準(zhǔn)的高精度。第三先做好不確定性校準(zhǔn)再談部署。模型訓(xùn)練完后不確定性輸出的絕對(duì)值通常不能直接當(dāng)作真實(shí)方差使用。需要通過一段帶有真值的序列做校準(zhǔn)得到不確定性到真實(shí)誤差的映射關(guān)系。這一步不做你很難在下游安全邏輯里設(shè)定一個(gè)合適的閾值。第四評(píng)估鏈路要覆蓋退化場(chǎng)景。很多團(tuán)隊(duì)在測(cè)試時(shí)只跑 normal 場(chǎng)景得到的結(jié)論是“模型挺準(zhǔn)的”但這掩蓋了退化場(chǎng)景下的風(fēng)險(xiǎn)。建議把退化場(chǎng)景自動(dòng)化地加入回歸測(cè)試集每次模型更新都要跑一遍防止“精度提升但魯棒性下降”的隱性回歸。第五關(guān)注多傳感器質(zhì)量評(píng)估體系。如果系統(tǒng)里同時(shí)有 camera、LiDAR、IMU、GPS建議為每一類傳感器都設(shè)計(jì)獨(dú)立的質(zhì)量評(píng)估指標(biāo)再在融合層把 LiDAR 定位的不確定性作為其中一個(gè)輸入。不確定性感知和傳感器質(zhì)量評(píng)估本質(zhì)上是一件事的兩個(gè)側(cè)面一個(gè)是讓每個(gè)傳感器模塊“誠實(shí)匯報(bào)”另一個(gè)是讓融合中心“合理信任”。9. 總結(jié)與后續(xù)方向UQ-Loc 的核心思想可以濃縮成一句話LiDAR 場(chǎng)景坐標(biāo)回歸不僅要回答“我在哪里”還要回答“我有多確定”。它把不確定性估計(jì)融入到從點(diǎn)云特征提取、場(chǎng)景坐標(biāo)回歸到位姿求解的完整鏈路中讓定位系統(tǒng)第一次有機(jī)會(huì)在“自信地犯錯(cuò)”之前向系統(tǒng)發(fā)出預(yù)警。如果你想從這篇文章開始動(dòng)手實(shí)踐建議按這樣的順序來先準(zhǔn)備一套帶真值位姿和三維地圖的點(diǎn)云數(shù)據(jù)確認(rèn)標(biāo)定無誤再實(shí)現(xiàn)一個(gè)最簡(jiǎn)的“共享主干 坐標(biāo)回歸頭 不確定性頭”結(jié)構(gòu)用負(fù)對(duì)數(shù)似然損失訓(xùn)練最后在傳統(tǒng)定位精度之外加上不確定性質(zhì)量的評(píng)估指標(biāo)驗(yàn)證不確定性是否真的有信息量。這個(gè)閉環(huán)跑通之后再討論與現(xiàn)有 SLAM 系統(tǒng)的集成。在更寬的技術(shù)趨勢(shì)里不確定性感知定位是“安全型定位系統(tǒng)”的基石之一。后續(xù)值得深入的方向包括將不確定性估計(jì)與相機(jī)-雷達(dá)深度融合用不確定性來動(dòng)態(tài)調(diào)節(jié)跨傳感器的信任權(quán)重在線的場(chǎng)景坐標(biāo)回歸與地圖更新以及更細(xì)粒度的不確定性分解把動(dòng)態(tài)物體、傳感器噪聲、模型知識(shí)盲區(qū)三類不確定性來源區(qū)分開。這些方向落地難度都不小但每進(jìn)一步機(jī)器對(duì)自身能力邊界的認(rèn)識(shí)就更清晰一層。如果你正在做定位方向的工程或研究建議把這篇文章的思路和你的實(shí)際場(chǎng)景對(duì)照一下看看你的系統(tǒng)在哪些環(huán)節(jié)最容易出現(xiàn)高誤判置信度那個(gè)地方很可能就是不確定性感知應(yīng)該介入的位置。 不確定性感知的LiDAR場(chǎng)景坐標(biāo)回歸：讓定位系統(tǒng)“知道自己不知道”
自動(dòng)駕駛和機(jī)器人定位一直有一個(gè)很反直覺的現(xiàn)象大多數(shù)系統(tǒng)不是在最差的時(shí)候崩潰的而是在“自我感覺良好”的時(shí)候崩潰的。LiDAR 定位看起來比純視覺穩(wěn)定得多點(diǎn)云不會(huì)受光照變化影響幾何結(jié)構(gòu)也更干凈。但真正在停車場(chǎng)、隧道、重復(fù)結(jié)構(gòu)樓道里跑過定位的人都知道LiDAR 也會(huì)漂也會(huì)匹配錯(cuò)而且錯(cuò)誤往往沒有任何預(yù)警——優(yōu)化器照樣輸出一個(gè)置信度很高的位姿直到下游任務(wù)跟著翻車。UQ-Loc 這個(gè)工作值得關(guān)注的真正原因不在于它又提出了一種更準(zhǔn)的 LiDAR 定位方法而在于它把“不確定性估計(jì)”這件事放到了 LiDAR 場(chǎng)景坐標(biāo)回歸Scene Coordinate RegressionSCR的框架里。換句話說它讓每一次定位輸出都能附帶一個(gè)可靠度評(píng)分系統(tǒng)終于可以知道自己“什么時(shí)候不知道”了。這篇文章會(huì)從 SCR 的前世今生講起拆解 UQ-Loc 的思路、不確定性建模的兩種來源、訓(xùn)練與驗(yàn)證的完整流程以及要想把這套方法真正用進(jìn)生產(chǎn)系統(tǒng)你會(huì)踩到的那些坑。什么樣的讀者適合讀這篇文章正在做 LiDAR SLAM 或重定位的算法工程師研究視覺/點(diǎn)云場(chǎng)景坐標(biāo)回歸的碩博生以及想給多傳感器定位系統(tǒng)增加安全邊界的開發(fā)同學(xué)。你不需要提前讀過原論文只需要對(duì)點(diǎn)位姿估計(jì)、相機(jī)/雷達(dá)投影變換有基本概念剩下的我會(huì)逐步展開。1. 這篇文章真正要解決的問題先拋開論文本身聊一個(gè)更寬泛的問題為什么我們其實(shí)很需要“帶不確定性的定位”傳統(tǒng) LiDAR 定位的主流做法是 ICP、NDT 或者基于特征匹配的配準(zhǔn)。這類方法有一個(gè)共同點(diǎn)它們輸出一個(gè)位姿同時(shí)也會(huì)輸出一個(gè)“分?jǐn)?shù)”比如 ICP 的 fitness score、NDT 的變換協(xié)方差。麻煩在于這個(gè)分?jǐn)?shù)和真正的定位誤差之間并不總是單調(diào)相關(guān)。一個(gè)在對(duì)稱走廊里匹配錯(cuò)誤的位姿可能 fitness score 看起來相當(dāng)不錯(cuò)因?yàn)閮蛇叺膲υ趲缀紊暇褪菐缀跸嗤?。再往深處想一步。定位系統(tǒng)往往不是孤立工作的。下游的路徑規(guī)劃、避障、地圖更新、傳感器融合都要拿這個(gè)位姿當(dāng)輸入。如果定位模塊每次輸出位姿的同時(shí)還能輸出一個(gè)誠實(shí)的不確定性估計(jì)——這次定位的誤差可能有多大——下游系統(tǒng)就可以做很多事在不確定性過高時(shí)降低速度、切換定位模式、觸發(fā)重新初始化、或者在后端融合時(shí)自動(dòng)降低該位姿的權(quán)重。這就是 UQ-Loc 這類工作的意義它不一定讓你在 benchmark 上刷出驚人的精度提升但它能讓定位系統(tǒng)從“自信地犯錯(cuò)”變成“知道自己可能犯錯(cuò)”。從論文標(biāo)題來看UQ-Loc 的三個(gè)關(guān)鍵詞可以這樣拆LiDAR輸入是激光雷達(dá)點(diǎn)云而不是圖像。Scene Coordinate Regression不直接回歸位姿而是先為每個(gè)點(diǎn)回歸三維場(chǎng)景坐標(biāo)再求解位姿。Uncertainty-Aware在回歸過程中同時(shí)估計(jì)不確定性并用不確定性來優(yōu)化定位結(jié)果。這類工作的價(jià)值不能只看位姿精度指標(biāo)還要看它能不能真正提升系統(tǒng)的安全性和魯棒性。2. 場(chǎng)景坐標(biāo)回歸從視覺定位到 LiDAR 重定位2.1 什么是場(chǎng)景坐標(biāo)回歸場(chǎng)景坐標(biāo)回歸Scene Coordinate RegressionSCR最早是在視覺定位領(lǐng)域發(fā)展起來的。它的核心思想很樸素給定一幀圖像對(duì)圖像中的每個(gè)像素直接回歸出該像素對(duì)應(yīng)表面點(diǎn)在世界坐標(biāo)系下的三維坐標(biāo)。有了足夠多這樣的 2D-3D 對(duì)應(yīng)關(guān)系就可以用 PnP 或 RANSAC 求解出相機(jī)在世界坐標(biāo)系中的位姿。通俗解釋傳統(tǒng)特征匹配需要先在圖像和三維地圖之間建立特征描述子對(duì)應(yīng)再做 PnPSCR 則把這個(gè)過程壓縮成一個(gè)端到端的回歸問題——我不管這個(gè)像素長什么樣我直接“猜”它在世界里的位置。這個(gè)思路相比傳統(tǒng)方法有幾個(gè)明顯優(yōu)勢(shì)不需要存儲(chǔ)大量特征描述子地圖可以做得更緊湊。端到端訓(xùn)練特征學(xué)習(xí)和位姿求解可以聯(lián)合優(yōu)化。在紋理缺失或重復(fù)紋理場(chǎng)景中比基于局部特征的方法更魯棒。DSAC* 等一系列工作把 SCR 推到了視覺定位的第一梯隊(duì)。這個(gè)路線的基本范式是卷積網(wǎng)絡(luò)回歸場(chǎng)景坐標(biāo) 可微 PnP/RANSAC 模塊求位姿 端到端損失傳播。2.2 從視覺轉(zhuǎn)到 LiDAR難點(diǎn)在哪視覺 SCR 的輸入是 RGB 圖像每個(gè)像素都有明確的規(guī)則網(wǎng)格結(jié)構(gòu)。LiDAR 點(diǎn)云則完全不同稀疏性一幀 64 線雷達(dá)大約十多萬個(gè)點(diǎn)但分布極不均勻近處密遠(yuǎn)處稀。無序性點(diǎn)云沒有固定的排列順序。幾何退化在隧道或長走廊中某些維度如沿走廊方向的平移在幾何上不可觀。外觀信息少激光雷達(dá)沒有豐富的紋理信息雖然強(qiáng)度值可以提供一些幫助但遠(yuǎn)不如 RGB 信息豐富。所以 LiDAR SCR 不能直接把視覺的網(wǎng)絡(luò)結(jié)構(gòu)搬過來用。它需要處理點(diǎn)云的稀疏與無序特性常見的做法包括體素化、PointNet 這類集合提取結(jié)構(gòu)、或者將點(diǎn)云投影到距離圖range image上做稠密預(yù)測(cè)。UQ-Loc 的賣點(diǎn)在于它不光做這個(gè)回歸還在回歸的同時(shí)估計(jì)每個(gè)點(diǎn)的不確定性并把不確定性信息用于最終的位姿優(yōu)化——這正是從視覺方法轉(zhuǎn)向 LiDAR 場(chǎng)景時(shí)容易被忽略的問題。2.3 SCR 與傳統(tǒng)定位方法的對(duì)比方法類型核心思想優(yōu)勢(shì)劣勢(shì)ICP / NDT 配準(zhǔn)迭代優(yōu)化源點(diǎn)云與目標(biāo)點(diǎn)云的幾何對(duì)齊精度高通用性強(qiáng)需要較好的初始位姿對(duì)稱場(chǎng)景易退化特征匹配 PnP提取關(guān)鍵點(diǎn)描述子匹配地圖點(diǎn)計(jì)算位姿可處理大范圍重定位依賴特征質(zhì)量弱紋理場(chǎng)景易失敗位姿回歸網(wǎng)絡(luò)直接回歸 6-DoF 位姿輕量推理快精度低泛化能力弱場(chǎng)景坐標(biāo)回歸回歸每個(gè)點(diǎn)的全局三維坐標(biāo)再求解位姿精度較高地圖緊湊可與 PnP 結(jié)合需要模型訓(xùn)練場(chǎng)景變化需要重新訓(xùn)練不確定性 SCRSCR 不確定性輸出具備誤差預(yù)警能力可輔助失敗恢復(fù)訓(xùn)練復(fù)雜度更高評(píng)估維度更多從這張表能看出來UQ-Loc 不是把上一行全部推翻而是在 SCR 這個(gè)已經(jīng)有較好精度基礎(chǔ)的框架上增加了一個(gè)最關(guān)鍵的維度風(fēng)險(xiǎn)感知。3. UQ-Loc 的核心思路拆解下面開始拆 UQ-Loc 的模型設(shè)計(jì)。需要說明的是不同論文對(duì)“不確定性感知”的具體實(shí)現(xiàn)差異較大這里重點(diǎn)講通用的方法論框架這樣無論你看的是哪篇具體論文都能快速對(duì)上號(hào)。3.1 不確定性從哪里來偶然不確定性與認(rèn)知不確定性幾乎所有帶不確定性估計(jì)的深度學(xué)習(xí)工作都會(huì)把不確定性分成兩類偶然不確定性Aleatoric Uncertainty來自觀測(cè)數(shù)據(jù)本身的噪聲。比如激光雷達(dá)測(cè)距誤差、標(biāo)定誤差、動(dòng)態(tài)物體的遮擋導(dǎo)致的測(cè)量不完整。這類不確定性無法通過增加訓(xùn)練數(shù)據(jù)消除只能估計(jì)它的大小并在推理時(shí)考慮進(jìn)去。認(rèn)知不確定性Epistemic Uncertainty來自模型的“知識(shí)盲區(qū)”。比如你訓(xùn)練集里全是室外道路場(chǎng)景突然來一個(gè)地下車庫模型在這個(gè)分布外輸入上就不知道該怎么辦。增加訓(xùn)練數(shù)據(jù)、增加模型容量、或者使用測(cè)試時(shí)增強(qiáng)可以降低認(rèn)知不確定性。UQ-Loc 這類工作在設(shè)計(jì)時(shí)通常會(huì)把這兩種不確定性分別建模偶然不確定性通過網(wǎng)絡(luò)的第二個(gè)輸出頭直接回歸出來比如為每個(gè)點(diǎn)的場(chǎng)景坐標(biāo)預(yù)測(cè)加上一個(gè)方差。認(rèn)知不確定性常見做法是用 MC Dropout 或 Deep Ensemble 來近似在推理時(shí)多次前向傳播觀察輸出的一致性。如果你只想落地一個(gè)最簡(jiǎn)版本先從偶然不確定性開始就夠用它的訓(xùn)練損失函數(shù)天然支持方差的學(xué)習(xí)工程成本也要小得多。3.2 共享主干 雙分支輸出UQ-Loc 的典型結(jié)構(gòu)可以概括為一個(gè)點(diǎn)云特征提取主干PointNet、稀疏卷積或范圍圖 CNN一個(gè)場(chǎng)景坐標(biāo)回歸分支輸出每個(gè)點(diǎn)/像素的全局三維坐標(biāo)一個(gè)不確定性回歸分支輸出對(duì)應(yīng)每個(gè)坐標(biāo)的不確定性方差或置信度。兩個(gè)分支共享同一個(gè)特征提取網(wǎng)絡(luò)。這樣做的好處是特征提取部分學(xué)習(xí)的幾何和語義信息既為坐標(biāo)回歸服務(wù)也為不確定性估計(jì)服務(wù)。在推理階段不確定性分支并不需要額外的計(jì)算量所以這個(gè)設(shè)計(jì)幾乎是免費(fèi)的。用代碼邏輯表示推理時(shí)的偽代碼如下# 偽代碼UQ-Loc 風(fēng)格推理流程示意 # 實(shí)際網(wǎng)絡(luò)結(jié)構(gòu)以論文實(shí)現(xiàn)為準(zhǔn) def forward(point_cloud): # 輸入一幀 LiDAR 點(diǎn)云形狀 [N, 3C] # N 為點(diǎn)數(shù)C 為額外特征強(qiáng)度、時(shí)間戳等 features backbone(point_cloud) # 點(diǎn)云特征提取 scene_coords coord_head(features) # 每個(gè)點(diǎn)的全局場(chǎng)景坐標(biāo) [N, 3] uncertainty unc_head(features) # 每個(gè)點(diǎn)的不確定性 [N, 1] return scene_coords, uncertainty這里最關(guān)鍵的設(shè)計(jì)決策是不確定性分支的輸出應(yīng)該是什么形式常見選擇有兩種輸出方差variance在損失函數(shù)中參與加權(quán)輸出置信度confidence在位姿求解時(shí)作為 RANSAC 的樣本權(quán)重。前者訓(xùn)練更穩(wěn)定后者和 PnP/RANSAC 的結(jié)合更直覺。UQ-Loc 這種工作一般會(huì)想辦法讓兩者在一個(gè)統(tǒng)一框架里工作。3.3 損失函數(shù)讓不確定性學(xué)會(huì)“誠實(shí)”不確定性估計(jì)最難的一點(diǎn)是它沒有直接的監(jiān)督標(biāo)簽。我們并不知道每個(gè)點(diǎn)真實(shí)的方差是多少那該怎么辦答案是利用似然函數(shù)來間接監(jiān)督。一個(gè)經(jīng)典的思路是假設(shè)場(chǎng)景坐標(biāo)預(yù)測(cè)服從高斯分布均值是預(yù)測(cè)的坐標(biāo)方差是不確定性分支的輸出。那么我們訓(xùn)練的目標(biāo)就是最大化這個(gè)高斯分布的似然等價(jià)于最小化如下所示的負(fù)對(duì)數(shù)似然損失loss 0.5 * ( (y_pred - y_true)^2 / sigma^2 log(sigma^2) )其中sigma^2是不確定性分支輸出的方差。這個(gè)損失函數(shù)的妙處在于當(dāng)預(yù)測(cè)誤差很大時(shí)模型會(huì)增大sigma來降低第一項(xiàng)但第二項(xiàng)會(huì)阻止sigma無限增大當(dāng)預(yù)測(cè)誤差很小時(shí)模型會(huì)減小sigma并保持它以降低第二項(xiàng)。這樣訓(xùn)練出來的不確定性可以理解為模型對(duì)自身預(yù)測(cè)誤差的“誠實(shí)估計(jì)”。誤差大的位置不確定性高誤差小的位置不確定性低。在實(shí)際工程中還需要注意如果不確定性分支輸出的是log(sigma^2)而不是直接輸出sigma^2訓(xùn)練數(shù)值會(huì)更加穩(wěn)定因?yàn)楸苊饬朔讲畋仨毞秦?fù)的約束帶來的優(yōu)化困難。3.4 位姿求解階段如何使用不確定性有了每個(gè)點(diǎn)的不確定性后續(xù)的位姿求解可以有兩種用法。一種是用不確定性作為 RANSAC 的權(quán)重。傳統(tǒng) RANSAC 在采樣時(shí)對(duì)所有點(diǎn)一視同仁但如果我們知道某些點(diǎn)的預(yù)測(cè)很不確定就可以降低它們被采樣的概率從而提高采樣效率減少迭代次數(shù)。另一種是用不確定性做加權(quán) PnP。在求解最小二乘問題時(shí)每個(gè) 2D-3D 對(duì)應(yīng)的殘差乘以一個(gè)權(quán)重權(quán)重由不確定性決定。這樣不確定度高的點(diǎn)在優(yōu)化中的影響被自動(dòng)壓低最終的位姿精度會(huì)更高。定位流程對(duì)比 無不確定性感知 原始點(diǎn)云 → 回歸場(chǎng)景坐標(biāo) → 均勻權(quán)重 PnP/RANSAC → 一次性位姿輸出 不確定性感知 原始點(diǎn)云 → 回歸場(chǎng)景坐標(biāo) 不確定性 → 加權(quán) PnP/RANSAC → 位姿 不確定性估計(jì)這個(gè)差異在表面上看只是多了一個(gè)分支實(shí)際上是把“系統(tǒng)是否可信”這個(gè)原本要靠事后閾值判斷的問題提前合并到了模型的推理過程中。4. 不確定性感知到底能改變什么很多人看到“不確定性”三個(gè)字第一反應(yīng)是“這個(gè)東西又不能直接提升精度有什么實(shí)際價(jià)值”這個(gè)判斷是不完整的。不確定性感知的核心價(jià)值在于讓定位系統(tǒng)具備了自我評(píng)估的能力。拿自動(dòng)駕駛場(chǎng)景來說如果定位模塊給出的位姿不確定度突然升高說明當(dāng)前環(huán)境可能進(jìn)入了結(jié)構(gòu)退化區(qū)域、動(dòng)態(tài)遮擋嚴(yán)重區(qū)域或者傳感器數(shù)據(jù)質(zhì)量在下降。這時(shí)候系統(tǒng)可以選擇降低車速、切換定位策略、請(qǐng)求重新初始化而不是繼續(xù)以高置信度往前沖。具體到工程系統(tǒng)不確定性輸出可以在至少三個(gè)環(huán)節(jié)被使用第一后端融合的權(quán)重調(diào)節(jié)。在多傳感器融合系統(tǒng)中GPS、IMU、輪速計(jì)、LiDAR 定位各自的可靠性是動(dòng)態(tài)變化的。傳統(tǒng)做法是配置一個(gè)固定的噪聲協(xié)方差。如果 LiDAR 定位模塊能輸出逐幀的不確定性就可以將這個(gè)不確定性動(dòng)態(tài)映射到融合權(quán)重中實(shí)現(xiàn)更合理的多傳感器融合。這也正好呼應(yīng)了當(dāng)前業(yè)內(nèi)非常關(guān)注的“針對(duì) camera / lidar / imu / gps 四類傳感器的專屬質(zhì)量評(píng)估指標(biāo)”這一趨勢(shì)——不確定性估計(jì)正是評(píng)估感知模塊輸出質(zhì)量的關(guān)鍵技術(shù)手段之一。第二失敗檢測(cè)與自動(dòng)恢復(fù)。定位系統(tǒng)最怕的不是失敗而是失敗了還不自知。不確定性估計(jì)算法可以提供一個(gè)天然的失敗檢測(cè)信號(hào)。設(shè)定一個(gè)閾值當(dāng)不確定性超過閾值時(shí)觸發(fā)重定位或地圖切換流程就能避免系統(tǒng)帶著錯(cuò)誤位姿長時(shí)間運(yùn)行。第三動(dòng)態(tài)場(chǎng)景過濾。在動(dòng)態(tài)物體行人、車輛占據(jù)的區(qū)域場(chǎng)景坐標(biāo)回歸的誤差通常很大不確定性也會(huì)相應(yīng)升高。利用不確定性信息可以在位姿求解前過濾掉這些不可靠的點(diǎn)相當(dāng)于給點(diǎn)云做了一次面向定位任務(wù)的動(dòng)態(tài)物體去除。從架構(gòu)層面看不確定性感知改變的并不是某一個(gè)模塊的算法而是定位系統(tǒng)對(duì)待自身輸出的態(tài)度從每次輸出一個(gè)唯一答案到輸出答案并且告訴你這個(gè)答案值不值得信賴。5. 實(shí)驗(yàn)設(shè)計(jì)與評(píng)估指標(biāo)怎么證明“不確定性”是有效的一個(gè)很實(shí)際的問題是如果我們要復(fù)現(xiàn)或者評(píng)估這類方法除了看傳統(tǒng)的定位精度還需要關(guān)心什么5.1 定位精度指標(biāo)LiDAR 定位精度通常使用兩類指標(biāo)ATE絕對(duì)軌跡誤差衡量估計(jì)軌跡與真值軌跡的整體偏差。RPE相對(duì)位姿誤差衡量相鄰幀之間的相對(duì)位姿誤差。這兩個(gè)指標(biāo)在任何 SLAM / 定位論文中都是標(biāo)配UQ-Loc 這類工作也會(huì)報(bào)告。但只看這兩個(gè)指標(biāo)無法回答“不確定性估計(jì)到底準(zhǔn)不準(zhǔn)”的問題。5.2 不確定性質(zhì)量指標(biāo)比精度更重要的驗(yàn)證維度假設(shè)一個(gè)模型預(yù)測(cè)出了不確定性sigma那怎么判斷它好不好核心標(biāo)準(zhǔn)是預(yù)測(cè)的不確定性和實(shí)際誤差是不是匹配。常用的指標(biāo)包括NLL負(fù)對(duì)數(shù)似然衡量模型預(yù)測(cè)分布和真實(shí)標(biāo)簽的擬合程度越低越好。ECE期望校準(zhǔn)誤差把預(yù)測(cè)不確定性分桶比較桶內(nèi)平均不確定性和實(shí)際誤差的一致性越低說明校準(zhǔn)越好。AUSE面積下的不確定性誤差對(duì)不同不確定性閾值計(jì)算“錯(cuò)誤被接受的比例”繪制曲線后積分。AURRE面積下的拒絕誤差在不同拒絕比例下計(jì)算平均定位誤差可以直觀看到“過濾掉高不確定性樣本后精度能提升多少”。這里的工程含義是一個(gè)良好的不確定性估計(jì)應(yīng)該能做到“當(dāng)系統(tǒng)說這次定位不可信時(shí)這次定位真的更可能出錯(cuò)”。如果不確定性輸出和真實(shí)誤差完全不相關(guān)那這個(gè)輸出對(duì)下游來說就是一個(gè)噪音。5.3 設(shè)計(jì)評(píng)測(cè)場(chǎng)景的注意事項(xiàng)評(píng)估不確定性算法比評(píng)估普通定位算法更要小心場(chǎng)景設(shè)計(jì)。建議準(zhǔn)備三類測(cè)試集正常場(chǎng)景常規(guī)道路、公開數(shù)據(jù)集上的標(biāo)準(zhǔn)序列。退化場(chǎng)景長走廊、隧道、停車場(chǎng)、空曠區(qū)域。動(dòng)態(tài)場(chǎng)景車流密集、行人混雜的環(huán)境。退化場(chǎng)景和動(dòng)態(tài)場(chǎng)景恰恰是不確定性估計(jì)發(fā)揮最大價(jià)值的地方。如果在這些場(chǎng)景下模型的不確定性輸出能夠顯著高于正常場(chǎng)景并且過濾掉高不確定性幀之后定位誤差明顯下降那基本可以判斷這套不確定性機(jī)制是有效的。6. 一個(gè)最小可驗(yàn)證的實(shí)驗(yàn)流程如果你打算在項(xiàng)目里試著引入類似 UQ-Loc 的方案可以參考下面的思路跑通一個(gè)最小閉環(huán)實(shí)驗(yàn)。6.1 數(shù)據(jù)準(zhǔn)備階段你需要準(zhǔn)備一個(gè)包含 LiDAR 點(diǎn)云、真值位姿和三維地圖的數(shù)據(jù)集。如果是自采數(shù)據(jù)需要先完成 LiDAR 的位姿標(biāo)定也就是通常說的 LiDAR SLAM 或 LiDAR-Inertial 建圖。這一步非常關(guān)鍵因?yàn)閳?chǎng)景坐標(biāo)回歸的訓(xùn)練標(biāo)簽就是“每個(gè)點(diǎn)在地圖坐標(biāo)系下的真實(shí)三維坐標(biāo)”標(biāo)簽不準(zhǔn)整個(gè)模型的上限就被卡死了。這里特別提醒一個(gè)容易出現(xiàn)問題的環(huán)節(jié)LiDAR 外參標(biāo)定。場(chǎng)景坐標(biāo)回歸需要把每幀點(diǎn)云從傳感器坐標(biāo)系變換到地圖坐標(biāo)系如果 LiDAR 相對(duì)車體的外參不準(zhǔn)確點(diǎn)云和地圖的對(duì)齊誤差會(huì)直接變成訓(xùn)練標(biāo)簽的誤差。工程上建議在建圖之前先做一次完整的 LiDAR-IMU-Camera 聯(lián)合標(biāo)定并建立標(biāo)定參數(shù)的版本管理機(jī)制。6.2 網(wǎng)絡(luò)訓(xùn)練流程一個(gè)最小訓(xùn)練流程可以這樣組織# 偽代碼訓(xùn)練循環(huán)核心邏輯 import torch # 假設(shè)已經(jīng)構(gòu)建好模型和數(shù)據(jù)加載器 model UQLocModel( backbonepointnet2, coord_dim3, uncertainty_modeheteroscedastic ) optimizer torch.optim.Adam(model.parameters(), lr1e-4) for epoch in range(args.epochs): for batch_points, batch_coords_true in dataloader: batch_points batch_points.to(device) batch_coords_true batch_coords_true.to(device) coords_pred, log_var model(batch_points) # 負(fù)對(duì)數(shù)似然損失帶不確定性加權(quán) precision torch.exp(-log_var) loss torch.mean(precision * (coords_pred - batch_coords_true) ** 2) loss torch.mean(log_var) # 正則項(xiàng)防止方差無限增大 optimizer.zero_grad() loss.backward() optimizer.step()這個(gè)代碼的核心邏輯就是前文提到的負(fù)對(duì)數(shù)似然損失。注意這里用的是log_var而不是直接輸出方差這是數(shù)值穩(wěn)定性的關(guān)鍵。6.3 后處理與位姿求解訓(xùn)練完場(chǎng)景坐標(biāo)回歸器后在位姿求解階段可以按如下方式結(jié)合不確定性# 偽代碼加權(quán) PnP 流示意 # 省略了 RANSAC 細(xì)節(jié)只展示加權(quán)核心思想 import numpy as np def solve_pose_with_uncertainty(scene_coords, uncertainties, keypoint_indices): # scene_coords: 模型預(yù)測(cè)的全局場(chǎng)景坐標(biāo) # uncertainties: 對(duì)應(yīng)每個(gè)預(yù)測(cè)點(diǎn)的方差 # keypoint_indices: 通過 RANSAC 采樣選中的內(nèi)點(diǎn)下標(biāo) selected_coords scene_coords[keypoint_indices] selected_weights 1.0 / uncertainties[keypoint_indices] # 方差異常處理可加 epsilon # 用加權(quán)最小二乘求解位姿示意 # 這類問題建議使用成熟 PnP 庫或?qū)崿F(xiàn)加權(quán)版本的 P3P/PnP 求解器 pose weighted_pnp( points_2dselected_coords, points_3dselected_coords, weightsselected_weights ) return pose這里面的關(guān)鍵處理是不確定度越高的點(diǎn)其權(quán)重越低?？梢杂行У挚箞?chǎng)景坐標(biāo)回歸中少量外點(diǎn)對(duì)位姿求解的干擾。6.4 運(yùn)行與驗(yàn)證跑完一套流程后建議至少輸出以下統(tǒng)計(jì)信息ATE / RPE驗(yàn)證定位精度。不確定性估計(jì)值和定位誤差之間的相關(guān)系數(shù)。設(shè)置不同不確定度閾值時(shí)的定位誤差變化曲線。如果看到的現(xiàn)象是過濾掉高不確定度的幀之后定位誤差明顯下降那就說明不確定性輸出是有信息量的。這也是 UQ-Loc 這類方法最值得關(guān)注的驗(yàn)證結(jié)果。7. 常見問題與排查思路圍繞不確定性感知 LiDAR 定位結(jié)合我過去定位方向的經(jīng)驗(yàn)整理出下面幾個(gè)高頻問題供大家快速排查。問題現(xiàn)象可能原因排查方式解決方案不確定性輸出全部接近幾乎沒有區(qū)分度訓(xùn)練數(shù)據(jù)場(chǎng)景單一模型沒有見過難度不同的樣本檢查訓(xùn)練集中退化場(chǎng)景和動(dòng)態(tài)場(chǎng)景的占比增加不同難度的場(chǎng)景尤其是長走廊、停車場(chǎng)場(chǎng)景坐標(biāo)回歸誤差很大模型不收斂訓(xùn)練標(biāo)簽地圖坐標(biāo)本身存在錯(cuò)誤可視化一部分點(diǎn)云和地圖的疊加檢查對(duì)齊情況重新做建圖和 LiDAR 外參標(biāo)定校驗(yàn)標(biāo)簽生成流程不確定性估計(jì)與定位誤差不相關(guān)不確定性分支只用 NLL 訓(xùn)練但未參與位姿求解檢查推理時(shí)是否使用了不確定性信息在位姿求解階段引入不確定性權(quán)重對(duì)稱環(huán)境下位姿錯(cuò)配但不確定性不高模型在訓(xùn)練時(shí)沒有見過這種對(duì)稱場(chǎng)景單獨(dú)構(gòu)造對(duì)稱場(chǎng)景測(cè)試集引入結(jié)構(gòu)退化感知的損失項(xiàng)或數(shù)據(jù)增強(qiáng)推理速度較慢不確定性分支增加了額外計(jì)算分析每個(gè)模塊耗時(shí)考慮用輕量級(jí)不確定性頭或只在定位結(jié)果下發(fā)時(shí)計(jì)算不確定性訓(xùn)練時(shí) loss 出現(xiàn) NaN方差出現(xiàn)極小值導(dǎo)致除零檢查 log_var 的輸出范圍和數(shù)值穩(wěn)定性給分母加 epsilon或約束 log_var 的上下界這里的前兩個(gè)問題最容易出現(xiàn)在自采數(shù)據(jù)集上。很多時(shí)候模型訓(xùn)不好不是網(wǎng)絡(luò)結(jié)構(gòu)不夠好而是地圖建得不夠好。8. 工程化落地的幾條建議把這類算法從論文帶到實(shí)際系統(tǒng)有幾點(diǎn)經(jīng)驗(yàn)值得分享。第一地圖資產(chǎn)和傳感器標(biāo)定要分開管理。UQ-Loc 這類方法強(qiáng)依賴三維地圖的質(zhì)量而地圖的質(zhì)量又強(qiáng)依賴 LiDAR 建圖時(shí)的外參、回環(huán)糾正、點(diǎn)云去畸變等環(huán)節(jié)。建議把標(biāo)定參數(shù)、地圖版本、算法權(quán)重三者納入獨(dú)立的版本管理避免某個(gè)環(huán)節(jié)變更后難以回溯。第二別丟掉傳統(tǒng)定位方法。“不確定性 SCR”和傳統(tǒng) ICP/NDT 不是替代關(guān)系而是互補(bǔ)關(guān)系。在工程上更穩(wěn)妥的做法是用 SCR 做全局重定位的初始猜測(cè)再用 ICP/NDT 精配準(zhǔn)最后用 SCR 輸出的不確定性來決定這個(gè)流程是否可信。這樣即兼容了 SCR 的重定位能力和傳統(tǒng)精配準(zhǔn)的高精度。第三先做好不確定性校準(zhǔn)再談部署。模型訓(xùn)練完后不確定性輸出的絕對(duì)值通常不能直接當(dāng)作真實(shí)方差使用。需要通過一段帶有真值的序列做校準(zhǔn)得到不確定性到真實(shí)誤差的映射關(guān)系。這一步不做你很難在下游安全邏輯里設(shè)定一個(gè)合適的閾值。第四評(píng)估鏈路要覆蓋退化場(chǎng)景。很多團(tuán)隊(duì)在測(cè)試時(shí)只跑 normal 場(chǎng)景得到的結(jié)論是“模型挺準(zhǔn)的”但這掩蓋了退化場(chǎng)景下的風(fēng)險(xiǎn)。建議把退化場(chǎng)景自動(dòng)化地加入回歸測(cè)試集每次模型更新都要跑一遍防止“精度提升但魯棒性下降”的隱性回歸。第五關(guān)注多傳感器質(zhì)量評(píng)估體系。如果系統(tǒng)里同時(shí)有 camera、LiDAR、IMU、GPS建議為每一類傳感器都設(shè)計(jì)獨(dú)立的質(zhì)量評(píng)估指標(biāo)再在融合層把 LiDAR 定位的不確定性作為其中一個(gè)輸入。不確定性感知和傳感器質(zhì)量評(píng)估本質(zhì)上是一件事的兩個(gè)側(cè)面一個(gè)是讓每個(gè)傳感器模塊“誠實(shí)匯報(bào)”另一個(gè)是讓融合中心“合理信任”。9. 總結(jié)與后續(xù)方向UQ-Loc 的核心思想可以濃縮成一句話LiDAR 場(chǎng)景坐標(biāo)回歸不僅要回答“我在哪里”還要回答“我有多確定”。它把不確定性估計(jì)融入到從點(diǎn)云特征提取、場(chǎng)景坐標(biāo)回歸到位姿求解的完整鏈路中讓定位系統(tǒng)第一次有機(jī)會(huì)在“自信地犯錯(cuò)”之前向系統(tǒng)發(fā)出預(yù)警。如果你想從這篇文章開始動(dòng)手實(shí)踐建議按這樣的順序來先準(zhǔn)備一套帶真值位姿和三維地圖的點(diǎn)云數(shù)據(jù)確認(rèn)標(biāo)定無誤再實(shí)現(xiàn)一個(gè)最簡(jiǎn)的“共享主干 坐標(biāo)回歸頭 不確定性頭”結(jié)構(gòu)用負(fù)對(duì)數(shù)似然損失訓(xùn)練最后在傳統(tǒng)定位精度之外加上不確定性質(zhì)量的評(píng)估指標(biāo)驗(yàn)證不確定性是否真的有信息量。這個(gè)閉環(huán)跑通之后再討論與現(xiàn)有 SLAM 系統(tǒng)的集成。在更寬的技術(shù)趨勢(shì)里不確定性感知定位是“安全型定位系統(tǒng)”的基石之一。后續(xù)值得深入的方向包括將不確定性估計(jì)與相機(jī)-雷達(dá)深度融合用不確定性來動(dòng)態(tài)調(diào)節(jié)跨傳感器的信任權(quán)重在線的場(chǎng)景坐標(biāo)回歸與地圖更新以及更細(xì)粒度的不確定性分解把動(dòng)態(tài)物體、傳感器噪聲、模型知識(shí)盲區(qū)三類不確定性來源區(qū)分開。這些方向落地難度都不小但每進(jìn)一步機(jī)器對(duì)自身能力邊界的認(rèn)識(shí)就更清晰一層。如果你正在做定位方向的工程或研究建議把這篇文章的思路和你的實(shí)際場(chǎng)景對(duì)照一下看看你的系統(tǒng)在哪些環(huán)節(jié)最容易出現(xiàn)高誤判置信度那個(gè)地方很可能就是不確定性感知應(yīng)該介入的位置。