SuMa是一個完整的激光SLAM框架，核心在于“基于面元”的過程，利用3d點云轉換出來的深度圖和法向量圖來作為輸入進行SLAM的過程，此外還改進了后端回環檢測的過程，利用提出的面元的概念和使用到的虛擬幀來優化回環檢測的過程。

一、SuMa 公式約定與總覽

在這篇論文中，將幀間的轉換用T表示，其中右下角帶兩個角標，分別表示轉換前后的兩個坐標系，后角標表示源坐標系，前角標表示目標坐標系：

根據SLAM里的常用表達方法，一個4×4的變換矩陣里面實際上包括兩部分：旋轉R和平移t，這兩部分也采用同樣的表示方法：

此外，還引入了與世界坐標系的轉換，表示起來只要將前角標換為W即可，并由此可以將位姿變換關系完整表示為：

和一般的SLAM一樣，這個過程也是利用前后兩幀作為輸入，輸出為幀間的位姿變換。整個過程首先會經過一個預處理的過程，這個過程會將點云預處理為兩張2d圖像，作為后續SLAM使用的圖，之后利用這兩張圖進行里程計的部分，得到幀間位姿變換T，并計算出當前幀在世界坐標系下的位置關系，利用這個關系去更新地圖物體，如果檢測出回環就進行回環的校正。

二、SuMa 預處理

預處理的部分將3d點云轉換為兩張圖，原文用的詞是vertex map和normal map，這里直接翻譯為頂點圖和法向量圖。
預處理的過程本質上就是建立一個3d到2d的轉換，原始的點云是3d的，頂點圖和法向量圖都是2d的，所以需要一個轉換關系，論文首先使用的是點云到頂點圖的轉換，對于點云中的每個點，有：

其中兩個參數分別為：

此外w和h分別表示頂點圖的大小。對于頂點圖中的每個點，論文都使用VD來表示。得到了頂點圖后，利用其相鄰的關系，就可以得到法向量圖，就像其名字一樣，法向量圖存儲的是每個位置上的法向量（normal），在計算時可以直接使用頂點圖中的相鄰元素進行計算：

為了保證計算的有效性，對于邊界的元素，如果位置超過了圖像的范圍，也就是說位置不合法了，這種情況直接不參與計算，只保留合法位置的法向量信息。

可以看出進行預處理之后，原本的3d點云就換成了兩張二維的圖像，后續的里程計和回環檢測都是在這兩個圖的基礎上進行的。

三、SuMa 地圖表示

前面看過的論文里，地圖的表示方法有很多種，最普通的基于特征點、特征線，稍微進階一些的比如基于物體的就是用的物體地圖，而LOAM也可以看作是基于邊和面的地圖，在SuMa中作者提出了一種基于面元的地圖。

面元即論文中的surfel這個單詞，面元地圖指的就是面元作為地圖中的基本元素，一個面元就相當于一個小平面，可以抽象為一個小的圓形，每個面元用一個位置向量、一個法向量和一個半徑表示，此外還保留兩個時間戳，分別是創建面元的時間和最后一次更新面元的時間。

除此之外，為了體現面元的置信度，方便后續篩選掉錯誤的面元，論文對每個面元設置了一個優勢比（odds ratio），用來體現一個面元的可靠程度，其計算方法為：

其中的odds為一個函數，具體表示為：

pstable和pprior表示面元是一個穩定面元的概率和先驗概率，這其實是一個不斷更新的過程，如果確定兩幀的兩個面元對應的是地圖上的同一個面元，那么更新時必然是一個迭代的更新，原面元法向量與待整合面元的法向量夾角越小、距離越近，對應到公式中的exp項也就越接近1，整體也就向著pstable的方向移動，從而讓面元的可靠程度越高。

此外，關于時間戳信息，引入這個量主要是為了將地圖中的面元區分為兩部分：動態圖和靜態圖。動態圖記為Mactive而靜態圖記為Minactive，里程計進行位姿估計的過程主要是用動態圖，而回環檢測的部分則使用靜態圖。最近更新時間超過一定閾值就將其記為靜態圖，否則放在動態圖。

四、SuMa 里程計估計

前面提到里程計估計這部分只用動態圖，也就是距離當前不太遠的內容。這部分就是利用這些內容，通過最小化一個偏差，來得到一個幀間位姿變換。論文使用的是類似于重投影誤差的方法或者說類似ICP的方法，對于當前幀的頂點圖的每個點，根據位姿變換T投影到上一幀，找到上一幀的頂點圖中距離其最近的一點，從而得到改點所在的面元，計算投影點到面元的距離并求和，最小化這個距離，就是我們優化過程的目標函數：


通過迭代優化圖中的T，從而得到一個更加準確的位姿變換T，這個過程稱為幀到模型的ICP。可以看到目標函數中是對當前幀頂點圖的所有點進行距離的統計，所以必然需要一定的篩選措施，這里除了去除投影后位置不合法的點，還將角度偏差過大和距離偏差過大的點也去除了，這里的角度偏差過大指的是法向量圖中對應位置的角度的偏差。
在優化的過程中，先用上一幀和上上幀之間的幀間變換作為初始值，之后利用高斯牛頓法進行后端的非線性優化，最終計算出幀間變換T。

五、SuMa 地圖更新

面元計算這部分，首先對當前幀的每個點，計算其所在面元的半徑，公式如下：

其中p和clamp都是函數，具體表示為：


這個半徑表示的是當前這個點所在的面元的可能的半徑，但是是不是保留這個點還需要判斷。判斷的過程首先利用前面得到的幀間變換矩陣，將當前幀的信息投影到世界坐標系下，在這一步我們需要用到前面保留的2d-3d之間的轉換關系，這個關系可以給我們指明點之間是怎樣的一個觀察方法，按照這個方法，我們沿著當前點的視線延長，看看延長線能否與一個地圖上的面元相交，如果沒有相交，那么根據當前點的信息新建一個面元添加到地圖中；如果存在一個面元，那么就檢測距離與角度的偏差值：

如果符合條件，則認為當前的點所在的面元和地圖上的面元是一個面元，如果新的測量更加準確，比如說半徑更小，就進行整合操作：

這時前面計算的半徑就能用到了，而且與位置和法向量的整合方法不同，半徑的整合是完全替代，直接用新的、更小的半徑代替前面的半徑。之后更新面元的可信度。
如果不符合條件，就調低面元的可信度，除此之外不做其它操作。在一次更新之后，去除過舊的面元和可信度過低的面元。

六、SuMa 回環檢測

回環檢測的部分首先是要獲得一個候選回環幀，論文中使用的方法是遍歷靜態圖，從中找出距離當前幀最近的一幀，公式如下：

這里所謂的最近是只考慮了平移沒有考慮旋轉，這是因為旋轉在尋找距離最近的時候并不會產生影響。

得到最近的幀之后，利用存儲的位姿變換關系，可以得到這兩幀之間的旋轉和平移關系：

對于這兩個關系，旋轉可以直接拿來使用，但是平移上論文又引入了一個參數量：

這里是考慮到單純使用重疊程度作為是否出現回環的判斷標準容易出現誤判，所以這里在平移上增加了一個系數，從而產生了三個初值，從這三個初值中選一個最優值作為回環的匹配程度。
除此之外，論文還提出了一個虛擬幀，虛擬幀本質上就是投影后的候選回環幀和當前幀的整合，具體做法是先利用位姿變換關系，將候選回環幀投影到當前位置，之后采用與前面一樣的視角檢測方法，如果視線上存在更近的點，就用這個點去替代投影幀上的點，以此構成虛擬幀。
得到虛擬幀之后，采用相同的方法計算幀間殘差，因為增加了參數，所以是三個位姿去計算殘差，從中選擇最小的作為評判回環是否出現的標準。如果出現了回環，還要持續檢測后面一個時間段內的位姿變換，必須保證一個時間段內位姿的變換與候選回環幀之間保持一致性才可以認為真正出現了回環。如果真的出現了回環，就利用位姿圖進行優化，對回環進行優化。
總的來說在回環檢測這部分，首先要遍歷靜態圖，選擇距離最近的一幀作為候選回環幀，根據候選回環幀和當前幀制作虛擬幀，利用虛擬幀和三個位姿變換計算相似度，如果相似度滿足閾值且后續一個時間段內都符合條件，就認為真正出現回環，通過圖優化進行回環校正。

參考鏈接：
https://zhuanlan.zhihu.com/p/93900364
https://zhuanlan.zhihu.com/p/348253022

SuMa這篇論文提出了一個激光SLAM的框架，一方面在里程計之前增加了一個預處理的部分，將原本的3D點云轉換為兩張圖：頂點圖和法向量圖，后續的過程中就不再使用點云而是換為使用這兩張圖。另一方面是引入了面元的概念，將里程計、地圖表示和回環檢測都更換為基于面元的方法。

七、SuMa++

SuMa++則是在SuMa的基礎上引入了語義相關的內容，因為大部分的框架都是相似的，這里就簡單記錄一下。

首先SuMa++使用RangerNet++來獲得點的語義標簽，產生一個語義mask用來標記點的語義信息。其次考慮到錯誤分類會對后序的內容產生影響，SuMa++引入了一種洪泛法來消除錯誤標簽。洪泛算法以語義mask和頂點圖為輸入，輸出修正之后的語義mask，偽代碼如下：

整個過程主要是對邊緣信息的處理，首先對于語義mask中的一個點，如果在其鄰域范圍內，有其它語義信息的點，那么就認為這個點是邊緣點，將這個點去除，全部處理一遍之后，相當于將兩個物體邊緣的部分去除掉了。之后再對這些去掉的空白位置，采用就近分配的原則，重新為其分配標簽。最終效果為：

除此之外，考慮到對于SLAM過程而言，制圖起著關鍵作用的，主要是靜態的物體，而非動態物體，因此SuMa++將動態物體去除掉，主要方法是在更新地圖時，檢測地圖與當前幀對應點的語義一致性，簡單來說就是如果是同一個位置但是標簽變了，那么就認為這個位置上的面元是動態物體上的一個面元，通過在可信度函數是增加語義一致性項來加速置信度的降低。

但是作者也提到了，在場景特征不那么豐富的情況下，這種過濾方法會讓特征不足，從而影響SLAM的效果。

最后SuMa++還使用了語義ICP，在里程計的目標函數部分，增加一個語義權重項，通過比較地圖點與掃描點之間的語義一致性來調整權重，也就是說不僅空間上要存在一致性，語義上也要滿足一致性。


參考鏈接：
https://blog.csdn.net/stephanezhang/article/details/115003183
https://zhuanlan.zhihu.com/p/92726781
https://blog.csdn.net/phy12321/article/details/103133981

SuMa++在SuMa的基礎上增加了語義信息，將語義信息與面元相結合，語義獲取部分使用Rangenet++來獲取語義mask，并通過洪泛算法對mask中的物體邊界部分進行修正。語義的應用方面，SuMa++利用語義的一致性，通過比較地圖點和當前幀的同一位置上的語義信息，將對制圖過程意義不那么明顯的動態物體篩除，此外還將語義信息應用在里程計的目標函數中，為目標函數增加語義權重，從而體現出語義一致性。

總結

以上是生活随笔為你收集整理的【论文阅读】SuMa SuMa++的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。