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

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

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

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

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

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

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

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

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

1、基本思想

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

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

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

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

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

，深度相機(jī)的候選相機(jī)位姿

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

其中

為當(dāng)前的 TSDF 場(chǎng)，對(duì)于全局坐標(biāo)系下的一個(gè) 3D 點(diǎn)

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

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

其中

即為候選相機(jī)位姿

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

滿足（見(jiàn)圖 6）：

其中，

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

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

圖 6：每一個(gè)迭代步的粒子群模板移動(dòng)和縮放，其中縮放采用了動(dòng)量更新機(jī)制

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

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

4、實(shí)驗(yàn)結(jié)果與評(píng)測(cè)

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

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

圖 7：在 FastCaMo-Synth 快速序列上的相機(jī)軌跡精度（ATE）對(duì)比（藍(lán)色為最佳）。

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

公開(kāi)數(shù)據(jù)集 ETH3D[7]包含了三個(gè)快速相機(jī)移動(dòng)的 RGB-D 序列（camera_shake），圖 9 給出了在這三個(gè)序列上的不同方法的對(duì)比，ROSEFusion 在全部序列上取得了最佳相機(jī)跟蹤效果。

圖 9：在 ETH3D 的 camera_shake 序列上的相機(jī)軌跡精度（ATE）對(duì)比（藍(lán)色為最佳）。

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

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

在普通速度的 RGB-D 序列上，ROSEFusion 也能達(dá)到與當(dāng)前最佳算法相當(dāng)?shù)南鄼C(jī)跟蹤精度（圖 11）。SOTA 算法一般都包含了全局位姿優(yōu)化，而 ROSEFusion 沒(méi)有。

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

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

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

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

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