慣例論文出處：

http://grail.cs.washington.edu/projects/dynamicfusion/papers/DynamicFusion.pdf?

grail.cs.washington.edu

在KinectFusion發表時隔4年之後，Richard A. Newcombe大神又為我們帶來了DynamicFusion，可以說和之前的KinectFusion是一脈相承啦，關於KinectFusion的演算法解讀大夥可以去看看這篇 啥是KinectFusion。

從字面上我們就可以窺見DynamicFusion的不同之處在於Dynamic。我們知道，之前的KinectFusion允許用戶通過低成本的單一深度感測器（Kinect）進行三維重建，但被重建的物體，或者說感測器掃描的物體是靜態的。比如重建一個人的半身像，人的姿勢是固定的，通過Kinect以該人為中心，繞一圈，在Kinect圍著人繞的時候，進行人物表面的實時重建。

這篇文章，就是在KinectFusion的基礎上，解決瞭如何在被測物體也能同時運動的情況下，進行實時的表面重建。由於該演算法的性質，也決定了物體表面即使有形變也能準確的實時還原物體表面形狀。這麼神奇嗎？我們一起梳理下其演算法：

0. Overview

其整體思路：首先將每幀獲取到的動態變化的場景（要重建的對象）通過某種變換，轉換到一個canonical 空間中，即在該空間中創建一個靜態的物體表面模型； 而每幀都有個對應的 volumetric warp field 的東西，能夠將canonical空間中的模型還原到live frame中（可想而知，這個還原過程可能經歷了旋轉平移和形變）。

Canonical下描述模型，通過W轉換到live frame

以上既是DynamicFusion的整體流程。我們可以注意到兩個關鍵的變換，一個是從實時的深度圖到canonical空間，並且在該空間中更新表面建模；另一個是從canonical空間轉換到live frame下，即volumetric warp field的求算。

到這裡已經出現了幾個空間坐標系，為了方便理解，可以把canonical空間解釋為世界空間，模型表面會在該空間中通過tsdf進行描述（關於tsdf可以查看 啥是KinectFusion），live frame理解為相機空間或者說是觀察空間。之後梳理演算法的時候，我也會重點標註相應的坐標空間。

1. Dense Non-rigid Wrap Field

通過overview我們知道了這個Wrap Field很關鍵，這一小節討論的是如何參數化的去描述這個wrap field。文中給出：

W: S ightarrow SE(3)

這裡有canonical空間中的點 : v_{c} in S

SE(3) 表示特殊歐式羣，用以描述旋轉和平移：

而 T_{lc}=W(v_{c}) 即是說通過 wrap field，canonical空間中的頂點都對應了一個變換矩陣 T_{lc} , 能夠將其轉換到實時的相機空間中。

注意：這裡不是 W 直接作用於頂點變換，而是生成對應的變換矩陣 T_{lc} 對canonical空間中的頂點進行變換。這個 W 稱為volumetric wrap function

如果這個 W 需要對canonical空間中的每一個點去採樣，計算量過於龐大，幾乎不可能做到實時構建表面。故而作者通過採樣一組稀疏變換作為基礎，通過插值去定義這個 W 。文中採用對偶四元數混合插值法（dual-quaternion bleding DQB）來定義 W ：

W(x_{c}) equiv SE3(DQB(x_{c}))

這裡的 SE3(.) 表示將其中的四元數描述轉換為李羣 SE(3) ，形如前文提到的 W: S ightarrow SE(3)

注意：這裡的 W 並不是我們最後的結果，這裡的 SE(3) 可以理解為校正物體運動和形變的矩陣，隨後還有處理模型其他剛性變換（例如相機位姿）的矩陣。

這裡先暫停以下，老實說，之前並不瞭解dual-quaternion（對偶四元數）及其性質，就去查閱了本文Reference中的 Skinning with Dual Quaternions 一文，以下對其性質作個記錄：

對偶數：定義可以參照複數定義，即形如 a^{dual} = a_{0} + varepsilon a_{varepsilon} ，其中前部分為非對偶部分，後部分為對偶部分， varepsilon 為對偶單位，且滿足 varepsilon^2 = 0 ; 當只有對偶部分的時候，稱之為純對偶數；

對偶四元數： hat q = hat w + i hat x + j hat y + k hat z

同普通四元數類似，w是對偶標量部分，（x,y,z）為對偶向量，注意，這裡的i, j, k 就是普通四元數虛部單位，其與對偶單位ε有 i varepsilon = varepsilon i

對偶四元數可以看作是由8個實數構成，或者是由兩個普通四元數的和，即 hat q = q_{0} + varepsilon q_{varepsilon}

共軛對偶四元數定義與普通四元數的共軛相同(向量部分取負)，記為 hat q^{}ast = q_{0}^{ast} + varepsilon q_{varepsilon}^{ast}

其norm定義為：

這裡引出了單位對偶四元數，即滿足 | hat q | = 1 ，後面會用到。

至於作者為啥要用對偶四元數來描述變化插值, 首先對偶四元數不僅能描述旋轉，也可以描述平移， 然後很重要一點是四元數插值可以做到圓滑線性插值，為了同時描述剛體和非剛體變換，求其加權均值就叫混合（Blending）好了。

有趣的是，關於插值和混合，在 Skinning with Dual Quaternions 一文中，作者無情吐槽了動畫曲線你叫插值也就算了， 但在描述同時混合了剛體和非剛體變化的情況下你還叫插值那就過分了。。。

下面繼續，上頭提到的 DQB(x_{c}) ，文中給出的公式如下：

這裡的 hat q_{kc} 為前面提到的單位對偶四元數；N(x) 為對於點 x 的 k 個最臨近變換點， w_{k} 為權重，是一個 mathbb{R}^{3} ightarrow mathbb{R} 的映射，用於調整對應節變化的影響（徑向）範圍。

這裡再把以上的內容放一放，關鍵來了，對時刻 t 的 wrap -field W_{t} 的定義，由一組 (i = 1...n) 形變節點來描述，記為：

這裡的 dg_{v}^{i}in mathbb{R}^3 表示在canonical空間中的點坐標，其對應的變換矩陣 T_{ic} = dg^{i}_{se3} , 如果理解了之前的內容，那麼也應該知道這裡的 dg_{w}^{i} 就是控制形變影響範圍的權重，其映射定義為：

這裡的徑向權重是為了確保這個被採樣到的形變節點能夠影響到其附近的其他節點，所以與採樣的節點矩陣稀疏程度相關。到這裡對 W_{t} 的參數化定義似乎越來越清晰了。

好了，我們再續上。在之前有提醒大家注意wrap function，還包括了除了物體形變外的另一個剛性變換，例如把物體從canonical空間轉換到相機空間的變換，把其他所有剛性變換記為 T_{lw} ，則最終的 wrap function應該為：

W(x_{c}) equiv T_{lw}SE3(DQB(x_{c}))

2. Dense Non-Rigid Surface Fusion

這一小節主要描述如何用tsdf去描述模型表面，整體思想和 啥是KinectFusion 裏的類似，說說不太一樣的地方，主要有兩個：

第一：在KinectFusion中，tsdf的值是在世界空間中計算的，為了方便理解，在這裡可以對應我們的canonical空間，但這篇DynamicFusion引入了一個 projective tsdf ，簡寫成 psdf ，其實是在相機空間下計算 tsdf 值，怎麼實現呢？上 W_{t} ！

在時刻t，將每個體素中心 x_{c} ，通過 W_{t} ，轉換到相機空間中，即：

然後就有：

熟悉又陌生的感覺。來看一看，K是相機內參， D_{t} 是深度， x_{t} 之前求得了， 像素u_{c}=pi (Kx_{t}) ， 這裡的pi() 表示由相機空間到裁剪空間的投影，方便理解的話可以看作圖片空間， z 就是相機空間下的z分量。所以上面式子的意思就是，將體素（中心點）轉換到相機空間，再投影到圖片空間，找到相應的像素點，然後就可以依據當前的深度圖，找到物體表面（如果有的話）這個點在相機空間下的位置，然後同體素中心點相減，得 psdf 的值。

為啥要在相機空間下求解所謂的 psdf 呢？作者給出的解釋是：

使得canonical 空間中的點的 tsdf 的更新可以直接由形變後的相機空間下的 psdf 的更新而得到，而不需要對經過形變的 tsdf 進行重新採樣。簡而言之就是反正要得的是相機空間下，經過旋轉平移形變後的表面模型，那我直接用投影后的 psdf 去描述豈不妙哉。

第二：在給每個 psdf 更新權重的時候，也略有不同，多了一個線性關係： w(x)propto frac{1}{k} sum olimits_{iin N(x_{c})} | dg^i_{w} - x_{c} |_{2}

可見以上兩個變化都是為了應對物體自身運動和形變而採取的。

3. Dense Non-Rigid ICP Data-term

完成了對 W_{t} 的參數化後，那麼這個 W_{t} 是怎麼求得的？ICP？沒錯，但不僅僅是ICP，文中列出了這樣一個能量方程，可以理解為作者設計了一個誤差計算方法，目標是要最小化這個誤差：

這裡的 V 是當前的重建的模型表面。 Data() 是model-to-frame的ICP cost function，直觀理解可能要做個差，具體怎麼構建這個cost function就是一會要討論的內容。 外加正則項 Reg() ，類似於對形變節點之間多了一層對邊的約束，為什麼這麼做，後面會有說到。先看 Data() 為了方便梳理，列下序號:

1. 首先來看下這個 V ， 文中說是從canonical volume下的零水平集（zero level set）提取。關於水平集的描述，推薦參看 level set （水平集）演算法是什麼？ 。 那麼這裡所謂的 「從零水平集提取」 即canonical空間中， tsdf 趨於 0 的點，也就是模型表面的那些點啦，以點-法線的形式存儲，記為 hat V_{c} equiv {V_{c}, N_{c}} ；

2. 隨後就是將 hat V_{c} 轉到相機空間下，即： hat V_{w} leftarrow W_{t} hat V_{c} ；

3. 將 hat V_{w} 渲染到圖像空間下，記為 Omega ，這樣就獲取了我們所預測出來的模型可見區域（currently predicted to be visible）。意思就是說，如果按照 W_{t} 變換後，那麼當前幀下我看到的應當就是這個 Omega 圖像；

4. 這裡的可見區域圖像 Omega 中，像素 u 對應的 v_{c} 就是canonical空間下，預測出來的可見的點，記為 v(u) 。可以看到，這實際上是一個匹配點的篩選過程；

5. 隨後帶入到wrap function中就可以獲得相應的變換矩陣 ilde T^u = W(v(u)) ，所以，預測到的可見模型，在相機空間下的點-法線對應當為： ilde{v}_{u} = ilde{T}^{u}v(u)， ilde{n}_{u} = ilde{T}^{u}n(u) ；

6. 以上是我們預測並篩選出的，由 W_{t} 變換後，得到的相機空間下的點-法線對 { ilde{v}, ilde{n}} ，然後來找實際情況下，由深度圖獲取的點法線對，找到後求個loss，就構成了我們的 Data() 了；

7. 要用深度圖去找對應相機空間下的點 vl(u) ，首先要找到像素點 ilde{u} ，(注意：不是之前 Omega 下的 u ）這個： ilde{u} = pi(K ilde{v}_u)

這裡的 pi() 是投影映射，將相機空間下的點投影到圖像空間，這就有了 ilde{v}_u 對應的 深度圖下的像素 ilde{u}；

8. 這個轉換也出現多次了哈，在KinectFusion中也是：

將之前求得的 ilde{u} 帶入進去，得到的 vl_{ ilde{u}} 就由深度圖獲取的當前相機空間下的對應點；

9. 該有的都有了，如何構建這個cost function？稍停頓下，再翻回之前的 啥是KinectFusion 給出的是：

可以看到同時考慮了預測點和實際深度圖獲取的點的距離和夾角，本文在此基礎上，採用了Tukey penalty，記為：

括弧裡頭的意義是一樣的，重點在Tukey penalty，它是一個魯棒回歸方法，定義為:

這裡的 ho^{}() 就是 psi_{data} 咯。另一種 Huber penalty 也用在隨後的 Reg() 構建中。為啥選用魯棒回歸，應該是因其對離羣（outlier）數據不敏感，另外，我們知道這裡處理的數據是不完整的（只是「當前可見」的數據），這也是為什麼作者還要加入 Reg() 的重要原因。

4. Wrap-field Regularization

以上我們約束的都是當前可見區域的點，那麼對於canonical空間中，那些當前不可見的點就沒有以上描述的關聯約束了，意味著缺失了一些數據，而當前缺失的那些數據，可能在隨後的幀中，作為新觀察到的數據出現。如何去為那些當前看不到的節點添加約束呢？

為變形節點之間添加類似一個邊約束，然後作為整體的cost加到能量方程裏去。並用Huber penalty構建：

alpha_{ij} 是一個描述點 i he j 之間構成的邊，所對應的權重，有： alpha_{ij} = max(dg_{w}^{i}, dg_{w}^{j}) 。

至於Huber penalty的定義：

同樣的，這裡的 ho 指的就是 psi_{reg} 。

很顯然，理解這個公式的關鍵是這裡出現的新參數 varepsilon ，它指是連接分級變形節點之間的邊集。前面有說到，對於當前幀觀測不到的點，怎麼去建立約束，文中提出通過每一幀變形節點 集N_{wrap} 的更新，構建一個層級正則節點集，記為: N_{reg} = {r_{v}, r_{se3}, r_{w}} ，當level為0，即是 N_{wrap} ，那麼這個邊集 varepsilon 就是從level為0，即N_{wrap} 中的點開始，到其k個最鄰近的來自下個level（即 N_{reg} ）中的點形成的邊構成。

知道了 varepsilon ，又出現了問題，如何構建並更新這個 N_{reg} ?

5. Inserting New Deformation Nodes & Updating the Regularisation Graph varepsilon

由於每一幀的變化， N_{wrap} 是保持持續更新的，總會由新的變形節點加入到其中，對於當前幀中，所不支持的物體表面頂點，按照 epsilon 的距離為半徑進行採樣得到的點集 hat{dg_{v}} ，DBQ混合初始化後與上衣幀的形變點集合併就DataData Data() Data()

注意：這裡的 epsilon 和我們接下來要說的 varepsilon 是完全不同概念喲，只是符號看起來略像，別看岔了。

N_{reg} 實際上是對當前形變點集 dg_{v} 的二次採樣。

重點來了，採樣半徑是： epsilon eta^{l}, eta > 1

這裡的epsilon是之前的採樣半徑，指數 l 就是level，顯然當 l = 0 時，就是原來的 N_{wrap} 。

所以這裡的邊集 varepsilon 就是level = 0 的點到 level = 1的最鄰近k個點構成。

至此 Data() 和 Reg() 都得到了解釋。本文的閱讀也暫到這了，感謝能夠讀到這的朋友。

至於代碼實現，github上有找到一個實現，我大致看了下，作者又增加了好幾個依賴庫，試著編了下，由於各種庫的版本匹配，出現了這樣那樣的問題，時間有限也就放棄啦，哈哈哈。感興趣的朋友可以在github上檢索dynamicfusion應該就能找到這個。如果有時間我想自己寫個輕量級的實現，可能運行效率不高，但該有的都有 ... 希望能做吧 ...

6. Reference & Acknowledgment

grail.cs.washington.edu

cs.utah.edu/~ladislav/k

web.as.uky.edu/statisti

十年磨一劍：DynamicFusion: Reconstruction and Tracking of Non-rigid scenes in real-time


推薦閱讀：
相關文章