KinectFusion解析

  版權聲明：本文為博主原創文章，未經博主允許不得轉載。   三維重建是指獲取真實物體的三維外觀形貌，並建立可復用模型的一種技術。它是當下計算機視覺的一個研究熱點，主要有三方面的用途：1）相比於二維圖像，可以獲取更全面的幾何信息；2）在VR/AR中，建立真實和虛擬之間的紐帶；3）輔助機器人更好的感知世界。傳統的三維重建方法主要是SfM（Structure from Motion），通過一系列不同位置采集的圖像，離線計算出三維模型。帝國理工和微軟研究院在2011年提出的KinectFusion開啟了用RGBD相機實時三維重建的序幕。本文主要參考KinectFusion的論文[1-2]，解析它的算法流程。   KinectFusion由四部分組成（圖1）：首先，處理采集到的原始深度圖，獲取點雲voxel的坐標以及法向量坐標；接着，根據當前幀的點雲和上一幀預測出的點雲計算當前相機的位置姿態；然后，根據相機位置姿態更新TSDF值，融合點雲；最后根據TSDF值估計出表面。

圖1： KinectFusion整體算法流程。

  KinectFusion描述三維空間的方式叫volumetric representation。它把固定大小的一個空間（比如$3m\times3m\times3m$）均勻分割成一個個小方塊（比如$512\times512\times512$），每個小方塊就是一個voxel，存儲TSDF值以及權重。最終得到的三維重建就是對這些voxel進行線性插值。具體算法如下文所示。

1. 原始數據的處理

  流程和公式如圖2所示。先對原始深度圖\(R_k\)進行濾波降噪，這里選擇雙邊濾波，目的是保持清晰的邊界。一般的濾波是在空間域做加權平均，像素越靠近中心點，權重越高。雙邊濾波是在空間域加權平均的基礎上再對值域加權平均，即像素灰度值越靠近中心像素的灰度值，權重越高。在邊界附近，灰度值差異很大，所以雖然邊界兩邊的像素在空間域靠在一起，但是由於灰度值差別非常大，對於互相的權重很低，所以可以保持清晰的邊界，如圖3所示。

圖2：原始數據處理流程。

圖3：雙邊濾波器。

  拿到降噪后的深度圖$D_k$之后，再根據相機內參$K$，可以反投影出每個像素點的三維坐標，這就是Vertex map $V_k$。公式中$u$是像素坐標，$\dot{u}$是對應的齊次坐標。每個vertex的法向量可以很方便的通過相鄰vertex用叉乘得到。然后對深度圖降采樣，行數、列數各減一半。降采樣使用的是均值降采樣，即深度圖上四個相鄰像素的深度值被平均成一個值。構建三層金字塔的目的是為了從粗到細地計算相機位置姿態，有加速計算的效果。

2. 相機位置姿態的估計

  相機的位置姿態是用ICP (Iterative Closest Point) 求解的。ICP是處理點雲的常規手段，通過最小化兩塊點雲的差別，迭代求解出拍攝兩塊點雲的相機之間的相對位置。有不同的方式來描述點雲的差別，最常用的是point-to-point和point-to-plane兩種。KinectFusion選擇的是point-to-plane的方式，要把點到點的距離向法向量投影，如圖4所示。2001年的一篇論文[3]詳細比較了point-to-point和point-to-plane的效果，結論是point-to-plane要比point-to-point收斂速度快很多，而且更魯棒。圖5列出了[3]中的Figure15和16，比較了在兩種點雲形貌的情況下不同ICP的收斂速度和殘差。

圖4：Point-to-plane ICP.

圖5：論文[3]中不同ICP的對比。

  損失函數的公式中，$T_{g,k}$是第$k$幀圖片時相機在世界坐標系下的位置姿態，這是優化求解的對象。$\dot{V}_k(u)$是第$k$幀深度圖上像素$u$反投影出的vertex坐標，$\hat{u}$是這個vertex在第$k-1$幀圖片的投影。為什么這里$\hat{V}_{k-1}^{g}$有hat上標呢？因為這里的vertex並不是直接在第$k-1$幀深度圖上，而是第$k-1$幀融合TSDF后的預測值。如果直接用第$k-1$幀的數據，那這就是frame-to-frame的方式，會帶來累計誤差。而實際采用的是frame-to-model的方式，誤差小很多。為了加速計算，這里利用了三層金字塔，從粗到細計算，最大迭代次數分別是$[4,5,10]$。

3. TSDF的更新

  先介紹一個概念SDF(Signed Distance Function)，SDF描述的是點到面的距離，在面上為0，在面的一邊為正，另一邊為負。TSDF(Truncated SDF)是只考慮面的鄰域內的SDF值，鄰域的最大值是max truncation的話，則實際距離會除以max truncation這個值，達到歸一化的目的，所以TSDF的值在-1到+1之間，如圖6所示。

圖6：TSDF的更新。

  TSDF的具體算法也在圖6中，利用GPU並行處理各個voxel。首先把每個voxel根據計算出的相機位置姿態投影到相機上，如果在相機的視椎內，則會有一個像素點和它對應，\(D_i(p)\)是這個像素點距離表面的實際測量值，\(t_i-v^g\)則是voxel到相機的距離，兩者的差就是SDF值。然后用max truncation歸一化得到當前TSDF值。接着，用加權平均的方式更新TSDF值。voxel越正對着相機（如圖7所示），越靠近相機，權重越大，用公式表示就是：\(W(p)\propto cos(\theta)/R_k(u)\)，\(u\)是\(p\)的像。但論文[1]中也提到把權重全部設為1，對TSDF做簡單的平均，也可以取得很好的效果；而如圖6算法第12行，設置max weight后，可以去除場景中動態物體的影響（這一點沒有特別想明白）。

圖7：TSDF的權重和相機觀察方向的關系。

4. 表面的估計

  更新完TSDF值之后，就可以用TSDF來估計voxel/normal map。這樣估計出來的voxel/normal map比直接用RGBD相機得到的深度圖有更少的噪音，更少的孔洞（RGBD相機會有一些無效的數據，點雲上表現出來的就是黑色的孔洞）。估計出的voxel/normal map與新一幀的測量值一起可以估算相機的位置姿態。具體的表面估計方法叫Raycasting。這種方法模擬觀測位置有一個相機，從每個像素按內參\(K\)投射出一條射線，射線穿過一個個voxel，在射線擊中表面時，必然穿過TSDF值為一正一負的兩個緊鄰的voxel（因為射線和表面的交點的TSDF值為0），表面就夾在這兩個voxel里面。然后可以利用線性插值，根據兩個voxel的位置和TSDF值求出精確的交點位置。這些交點的集合就呈現出三維模型的表面。

參考文獻：
[1] Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion: Real-time dense surface mapping and tracking[C]//Mixed and augmented reality (ISMAR), 2011 10th IEEE international symposium on. IEEE, 2011: 127-136.
[2] Izadi S, Kim D, Hilliges O, et al. KinectFusion: real-time 3D reconstruction and interaction using a moving depth camera[C]//Proceedings of the 24th annual ACM symposium on User interface software and technology. ACM, 2011: 559-568.
[3] Rusinkiewicz S, Levoy M. Efficient variants of the ICP algorithm[C]//3-D Digital Imaging and Modeling, 2001. Proceedings. Third International Conference on. IEEE, 2001: 145-152.