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

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

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

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

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

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

如下展示了快速相機移動情況下的實時在線重建效果（視頻未加速）。可以看到，操作者非?？焖俚膿u動相機，導(dǎo)致 RGB 圖像運動模糊嚴(yán)重。在這樣的情況下，ROSEFusion 仍然可以非常準(zhǔn)確、穩(wěn)定的跟蹤相機位姿，并得到準(zhǔn)確的三維重建。

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

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

1、基本思想

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

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

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

2、基于 depth-to-TSDF 的適應(yīng)性函數(shù)

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

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

，對應(yīng)的觀察似然函數(shù)為：

其中

為當(dāng)前的 TSDF 場，對于全局坐標(biāo)系下的一個 3D 點

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

，取公式 (1) 的負(fù)對數(shù)，得到如下優(yōu)化目標(biāo)：

其中

即為候選相機位姿

的適應(yīng)性函數(shù)，它度量了當(dāng)前深度圖

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

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

3、基于隨機優(yōu)化的相機位姿跟蹤

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

圖 3：ROSEFusion 的概率圖模型：左半部為 SLAM 的概率圖模型，右半部為某一幀相機位姿的粒子濾波優(yōu)化過程?；诹Ｗ訛V波的 SLAM 是面向連續(xù)幀的序列位姿估計，而 ROSEFusion 中的粒子濾波優(yōu)化則是面向某一幀位姿的序列迭代優(yōu)化。

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

初始權(quán)重均為 1。每次迭代中，首先根據(jù)粒子的權(quán)重進行重采樣，然后依據(jù)動力學(xué)模型

驅(qū)動粒子移動，再根據(jù)觀察似然更新粒子權(quán)重：

。重復(fù)上述步驟直至最優(yōu)位姿被粒子群覆蓋或達到最大迭代次數(shù)。上述似然函數(shù)

即為前述適用性函數(shù)。

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

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

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

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

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

滿足（見圖 6）：

其中，

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

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

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

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

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

4、實驗結(jié)果與評測

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

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

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

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

公開數(shù)據(jù)集 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 也能達到與當(dāng)前最佳算法相當(dāng)?shù)南鄼C跟蹤精度（圖 11）。SOTA 算法一般都包含了全局位姿優(yōu)化，而 ROSEFusion 沒有。

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

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

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

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

作者希望通過本文引起領(lǐng)域?qū)γ嫦蚩焖傧鄼C移動的 SLAM / 在線重建問題的關(guān)注?，F(xiàn)有方法一般基于 RGB 圖像的特征或像素匹配，采用梯度下降法求解非線性優(yōu)化問題。由于快速相機運動導(dǎo)致的 RGB 圖像運動模糊，大角度旋轉(zhuǎn)優(yōu)化帶來的高度非凸 / 非線性問題，以往方法難以有效實現(xiàn)相機位姿跟蹤。ROSEFusion 采用隨機優(yōu)化的方法求解 SLAM 的視覺里程計問題，結(jié)合純幾何的適應(yīng)性函數(shù)計算，實現(xiàn)了無 IMU 輔助的快速相機運動在線重建。ROSEFusion 當(dāng)然可以結(jié)合全局位姿優(yōu)化和回環(huán)檢測，實現(xiàn)更高質(zhì)量的三維重建。事實上，后者也很可能可以基于隨機優(yōu)化來實現(xiàn)。