ElasticFusion 解析

泡泡機器人ElasticFusion講解PPT：
http://pan.baidu.com/s/1eSDuhZs

文章評價

1、基於 RGB-D 稠密的三維重建，一般使用網格模型融合點雲，ElasticFusion 是爲數不多使用 surfel 模型表示的。
2、傳統的 SLAM 算法一般通過不斷優化相機軌跡或者特徵點的方式，來提高軌跡估計或者重建的精度，這篇文章採用不斷優化重建的 map 的方式，提高重建和位姿估計的精度。
3、優化採用 deformation graph 的形式，和 DynamicFusion 中優化的方式如出一轍。
4、融合了重定位算法（當相機跟丟時，重新計算相機的位姿）。
5、算法使用 OpenGL 對點雲進行更新、融合、顯示和投影。
6、算法融合 RGB-D 圖像進行位姿估計，對於 RGB 圖像通過顏色一致性約束計算位姿，對於點雲通過 ICP 算法計算位姿，ElasticFusion 融合了二者。
7、適合重建房間大小的場景，代碼沒有做優化，重建較大場景時，代碼不能適用。

後續博客給出代碼結構。

文章和 2009 年 ICCV 上發表的文章 “In-hand Scanning with Online Closure” 思路很類似

算法流程

1、ElasticFusion 通過 RGB-D 圖像配準計算位姿，根據輸入點雲利用 ICP 算法配準計算位姿的方法參見博客：
http://blog.csdn.net/fuxingyin/article/details/51425721

2、計算相機位姿如果誤差大於設定閾值，表示跟蹤失敗，啓動重定位算法；如果誤差小於設定閾值，則進入下一部分。

3、利用 Random Ferns 進行全局的迴環檢測算法（後續博客單獨介紹），檢測是否存在全局的迴環，如果存在全局的迴環，假設當前幀爲 Icur$I_{cur}$ 檢測到和第 i$i$ 幀存在迴環；再利用第一部中的跟蹤算法計算當前幀和第 i$i$ 幀之間的位姿，計算得到位姿變換後，在圖像中均勻抽取一些點，建立約束，優化 node 參數（關於如何優化 map 後續文章還會展開介紹）。

4、在第 3 部中如果不存在全局的迴環，則檢測是否存在局部的迴環（局部迴環檢測算法後續會展開介紹），如果存在局部的迴環，則同第 3 步，進行位姿估計，並且建立約束，優化 node 參數。

5、計算得到相機位姿後，將當前幀的點雲和重建好的做融合，融合使用 openGL 的 shading language，如果在存在局部的或者全局的迴環，在使用 openGL 進行點的融合時候，將優化之後的節點變量，作用於全部的點。

6、融合到全局模型中後，再用 openGL 投影得到當前視角下可以看到的點，用來對下一幀圖像配準。

surfel 表示模型

實時的 RGB-D 重建一般用 KinectFusion 算法中提到的 TSDF 模型，這篇文章用 surfel 模型（點表示模型），示意圖如下：

對於每個點，存儲了:
點的位置信息：(x,y,z)$(x,y,z)$
面片得半徑: r$r$
法向量： n$n$
顏色信息：(R,G,B)$(R,G,B)$
點的獲取時間信息：t$t$

在進行點的融合更新時：點的位置信息，法向量，和顏色信息的更新方法類似於 KinectFusion 採用加權融合的方式，面片的半徑通過場景表面離相機光心的距離的求得，距離越大，面片的半徑越大。

Deformation Graph

由於每次重新初始化一個新的 deformation graph 要比保持更新一個已有的計算量小，並且簡單可行，對於每次新獲取的點雲融合後，初始化一個新的 deformation graph。

Deformation Graph 結構

deformation graph 由一些 nodes 組成，node 是在重建好的點均勻抽樣得到，如上圖所示，紅色的表示抽取的 node，黑色的表示重建好的其它的點，node 的數量和重建好點的數量成正相關。

每個 node 包含了本身的位置信息 Gng$G_g^n$ ，待優化的仿射變換參數 GnR$G_R^n$ 和 Gnt$G_t^n$ ，利用局部迴環檢測和全局迴環檢測建立的約束優化參數 GnR$G_R^n$ 和 Gnt$G_t^n$ 。

Deformation Graph 構建

新獲取的點雲融合到全局模型中後，每次初始化新的 Deformation Graph，每次初始化新的 Deformation Graph 要比保持更新同一個 graph 計算量小而且還要簡單可行。

均勻抽取重建好的模型中的點來初始化 Deformation Graph，抽取點的個數和重建好的模型點的個數成正相關，由於每次新的點添加到模型時是按照時間的先後順序進行的，均與抽取 Deformation Graph 的點也是按照時間先後順序排列的。

抽取的 Deformation Graph 點之間建立連接，根據 Deformation Graph node 的時間來建立連接，而非根據 Deformation Graph node 空間上的關係建立連接，按照空間關係建立連接，會造成 ACTIVE 和 INACTIVE 交疊的點相互干擾，並且 Deformation Graph 是將 ACTIVE 點向 INACTIVE 點對齊，需要按照時間關係抽取。關於 ACTIVE 點和 INACTIVE 點定義見下文。

建立模型點和 Graph nodes 的圖連接

建立每個模型點和 Deformation Graph nodes 圖連接：

對於模型中點每個點，在 Graph nodes 中時間最近的節點 c$c$ ，

找到時間上最相近的點後，和 Graph nodes c$c$ 前後 α$\alpha$ 個節點計算距離，並且選取距離最近的 k$k$ 個節點相連接。

建立連接後根據和每個 node 之間的距離關係計算更新模型點的權值。

根據模型點和 Graph nodes 的圖連接優化模型點

模型中的點按照和 node 之間的距離關係，按照加權平均的方式，用優化之後的 GnR$G_R^n$ 和 Gnt$G_t^n$ 更新位置和法向量。

上式中 Msp$M_p^s$ 是模型中的點座標（非 node）
上式中 M^sp${\hat{M}}_p^s$ 表示 node 作用後的座標
GnR$G_R^n$ 和 Gnt$G_t^n$ 指建立連接 node 優化後的參數
wn(Ms)$w^n(M^s)$ 是node n$n$ 的權值

法向量更新如下所示：

權值計算公式如下：

在 Msp$M_p^s$ 周圍選取 n+1$n+1$ 個 nodes，每個 node 的權值和 node 到 Msp$M_p^s$ 的距離成負相關，dmax$d_{max}$ 指和第 n+1$n+1$ node 的距離。

對於模型點和 Graph nodes 建立連接，並且對模型點做位姿優化，在 openGL 中的 shader 中進行。

局部迴環檢測算法

當相機移動的時候，存在迴環，如果檢測不出來，重建出來的結果會有重影。

如上圖，相機開始不斷地向左移動，移動一段距離後，返回到以前經過的位置，如第2幅圖所示。

局部的迴環檢測算法是把重建好的點，按照時間節點劃分成 ACTIVE 和 INACTIVE，ACTIVE 是最近時間內重建好的點，INACTIVE 是以前重建好的點，根據第一步計算得到的位姿，將點雲向相平面做投影，可以計算得到兩幀點雲，將這兩幀點雲配準。

如圖 3 所示，如果兩幅點雲有交疊，存在迴環，那麼投影得到的兩幀點雲可以配準上（第一步計算的誤差小於一定閾值，Hessian 矩陣特徵值大於一定閾值，詳見論文）。

如果可以配準上，存在迴環，則抽取一些點建立約束
建立的約束如下表達式所示：

Qp=(TcurP(u, Dat); TinaP(u, Dat); tcur; tina)=(QPs; QPd; ts; td)$Q_p=(T_{cur}P(u,D_t^a);T_{ina}P(u,D_t^a);t_{cur};t_{ina})=(Q_s^P;Q_d^P;t_s;t_d)$

其中 ：
Tcur$T_{cur}$ ：當前幀的 pose。
p(u, Dat)$p(u,D_t^a)$ ：通過上一幀 pose 從模型投影得到的點。
Tcurp(u, Dat)$T_{cur}p(u,D_t^a)$ ：active 點座標。
Tinc$T_{inc}$ ： 將投影得到的相機系下的 active 點 align 到世界系下的 inactive 點的 pose，pose 通過將世界系下的 inactive 點設爲配準的模型，將相機系下的 active 點設爲待配準的幀獲得。 TinaP(u, Dat)$T_{ina}P(u,D_t^a)$ 爲世界系下 inactive 點座標。
tcur$t_{cur}$ ：當前幀的時間
t$t$ ：對應的 INACTIVE 點的時間（模型投影點）

約束的含義是將 ACTIVE 點對齊到 INACTIVE 點。

全局迴環檢測

後續文章給出全局迴環檢測 Random Ferns 的論文和代碼解析，ElasticFusion 重定位算法也是用的 Random Ferns 算法。

這裏說下爲什麼有了局部的迴環檢測，爲什麼還要用全局的迴環檢測。

當相機移動距離或者角度較大時，因爲累積誤差的作用，當存在迴環時，ACTIVE 和 INACTIVE 對應點不能交疊，從 ACTIVE 和 INACTIVE 投影出的點也解算不出來位姿變換來（兩幀配準誤差大於閾值），所以，這個時候需要全局的迴環檢測。

同樣全局的迴環檢測建立約束：
Qp=(TcurP(u, Dat); TFernP(u, Dat); tcur; tFern)=(QPs; QPd; ts; td)$Q_p=(T_{cur}P(u,D_t^a);T_{Fern}P(u,D_t^a);t_{cur};t_{Fern})=(Q_s^P;Q_d^P;t_s;t_d)$

其中：
Tcur$T_{cur}$ ：當前幀 pose。
p(u, Dat)$p(u,D_t^a)$ ：通過上一幀 pose 從模型投影得到的點。
TFern$T_{Fern}$ ：檢測到的迴環幀的 pose。

tcur$t_{cur}$ ： 當前幀時間戳。
tFern$t_{Fern}$ ：迴環幀時間戳。

含義：將當前幀點雲對齊到 Fern 的點雲

通過局部和全局迴環檢測建立的約束優化 node 參數

用全局和局部回函檢測建立的約束優化節點的參數 GnR$G_R^n$ 和 Gnt$G_t^n$ ，優化的目標函數如下：

優化過程中，保持仿射變換參數 R$R$ 是單位正交的

優化過程中，保持相鄰的 node 間參數的連續性

和模型點的更新策略一樣，用公式 (8) (9) (10) 更新建立的約束點的座標 ϕ(Qps)$\varphi (Q_s^p)$ ，下式的含義是，將 source 點向 destination 點做優化，將兩次重建好的點對齊。

固定 destination 點，優化前後 destination 點不能變化太大，改變 source 點座標，向 destination 點對齊。

綜合以上優化以下目標函數

優化後的 deformation graph 參數，再用 openGL shader 作用於模型中其它點。

版權聲明：本文爲博主原創文章，未經博主允許不得轉載。