近日，國防科技大學徐凱教授團隊提出了基于隨機優化求解快速移動下的在線 RGB-D 重建方法 ROSEFusion，在無額外硬件輔助的條件下，僅依靠深度信息，實現了最高 4 m/s 線速度、6 rad/s 角速度相機移動下的高精度在線三維重建。

自 2011 年 KinectFusion 問世以來，基于 RGB-D 相機的實時在線三維重建一直是 3D 視覺和圖形領域的研究熱點。10 年間涌現出了大量優秀的方法和系統。如今，在線 RGB-D 重建已在增強現實、機器人等領域廣泛應用，已逐漸發展成為「人 - 機 - 物」 空間智能（Spatial AI）的重要使能技術。

在線 RGB-D 重建的底層技術是 RGB-D SLAM。其中，建圖（mapping）部分采用專門適合于實時三維重建的深度圖融合（depth fusion）技術。深度圖融合一般有基于截斷符號距離場（TSDF）的體融合（volumetric fusion）和基于表面片元（surfel）的點云融合（point-based fusion）兩種基本方式。而相機跟蹤（tracking）部分則一般分為 frame-to-frame 的幀注冊方法（包括特征法和直接法）以及 frame-to-model 的 ICP 方法。

目前，一般的在線 RGB-D 重建方法只能處理相機移動較為慢速（如平均線速度 < 0.5 m/s，平均角速度 < 1 rad/s）的情況。過快的相機運動會導致相機跟蹤失敗，從而產生錯誤的重建結果。這就非常影響在線三維重建的效率：手持 RGB-D 相機的掃描者必須小心翼翼地移動相機，任何快速移動或抖動都可能讓之前的掃描前功盡棄。不支持快速相機移動，也極大限制了在線 RGB-D 重建在機器人、無人機等領域中的實際應用。

快速相機移動下的在線 RGB-D 重建主要面臨兩個方面的挑戰。首先，SO(3)中的相機姿態跟蹤涉及非線性最小二乘優化，采用梯度下降法求解時，線性化近似的前提是前后兩幀之間的旋轉量較小，快速相機移動中的大角度旋轉會增加問題的非線性程度，容易讓優化陷入局部最優。其次，快速相機移動（特別是在光線昏暗條件下）會導致 RGB 圖像產生嚴重的運動模糊，從而無法進行可靠的 RGB 特征（或像素）跟蹤，這對基于 RGB 的相機跟蹤方法（如 ORB-SLAM）是致命的。一個容易想到的辦法是借助慣性測量單元（IMU）輸出的高幀率的線加速度和角速度來輔助相機跟蹤。但是，IMU 的初始化不是很魯棒，讀數會有漂移，更重要的是 IMU 需要與相機進行時間同步和空間標定，這些因素都會帶來估計偏差和誤差積累，同時也增加了系統的復雜性。

能否在不借助額外硬件的前提下，實現快速相機移動下的在線 RGB-D 重建？近日，國防科技大學徐凱教授團隊提出了基于隨機優化求解快速移動下的在線 RGB-D 重建方法 ROSEFusion，在無額外硬件輔助的條件下，僅依靠深度信息，實現了最高 4 m/s 線速度、6 rad/s 角速度相機移動下的高精度在線三維重建。

如下展示了快速相機移動情況下的實時在線重建效果（視頻未加速）。可以看到，操作者非常快速的搖動相機，導致 RGB 圖像運動模糊嚴重。在這樣的情況下，ROSEFusion 仍然可以非常準確、穩定的跟蹤相機位姿，并得到準確的三維重建。

ROSEFusion 在不加全局位姿優化和回環檢測的情況下，在快速移動 RGB-D 序列上達到了 SOTA 的相機跟蹤和三維重建精度，在普通速度序列上與以往性能最佳方法（包含全局位姿優化）的精度相當。該工作發表于 SIGGRAPH 2021。

• 論文鏈接：https://arxiv.org/abs/2105.05600
• 代碼和數據集鏈接：https://github.com/jzhzhang/ROSEFusion

1、基本思想

該工作基于一個基本觀察：快速相機移動雖然會帶來 RGB 圖像的運動模糊，但對深度圖像的影響較小。快速移動對深度圖的影響往往表現為在前景和背景過渡處的深度值過測量（overshoot）或欠測量（undershoot），而非全幅圖像的像素深度值混合[1]。對于上述遮擋邊界處的假信號，可以基于硬件很容易地檢測和去除（很多深度相機已實現）[2]，其結果呈現為：遮擋邊界處的深度值為空（見圖 1）。既然如此，很自然地考慮僅基于深度圖實現相機跟蹤。不過，面向深度圖的特征點檢測與匹配工作相對較少，因為深度圖特征點的判別力和魯棒性遠不如 RGB 圖像特征點。

圖 1：快速相機移動導致 RGB 圖像（左）出現嚴重運動模糊的情況下，對應的深度圖（右）僅在遮擋邊界處出現了空洞，但并未出現全幅圖像的像素模糊。

在非線性優化求解相機位姿方面，既然梯度下降法不能很好地處理大角度旋轉，ROSEFusion 采用隨機優化方法。據了解，這是領域內首個基于隨機優化的在線 RGB-D 重建方法。隨機優化算法的基本過程就是不斷地對解空間進行隨機采樣，評估每個采樣解的最優性（也稱適應性，fittness），再根據它們的適應性引導下一輪的采樣。因此，適應性函數和采樣策略是隨機優化算法的兩個重要方面。一個好的適應性函數應該對解的最優性判別力強且計算開銷小。一個好的采樣策略應該能讓采樣盡快覆蓋最優解。

2、基于 depth-to-TSDF 的適應性函數

為了最小化運動模糊帶來的影響，ROSEFusion 基于深度圖實現相機跟蹤。因此，適應性函數也要基于深度圖計算。一種直觀想法是計算相鄰兩幀的深度圖的匹配和注冊。然而，深度圖往往帶有噪聲，深度圖特征點的判別力和魯棒性較低；而且快速相機移動下準確的重投影匹配關系難以計算，不利于進行幀間匹配和注冊。ROSEFusion 采用 depth-to-TSDF 的適應性函數計算方法。給定當前深度圖

，深度相機的候選相機位姿

，對應的觀察似然函數為：

其中

為當前的 TSDF 場，對于全局坐標系下的一個 3D 點

，其 TSDF 值越接近 0 則說明該點越靠近重建表面。采用極大似然估計的方法優化

，取公式 (1) 的負對數，得到如下優化目標：

其中

即為候選相機位姿

的適應性函數，它度量了當前深度圖

與當前 TSDF 場的符合性（conformality）。參見圖 2 的直觀示意圖。這是一種純幾何（與 RGB 無關）的位姿適應性度量，且無需計算幀間的匹配和注冊。

圖 2：基于 depth-to-TSDF 的相機位姿適應性（最優性）計算：深度圖對應的 3D 點云經過相機位姿變換后，在 TSDF 場中取值的總和。

3、基于隨機優化的相機位姿跟蹤

粒子濾波優化（Particle Filter Optimization, PFO）是近年來提出的基于粒子濾波思想設計的隨機優化算法[3]。粒子濾波是非常著名的狀態估計算法，它依據序列觀察，通過重要性采樣來最大化觀察似然，以模擬狀態的真實分布。早年的很多 SLAM 算法都基于濾波方法[4]。但需要指出的是，本文涉及的粒子濾波優化，是一個優化器，而非狀態估計算法。直觀上講，在本文方法中，每一幀的相機位姿優化涉及若干次粒子重采樣（濾波迭代步），而在傳統基于粒子濾波的 SLAM 算法中，每一幀的姿態估計對應一次粒子重采樣。如圖 3 所示，在 ROSEFusion 中，SLAM 的每一幀（下標為 t）位姿優化涉及若干次粒子濾波優化迭代（下標為 k）。

圖 3：ROSEFusion 的概率圖模型：左半部為 SLAM 的概率圖模型，右半部為某一幀相機位姿的粒子濾波優化過程。基于粒子濾波的 SLAM 是面向連續幀的序列位姿估計，而 ROSEFusion 中的粒子濾波優化則是面向某一幀位姿的序列迭代優化。

粒子濾波優化的過程如下：以前一幀的相機位姿為中心，在 SE(3)空間中采樣大量 6D 位姿作為粒子：

初始權重均為 1。每次迭代中，首先根據粒子的權重進行重采樣，然后依據動力學模型

驅動粒子移動，再根據觀察似然更新粒子權重：

。重復上述步驟直至最優位姿被粒子群覆蓋或達到最大迭代次數。上述似然函數

即為前述適用性函數。

然而，粒子濾波優化涉及到大量粒子的采樣和權重更新，計算開銷較高，難以滿足在線重建的實時性要求。此外，如何設置一個好的動力學模型

是提高優化效率的關鍵。為此，本文結合粒子群智（particle swarm intelligence）來改進濾波優化方法，充分利用粒子群中的當前最優解來引導粒子的移動，實現基于粒子群智的動力學模型。同時，為避免大量粒子采樣與更新的計算開銷，論文提出預采樣的粒子群模板（Particle Swarm Template, PST）：預先采樣一個粒子集，以群智為引導，通過不斷移動和縮放粒子集，來達到驅動粒子覆蓋最優解的目的。圖 4 和圖 5 給出了預采樣的粒子群模板及其移動、縮放的示意圖。

圖 4：預采樣的粒子群模板（PST）及其隨迭優化代移動和縮放的示意圖

圖 5：對于每一幀的相機位姿優化，粒子群模板都要經過若干次移動和縮放，直至收斂到覆蓋最優解或達到最大迭代次數

在第k步迭代中，首先將 PST 整體移動到上一步適應性最高的粒子所在的位置，然后縮放 PST 橢球使其軸長

滿足（見圖 6）：

其中，

代表了 PST 橢球的各向異性程度，而

代表了 PST 橢球的尺度。直觀上講，v為相鄰兩步最優解之間的位移向量。因此，PST 橢球會沿著最優解出現的方向進行更大范圍的搜索；并且這個搜索范圍和上一步的最高適應性成反比，這使得算法越接近最優解則搜索范圍越小，更容易收斂。公式 (3) 借鑒了隨機梯度下降中的動量更新（momentum update）機制。

圖 6：每一個迭代步的粒子群模板移動和縮放，其中縮放采用了動量更新機制

下面的視頻給出了深度圖位姿優化過程的可視化，包括目標函數的優化（右上角）和 PST 的更新（右下角）。該視頻僅可視化了 PST 的各向異性程度（橢球的形狀）和 PST 中位姿的朝向分布（橢球的顏色），PST 的尺度則體現在右上角圓的半徑。從中可以看出，目標函數的非凸性嚴重，而 ROSEFusion 可以很魯棒地收斂到最優位姿。

在實現中，PST 和 TSDF 都存儲在 GPU 中，每個粒子的適應性計算在 GPU 中并行完成，計算效率很高，每次迭代的計算時間約為 1 ms，每幀大約需要 20~30 次迭代。而 CPU 僅負責 PST 的移動和縮放參數的計算。這也最小化了 CPU 與 GPU 之間的數據交換量。

4、實驗結果與評測

現有的在線 RGB-D 重建公開數據集很少包含快速相機移動的 RGB-D 序列。因此，該文提出了首個面向快速相機運動的 RGB-D 序列數據集 FastCaMo。該數據集分為合成和真實兩個部分：合成數據集 FastCaMo-Synth 基于 Facebook 開源的 Replica 室內場景數據集構建，作者合成了快速移動的相機軌跡，并渲染了 RGB 和深度圖，同時對 RGB 圖像添加了合成運動模糊，對深度圖添加了合成噪聲；真實數據集 FastCaMo-Real 包含了作者用 Kinect DK 掃描的 12 個不同場景的 24 個 RGB-D 序列，由于相機速度較快，難以獲得高質量的相機軌跡作為真值，作者采用激光掃描儀獲取了場景的完整三維重建，通過度量三維重建的完整性和準確性來評價相機跟蹤的準確性。FastCaMo 數據集的相機速度最快達到了線速度 4.6 m/s、角速度 5.7 rad/s，遠超以往任何公開數據集。

論文在 FastCaMo 上對比了兩個重要的在線 RGB-D 重建方法 BundleFusion[5]和 ElasticFusion[6]。結果如圖 7 和圖 8 所示。可以看出，ROSEFusion 的軌跡精度、重建質量（包括完整性和準確性）都顯著高于其它兩個方法。值得注意的是，ROSEFusion 是在無全局位姿優化、無回環檢測、不丟棄任何一幀的情況下達到這樣的性能的。

圖 7：在 FastCaMo-Synth 快速序列上的相機軌跡精度（ATE）對比（藍色為最佳）。

圖 8：在 FastCaMo-Real 快速序列上的重建質量（完整性和準確性）對比（藍色為最佳）。

公開數據集 ETH3D[7]包含了三個快速相機移動的 RGB-D 序列（camera_shake），圖 9 給出了在這三個序列上的不同方法的對比，ROSEFusion 在全部序列上取得了最佳相機跟蹤效果。

圖 9：在 ETH3D 的 camera_shake 序列上的相機軌跡精度（ATE）對比（藍色為最佳）。

圖 10 為 camera_shake_3 序列的重建效果對比，以及相機軌跡精度曲線（不同位姿精度下的幀占比）。

圖 10：在 ETH3D camera_shake_3 序列上的重建效果和相機跟蹤精度（ATE）對比。

在普通速度的 RGB-D 序列上，ROSEFusion 也能達到與當前最佳算法相當的相機跟蹤精度（圖 11）。SOTA 算法一般都包含了全局位姿優化，而 ROSEFusion 沒有。

圖 11：在 ICL-NUIM 數據集的普通速度序列上的相機軌跡精度（ATE）對比（藍色為最佳，綠色次之）。

圖 12 給出了位姿優化過程中 PST 的 2D 可視化，該圖對比了基于 PST 的粒子濾波優化（本文方法）、粒子群優化算法（Particle Swarm Optimization, PSO）以及普通的粒子濾波優化（無 PST）的優化過程。可以看出，基于 PST 的粒子濾波優化在快速探索最優解的速度和收斂性方面具有明顯優勢。

圖 12：不同優化方法的 2D 可視化過程對比（藍色為更優），基于 PST 的粒子濾波優化（第三行）可以快速收斂到更優的解。

如下視頻展示了快速掃描一個完整室內場景的過程（視頻未加速）。這個序列同樣包含了大量快速運動。其中一段掃描過程中，屋子里的燈被部分關閉。由于 ROSEFusion 的優化方法是純幾何的，與 RGB 成像無關，因而可以很魯棒的處理上述情況。

作者希望通過本文引起領域對面向快速相機移動的 SLAM / 在線重建問題的關注。現有方法一般基于 RGB 圖像的特征或像素匹配，采用梯度下降法求解非線性優化問題。由于快速相機運動導致的 RGB 圖像運動模糊，大角度旋轉優化帶來的高度非凸 / 非線性問題，以往方法難以有效實現相機位姿跟蹤。ROSEFusion 采用隨機優化的方法求解 SLAM 的視覺里程計問題，結合純幾何的適應性函數計算，實現了無 IMU 輔助的快速相機運動在線重建。ROSEFusion 當然可以結合全局位姿優化和回環檢測，實現更高質量的三維重建。事實上，后者也很可能可以基于隨機優化來實現。