論文題目：Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments
開源代碼：https://github.com/jbehley/SuMa
作者：任乾
之前我們介紹過SuMa++，它是一個語義SLAM，借助語義提高了比對精度并移除了動态物體，最終提高了點雲地圖的品質。而本文介紹的SuMa則為SuMa++提供了基本的SLAM架構，算是它的基礎版，它包含了很多細節方面的介紹和實作方法，為了更好地了解這些方法，我們有必要對這篇文章做個介紹。
既然有了增強版，這裡就隻介紹原理了，效果就不做過多介紹，感興趣的可以去論文中看
一、前傳
這篇文章的核心就是使用Surfel地圖去實作前端裡程計和閉環檢測，是以了解這篇文章的核心就變成了了解Surfel地圖。
Surfel地圖并不是作者原創，但是該作者第一次把它用在室外大場景三維SLAM中，是以為了摸清Surfel的來龍去脈，我們對它的曆史做一個梳理。
Surfel地圖最早是用在基于RGB-D相機的三維重建任務中的，在Surfel出現之前，基于RGB-D的三維重建以KinectFusion[1]為主，後來受到[2]的啟發，ElasticFusion出現了，這是Surfel地圖第一次被完整表述。這種地圖表示方式出現之前，三維重建都是存儲關鍵幀，然後以關鍵幀作為頂點建立機率圖然後做圖優化，這種屬于優化軌迹，而Surfel地圖引入之後，優化的元素變成了地圖中實際的點(當然不是單純的三維點，而是以一種特定的組織形式)，以地圖一緻性作為最終的優化名額，這樣不僅提高了地圖精度，而且不會像優化軌迹那樣因為在同一個地方反複走，雖然地圖其實并沒有增加，但是優化的機率圖卻越來越大。這種優點對slam來講而已同樣适用，是以很快就被引入slam領域，[4][5][6]就是典型的例子，但這些都是在室内小場景，我們自然會想到對于室外大場景，它的作用是怎麼樣的呢？是以就有了本篇論文。
二、算法介紹
1. surfel介紹
既然SuMa其實就是Surfel方法在室外大場景的應用，更具體說是ElasticFusion方法在室外大場景的應用，那麼我們就可以按照ElasticFusion的思路先把流程捋一捋，這樣就可以更好地了解本文的方法了。
先看下surfel是個啥吧
ElasticFusion的核心流程如下圖所示，可以總結為以下幾步：
1)ElasticFusion 通過 RGB-D 圖像配準計算位姿
2)計算相機位姿如果誤差大于設定門檻值，表示跟蹤失敗，啟動重定位算法；如果誤差小于設定門檻值，則進入下一部分。
3)利用 Random Ferns 進行全局的回環檢測算法，檢測是否存在全局的回環，如果存在全局的回環，計算得到相對位姿變換後，在圖像中均勻抽取一些點，建立限制，優化 node 參數。
4)在第 3 部中如果不存在全局的回環，則檢測是否存在局部的回環，如果存在局部的回環，則同第 3 步，進行位姿估計，并且建立限制，優化 node 參數。
5)計算得到相機位姿後，将目前幀的點雲和重建好的做融合，融合使用 openGL 的 shading language，如果在存在局部的或者全局的回環，在使用 openGL 進行點的融合時候，将優化之後的節點變量，作用于全部的點。
6)融合到全局模型中後，再用 openGL 投影得到目前視角下可以看到的點，用來對下一幀圖像配準。
作者用一張圖梳理了自己的算法流程
步驟标的明明白白，我們就按照這個來一一介紹
1)處理點雲
這個處理就是把點雲從三維展開成二維，就是把點雲從正前方切開，按照從0到360度的順序再給他平鋪開，從三維到二維的投影可以按下式進行
投影之後，要計算頂點和向量(這兩個是surfel地圖的元素)，點雲、頂點和向量三者的關系如下圖所示，其中P就是原始點雲，Vd是頂點，Nd是向量。
2)局部地圖
局部地圖是用來給目前幀做比對的，把目前幀和局部地圖做比對比跟上一幀做比對要精确，如果是第一幀，則直接作為局部地圖使用
3)位姿更新
把目前幀和局部地圖使用ICP方法做比對，獲得位姿之後，再按照目前位姿做投影去更新局部地圖
4)更新surfel地圖
這個surfel地圖就是把曆史幀看到的所有資訊包含進去，每次有新的關鍵幀進來，它就要重新對每個surfel單元做初始化，因為實際效果顯示，重新初始化surfel地圖比在原有surfel基礎上對地圖做更新要快
5)閉環檢測
把目前幀按照位姿投影到surfel地圖，如果和時間相隔較遠的地圖部分有重疊，說明檢測到閉環
6)閉環驗證
如果和目前幀相似的地圖node是時間t添加的，則把t附近的node也找出來，然後把目前幀的node和地圖的node建立關聯
7)後端優化
如果檢測到閉環，則在後端優化的獨立線程裡去做優化，之後更新地圖。
上面這些步驟中，很多在原論文中都有對應的公式，在這裡貼公式也沒啥意思，大家自己去論文中看吧。
[1]. KinectFusion: Real-Time Dense Surface Mapping and Tracking
[2]. Real-time 3D Reconstruction in Dynamic Scenes using Point-based Fusion
[3]. ElasticFusion: Dense SLAM Without A Pose Graph
[4]. Elastic LiDAR Fusion: Dense Map-Centric Continuous-Time SLAM
[5]. Multi-Resolution Surfel Maps for Effcient Dense 3D Modeling and Tracking
[6]. Online loop closure for real-time interactive 3D scanning