（每日一讀2019.10.22）cartographer論文(Real-time loop closure in 2D LIDAR SLAM)

論文：pdf

自己看了原版論文，但是後來一些內容忘記了，因此翻譯一下加上自己的一些理解。

摘要

便攜式激光測距儀(也就是LIDAR)以及實時定位和繪圖（slam）都是比較有效的一種建立平面圖的方法。實時生成和繪製平面圖能很好的評估捕獲的數據的質量。因此建立一個在有限的資源下可接入的平臺是非常有必要的。這篇論文提供了一種在mapping 平臺後臺使用的方法來實現5cm分辨率的實時繪圖和閉環檢驗。爲了達到實時閉環檢測，我們使用了分支定界法來計算scan-to-map的匹配作爲約束。

I 介紹

建立平面圖對於各種應用都是非常有用的。SLAM並不是這篇論文所主要的研究，這篇論文的contribution在於減少了計算量，提高了計算效率，對於大平面圖更加適用。

II 相關工作

1. scan-to-scan matching：會累計誤差，最終誤差會很大
2. scan-to-map matching：減少了誤差的累計（本篇論文使用方法）
3. pixel-accurate scan matching：最後有說這個算法在後臺用於將scan點集和最近的submap進行匹配，生成loop closing的約束條件。

解決局部誤差的累計有兩種方法：

1. 一種是粒子濾波法，
2. 一個基於圖的SLAM方法（graph-based）。

這篇文章使用的是後者。講一下關於graph-based的吧。

graph-based是一種工作於基於位姿和特徵的集合的方法。圖中的邊表示從觀察結果中生成的約束，節點表示的是位姿和特徵。對於優化由約束引進的誤差有很多方法，比如文中的參考文獻11，12。對於室外繪圖的系統使用詳情見參考論文13。
對於第二部分也就不多解釋了，接下來重點來了！第三，四，五部分是這篇論文的核心，基本的理論知識闡述！非常重要。

III 系統概述

谷歌的cartographer提供了一個室內地圖的實時解決方案，可以生成分辨率爲r=5cm的二維柵格地圖。系統的操作員可以在穿過建築物時看到正在創建的地圖。

經本人實驗確實可以達到上述的精度和效果

激光掃描被插入到一個最佳估計位置的submap中，該位置被認爲在短時間內足夠精確。掃描匹配發生在最近的子地圖上，因此它只依賴於最近的掃描，對於位姿的估計誤差會在全局的幀當中積累起來。

累計誤差確實存在

綜合硬件等條件，這篇論文沒有使用粒子濾波，使用的是pose optimization來處理誤差累計。

（如何使用pose optimization實現累計誤差的優化???接下來講的是關於位姿優化算法？？）

當一個子圖構建完成之後，不在有其它的scan插入這個子圖當中，這個已有的子圖會用來作爲loop closing的scan matching。所有子圖和scan點集都會被用來進行loop closure。
當一個新的laser scan加入到地圖中時，如果該laser scan的估計位姿與地圖中某個submap的某個laser scan的位姿比較接近的話，那麼通過某種 scan match策略就會找到該閉環 。

匹配的結果？指的是什麼？也就是需要存儲什麼？

每隔幾秒就進行閉環檢測，按照經驗來說，就是一個位置點被重複訪問之後就算達到閉環了.

（這個重複訪問是如何判斷的，兩個位姿的距離足夠小？？）

這樣的判斷要求閉環檢測必須在新的點被加入到submap中之前完成。如果不是，可能會導致失敗.爲了達到這樣的目的（之前提高的必選檢測在前的目的？？）,通過使用分支上界法以及對於每個完成的submap有對應的幾個預處理的網格（several precomputed grids per finished submap）.

IV 局部二維的實時定位和繪圖

我們的系統結合了單獨的局部和全局方法來實現二維slam。這兩種方法都優化了由(x，y)平動和轉動激光雷達觀測（又稱掃描）組成的姿態，$ζ=(ζx，ζy，ζθ)$。在一個不穩定的平臺上，IMU被用來估計重力方向，以便從水平安裝的激光雷達向二維世界投射掃描。

非線性優化的submap匹配：每個連續的點集被拿來和整個地圖的一部分進行匹配，就是和submap進行匹配。使用一種非線性的優化方法將submap和scan點集聯繫起來，這也是scan matching的過程。局部方法積累的誤差在之後的第五部分的全局方法去掉。

A scans

submap構造是反覆對齊掃描和submap座標幀（也稱爲幀）的迭代過程。當掃描原點爲$0∈r2$時，我們現在將有關掃描點的信息寫成$h={h k}k=1,…,k,hk∈r2$。submap幀中掃描幀的位姿$ζ$表示爲變換$t_ζ$，它將掃描幀中的掃描點嚴格變換爲submap幀，定義爲

B submaps

一個submap是由連續的幾個scans建立的，這些submap採用的是概率網格$M : r_Z × r_Z →[p_{min}, p_{max} ]$，這個代表着離散網格點中概率值。這個值可以作爲表示這個網格是否爲障礙物的概率。對於每個網格點，論文中都定義了一個相應的像素，這個像素包含了所有靠近這個網格點的scan points。

無論什麼時候一個scan點集被插入到這個概率網格中，一個包含網格點的hits集合以及一個miss集合都會被重新計算。對於每一次的hit，我們將這個最近的網格點加入hit集合，對於每一次的miss，我們將每個像素關聯的網格插入這個miss集合，對於已經在hit集合中的網格點不需要插入。
對於每一個之前沒有觀察到的網格點都會根據他們所在集合是hit還是miss賦予一個概率值$p_{hit} or p_{miss}$。
如果一個網格點已經被觀察到了，我們將會更新這個概率值。通過以下的方式（hit的修改）：

對於miss的網格點的修改也是同樣的。

灰色的網格代表的是一次scan點集的範圍，黑色的點表示hit（是障礙物的）的網格。

C Ceres scan matching

在將一個scan點集插入submap中之前，這個scan點集的位置必須使用Ceres-based的scan matcher的方法進行優化（這個優化是相對於submap的位置）。這個scan matcher目的是爲了找到一個點集位置，這個點集的位置在submap中的概率最大，將其作爲一個非線性最小二乘問題。如下：

M這個函數採用了雙三次插值法。這個函數的數學優化往往比網格分辨率優化有更高的準確性，因爲這是一個局部的優化，所以一個好的初值很重要。IMU被用來測量scan match中的角度旋轉參數theta。一個高概率的scan matcher 或者一個高的pixel-accurate scan matching方法會被用來彌補IMU不能使用的情況，儘管複雜度很高。

V 閉環的實現

因爲一個submap是有少數的幾個連續scan點集生成的，所以誤差很小。這篇論文中優化所有點集和submap位置的方法使用的是Sparse Pose Adjustment（參見原論文的[2]）。一個scan點集唄插入submap中的這個相對位置會被存儲下來，用於之後的閉環優化。除了這些位置信息，還有的包含scan點集和submap，而且這個submap不再變化的時候，都會被用來作爲閉環檢測。一個scan matcher一直會在後臺不斷的運行，當一個好的scan match被找到之後，這個相應的位置也會被加入到優化問題中。

A 優化優化問題

閉環的優化，就像scan matching 一樣，也被表示成非線性最小二乘問題。每隔幾秒，就會使用Ceres計算一個解決方法。計算方式如下：

損失函數，比如Huber loss等等。使用損失函數的目的是減少加入到優化問題中的離羣點對於系統的影響。

B Branch-and-bound scan matching

對於優化的，pixel-accurate match還是非常感興趣的。這個匹配算法公式如下：找到最佳的位置點:

其中的W表示的是一個窗口大小，M是之前M函數一個延伸。這裏的含義以及包括接下來的算法1的含義，在我理解看來就是在一個新的scan點集的周圍畫一個圓（窗口），然後不斷的修改x，y，以及角度來找一個最佳的位置。這個位置的修改不能超過一定的範圍。這個範圍是點集的最大範圍。同時角度變化step也設置如下：

計算了覆蓋給定線性和角度搜索窗口大小的整數步數.
比如$W_x=W_y=7m，且W_\theta=30度$。

我們可以得到一個基於估計值$ζ_0$爲中心的一個w集合的搜索窗口。

一個簡單的算法來尋找$ζ^*$可以很容易地公式化，見算法1，但是對於搜索窗口的大小，我們認爲它太慢了。

我們說了半天，但是作者直接在論文裏用了一個instead單詞，說最後沒有采用這個辦法，而是採用了分支定界方法來尋找最佳位置。

這個算法的主要思想是將可能性子集表示爲樹中的節點，其中根節點表示所有可能的解決方案，在我們的例子中是$W$。每個節點的子節點構成其父節點的一個分區，因此它們一起表示相同的可能性集。葉節點是單變量的；每個節點代表一個可行解。只要內部節點C的$score(c)$是其元素得分的上界，它就提供了與樸素方法相同的解決方案。在這種情況下，每當節點有界時，在這個子樹中不存在比迄今爲止已知的解還要好的解。
爲了得到一個具體的算法，我們必須確定節點選擇、分支和上界計算的方法。

分支上界法就是每個分支代表一種可能。使用DFS找到最佳的那個位置即可。他和算法1一樣都是解決的相同的問題。只不過這個節點的score是可能的最大值而已。爲了達到一個實際的算法，需要分爲以下幾步：節點選擇，分支，計算上限。對於這三步，論文中有具體講解。節點選擇採用的是DFS，對於分支算法，採用的是算法2。算法3是將節點選擇和分支結合到一起之後的算法。

• 1）節點選擇：在沒有更好的選擇的情況下，我們的算法使用深度優先搜索（DFS）作爲默認選擇：算法的效率取決於被修剪的大部分樹。這取決於兩件事：一個好的上限，一個好的當前解決方案。後一部分由dfs提供幫助，它可以快速評估許多葉節點。由於我們不想將差的匹配添加爲迴環檢測約束，因此我們還引入了一個分數閾值，低於該閾值，我們對最優解不感興趣。由於在實踐中，閾值通常不會被超過，這就降低了節點選擇或尋找初始啓發式解決方案的重要性。對於dfs中訪問子節點的順序，我們計算每個子節點得分的上界，先訪問最有前途的子節點，然後訪問最大的子節點。這種方法是算法3。

• 2）分支規則：樹中的每個節點由一個整數$c=(c_x,c_y,c_θ,c_h)$的元組來描述。高度爲$c_h$的節點組合最多$2 c_h×2 c_h$可能的平移，但表示特定的旋轉：葉節點具有高度$c_h=0$，並且對應於可行解$w 3ζ_c=ζ_0+(rc_x，rc_y，δ_θc_θ)$。
  在我們的算法3的公式中，包含所有可行解的根節點沒有顯式地出現，並在覆蓋搜索窗口的固定高度$h_0$分支成一組初始節點$c_0$
  
  在$c_h>1$的給定節點c處，我們將分支到至多四個子節點$c_{h-1}$
  

• 3)計算上界：計算內部節點上界是在計算工作量方面還是在定界質量方面都是非常重要的。我們用
  
  爲了能夠有效地計算最大值，我們使用預先計算的網格$M^{c_h}_{precomp}$。預先計算每個可能高度的一個網格$c_h$允許我們用$effor$線性計算掃描點數的分數。請注意，爲了能夠做到這一點，我們還計算最大WC，可以大於我們的搜索空間邊界附近的WC。
  
  對於葉節點，與前面一樣使用$ζ_c$。注意，$M^H_{PrimCMP}$具有與$M^n_{Enm}$相同的像素結構，但在每個像素中存儲$2 h×2 h$像素的值的最大值。圖3給出了這種預計算網格的一個例子。
  
  爲了使構建預計算網格的計算工作量保持在較低的水平，我們等待一個概率網格不再接收到進一步的更新,然後我們計算一組預計算的網格，並開始與之匹配。
  對於每個預先計算的網格，我們計算從每個像素開始的2小時像素寬行的最大值。使用這個中間結果，然後構建下一個預計算網格。
  如果按已添加的順序去除值，則可改變的值集合的最大值可在已攤銷的$O(1)$中保持最新。連續最大值被保存在一個DEGO中，它可以遞歸地定義爲包含當前在集合中的所有值的最大值，接着是最大值的第一次出現之後的所有值的連續最大值的列表。對於空值集合，此列表爲空。使用這種方法，可以在$O(n)$中計算預計算的網格。
  其中$n$是每個預計算網格中的像素數。計算上界的另一種方法是計算低分辨率概率網格，將分辨率連續減半，見[1]。由於我們的方法的額外內存消耗是可以接受的，所以我們更喜歡使用較低分辨率的概率網格，這會導致比（15）更差的邊界，從而對性能產生負面影響。(犧牲空間和部分精度節約時間)

VI 實驗

VII 總結

這篇論文闡述了一個2D的SLAM的系統，這個系統採用了閉環檢測的scan-to-submap matching，同時還有圖優化（graph optimization）。一個submap的創建是使用的是局部的，基於網格的（grid-based）SLAM方法。在後臺，所有的點集與相近的submap的匹配使用的是pixel-accurate scan matching的方法，然後建立閉環檢測的約束。這個約束圖（基於submap和scan pose的）都會週期性的在後臺被更改。這個操作是採用GPU加速將已完成的submap和當前的submap進行結合。