Real-Time Loop Closure in 2D LIDAR SLAM 翻譯和總結（二）

5.2 分支定界掃描匹配

像素級（pixel-accurate）匹配優化公式爲：
$\xi^*= argmax(\xi \in \omega)\sum_{k=1}^KM_{nearest}(T_\xi h_k) \tag{BBS}$
上式中$\omega$表示搜索窗口，$M_{nearest}$是將轉換後的座標M拓展到$R^2$空間。通過將計算得到的座標舍入到距離最近的柵格中，然後將該柵格點拓展到其對應的Pixel（附近距離最近的其他柵格點？）。匹配的質量可以進一步通過公式$CS$來優化提高。適當的選取遍歷步長，可以有效提高效率。文中選擇當激光雷達以最大的探測距離$d_{max}$掃描時移動不超過一個像素的寬度($r$, 也是一個最小分辨率柵格的寬度？) 爲限制時對應的方向轉角爲角度遍歷步長$\delta_{\theta}$。基於餘弦定理，可以得到：$d_{max} = max( k = 1, ..., K) ||h_k|| \tag6$$\delta_{\theta }= arccos(1- \frac{r^2}{ad_{max}^2}) \tag7$接着，可以基於給定的位移和角度搜索窗口大小計算積分次數，比如$W_x=W_y=7m，W_\theta=30\degree$:$w_x=\frac{W_x}{r}, w_y=\frac{W_y}{r}, w_\theta=\frac{W_\theta}{r} \tag8$這會產生一個有限集合$\omega$，基於在當前最新估計位置$\xi_0$爲中心的搜索窗口：$\bar{\omega} = \{-w_x, ..., w_x \} \times \{-w_y,..., w_y \} \times \{-w_\theta,...,w_\theta \} \tag9$$\omega = \{\xi_0 + (rj_x, rj_y, \delta_\theta j_\theta):(j_x, j_y, j_\theta) \in \bar{\omega}\} \tag{10}$基於傳統方法進行暴力遍歷尋找最優的匹配$\xi^*$實現較爲簡單，可見下述算法1，但是窗口越大，搜索效率越慢：
算法1:暴力匹配方法

與上述方法不同的是，作者採用一種分支定界的方法來實現在更大的搜索窗口有效計算$\xi^*$。算法2描述了此方法的一般實現過程。該方法首次在混合整型線性規劃問題中使用[17]。關於這方面的文獻較多，可以參見[18]中的文獻概述。
算法2：一般分支定界算法
該方法的主要思想是，在一個樹結構裏，根節點表示所有可能的概率（$\omega$），孩子節點表示概率子集。每個節點的孩子節點組成雙親節點的一個分支，因此這些孩子節點一起可以表示相同的概率集合。葉節點是單獨的，每一個表示單獨的結果。算法是嚴格的，只要內部節點c的得分$score(c)$是它元素（其孩子）得分的上界，則和暴力匹配方法一樣可以提供相同的結果。在這種情況下，無論什麼時候一個節點成爲邊界，則比已知的最優的結果更優的結果不會出現在其他子樹中。

具體而言，論文中的算法過程可分爲以下步驟：1）節點選擇（node selection）; 2) 分支（branching）; 3) 計算上界（computation of upper bounds）。

1）節點選擇（Node Selection）：作者採用深度優先搜索（DFS）作爲無先驗信息下的默認搜索方式：此時搜索效率高低很大一部分取決於樹的結構。這由兩件事情決定：一個是好的上界； 一個是好的當前結果。後者可以有助於深度優先搜索，這樣能夠很快的對許多葉子節點進行評估計算。因爲在迴環里加入低匹配度的約束可能反而導致錯誤，所以作者也設了最低分數閾值，低於此閾值的匹配結果不會在優化過程中使用。實際中的得分並不會頻繁超過此閾值，這能夠降低節點選擇或者尋找初始啓發解的重要程度（這樣可以提高節點的篩選效率）。基於深度優先搜索遍歷孩子節點的順序，計算每個孩子節點得分的上界，並首先遍歷擁有最大邊界的最有可能匹配的孩子節點。該方法具體見算法3所示。
算法3：深度優先的分支定界算法

2）分支規則（Branching rule）：樹中的每個節點都用一個整型元組表示$c=(c_x, c_y,c_\theta, c_h)\in Z^4$。高度爲$c_h$的節點除了表示一個旋轉之外，還一起表示$2^{c_h}\times2^{c_h}$個可能的位移變換：

$\bar{\bar{\omega}}_c = (\{(j_x, j_y)\in Z^2:c_x \leq j_x < c_x + 2^{c_h}, c_y \leq j_y < c_y +2^{c_h}\}\times{c_\theta})\tag{11}$

$\bar\omega_C =\bar{\bar{\omega}}_c\bigcap\bar\omega \tag{12}$

葉節點高度爲0，即$c_h=0$, 對應最終的匹配結果$\xi_c=\xi_0+(rc_x, rc_y, \delta_\theta c_\theta) \in \omega$。

在圖示的算法3中，包含所有可能結果的根結點，不會顯式的出現，而是會以一個固定的高度$h_0$（此相當於初始的粗分辨率，將搜索窗口均勻的粗劃分形成初始節點集合$C_0$；具體大概設爲多少論文沒說）分爲一組覆蓋搜索窗口的初始節點$C_0$集合：

$\bar\omega_{0,x}=\{-w_x+2^{h_0}j_x:j_x\in Z, 0 \leq 2^{h_0}j_x\leq2w_x\} ,$

$\bar\omega_{0,y}=\{-w_y+2^{h_0}j_y:j_y\in Z, 0 \leq 2^{h_0}j_y\leq2w_y\},$

$\bar\omega_{0,\theta}=\{j_\theta\in Z, -w_\theta \leq j_\theta\leq w_\theta\},$

$C_0 = \bar\omega_{0,x}\times\bar\omega_{0,y}\times\bar\omega_{0,\theta}\times{h_0}.\tag{13}$

基於給定的節點$c(c_h>1)$, 將其分爲不超過4個高度爲$c_h-1$的孩子節點：
$C_c=((\{c_x, c_x+2^{c_h-1}\}\times{\{c_y,c_y+2^{c_h=1}}\}\times c_\theta)\bigcap\bar\omega)\times\{c_h-1\} \tag{14}$
上式可以理解爲，每當C集合中出現$score(c) > best\_score$時，則對當前節點c進行分支，分支的規則是：$c_\theta$保持與c節點一致，對$c_h-1$，此相當於提高遍歷分辨率，也就是下沉到更深的節點（DFS思想），從而形成新的更高分辨率的子窗口集合，這裏不超過4個就是因爲以2分的思想（$2^{c_h-1}$）來處理，所以x, y方向各最多增加2個孩子節點，則一共最多分爲4個孩子節點。然後新的C集合變爲$C_c$與上一個C集合的並集繼續按照得分高低遍歷循環。
3）計算上界（Computing upper bounds）：分支定界剩下的部分是在內部節點如何有效計算上界，包括考慮到計算複雜度和上界質量。上界的具體計算公式爲：

$score(c)=\sum_{k=1}^Kmax_{ j \in\bar{\bar \omega}_c}M_{nearest(T_{\xi_j}h_k)} \geq \sum_{k=1}^Kmax_{ j \in\bar{\omega}_c}M_{nearest(T_{\xi_j}h_k)} \geq max_{ j \in\bar{\omega}_c}\sum_{k=1}^K M_{nearest(T_{\xi_j}h_k)} \tag{15}$

爲了高效率計算最大值，作者使用了柵格預計算(precomputed grids $M_{precomp}^{c_h}$)方法。在每個可能的高度$c_h$對每個柵格預計算，可以使得計算得分的複雜度與掃描點數量成線性增長關係(這樣計算得分的時間複雜度和之前的待匹配柵格圖無關，因爲已經預計算了)。需要注意的是，爲了這樣處理，作者也在整個$\bar{\bar{\omega}}_c$（當前節點對應的搜索窗口）計算最大值，這在搜索空間的邊界附近可能會大於$\bar\omega_c$框出來的範圍。

$score(c) = \sum_{k=1}^KM_{precomp}^{c_h}(T_{\xi_c}h_k), \tag{16}$

$M_{precomp}^{h}（x, y）= max_{\{x'\in[x, x+r(2^h-1)], y'\in[y,y+r(2^h-1)]\}}M_{nearest}(x', y') \tag{17}$

上式中$\xi_c$與之前的葉子節點相同。$M_{precomp}^{h}$和$M_{nearest}$有相同的像素結構，但是在每一個像素裏都存儲$2^h\times2^h$的box裏的最大值。此處$h_k$對應的是當前的scan, 每次掃描更新都會隨時間更新，$M_{precomp}^{h}(T_{\xi_c}h_k)$會在更新之前預計算當前機器人位置對應迴環搜索空間的樹結構中每個節點對應的搜索窗口裏x,y位置對應的柵格概率圖，等scan更新之後基於每個待搜索轉換關係$T_{\xi_c}$得到每個節點對應位置$T_{\xi_c}h_k$=（x, y），接着在$M_{precomp}^{c_h}$直接查找對應的概率值進行累加即可，不用重新計算。這樣的預計算的柵格結果如圖3所示：

圖3 當前節點搜索窗口下柵格大小分別爲1，4，16，64的預計算概率結果（對應$c_h$分別爲0，2，4，6）

爲了使得構造預計算柵格的計算複雜度較低, 會等每個概率柵格不再收到更新（也就是scan不再掃描到這個柵格）纔會進行預計算。接着計算預計算柵格集合，而且基於此開始匹配。

對每一個預計算柵格，計算以每個pixel爲起始的$2^h$像素寬度的下的最大值（類似於圖像中的$2^h\times2^h$的卷積核滑動濾波？）。使用這個中間結果，則下一個預計算柵格也就被順便構造出來了。

如果動態變化的一組值刪除的順序與添加的順序一致，則它們的最大值可以保持每次以$O(1)$的複雜度更新。連續的最大值保存在一個隊列裏，該隊列可以遞歸的被定義爲包含集合裏當前所有值的最大值。當第一個最大值產生時，隊列後面便會跟着所有持續更新的所有當前值的連續最大值列表。如果當前值的集合是空的，則對應的列表也是空的。基於此方法，則預計算柵格的複雜度可以控制在$O（n）$, n爲每個預計算柵格的像素pixel數量。

另外一個可選的上界計算方法是，可以在更低分辨率的概率柵格上計算得分上界，並繼續將分辨率降低一半得到下一個上界, 可參見[1]。作者認爲本文使用的上界計算方法（公式（15））增加的額外內存消耗是可接受的，所以更傾向於使用公式（15），因爲基於低分辨率概率柵格的方法相對公式（15）會產生更差的邊界值，因此對系統性能產生負面影響。

6. 實驗結果

作者使用三種數據進行了實驗驗證，一是在慕尼黑德國博物館使用Cartographer 的揹包進行掃描的數據；二是使用一般性能的掃地機器人傳感器獲得的數據；三是使用Radish數據集進行了測試，並與已經公佈的其他方法進行了對比。

1）真實場景測試：慕尼黑德國博物館

圖4展示了慕尼黑博物館2253m，1913秒的數據結果。運行機器是主頻3.2GHz的Intel Xeon E5-1650工作站，SLAM算法使用了1018s的CPU時間，2.2G內存，4個線程進行迴環匹配。總共花了1個小時跑完了數據，表示具有5.3倍的實時性能。

用於迴環優化的圖大概包含11456個節點，和35300個邊。當每隔幾個節點加入到圖中時，SPA(Sparse Pose Adjustment)優化便會允許一次。優化過程典型值爲：迭代3次，耗費0.3s。
圖4 慕尼黑博物館2D的Cartographer Map

2）真實場景測試：Neato’s Revo LDS

Neato’s Revo LDS是一個激光雷達傳感器，一般用於清掃，其價格大概在30美元。作者以2Hz的採樣率採集數據運行算法，生成了圖5所示的5cm分辨率的2D平面圖。使用激光筆測距和算法輸出的結果，比較了5條直線的長度，結果如表1所示，大致精度在每條線上是1個像素的誤差。
圖5 基於Revo LDS的Cartographer map

表1 Revo LDS 誤差

3）Radish數據集測試

作者將本文方法和其他方法進行了對比, 參考[21]， 比較了基於真值軌跡的相對位姿誤差。表2/3展示了位姿對比結果。

因爲文中採用submaps和scans間添加回環約束的方法，所以沒有具體的數據集指標量化，也很難與其他scan-to-scan的方法進行比較。表4列出了每次測試的迴環數量（包括正確約束和錯誤約束），以及精度（其中正確約束的佔比）。關於是否是正確迴環約束，作者認爲在計算SPA優化時，這些約束不超過20cm或者1度。儘管文中的scan-submap匹配步驟會添加一些錯誤的約束，但是整體上保證了SPA在所有的測試裏能夠具有大量的迴環約束（迴環數量和質量不能兼得）。此外，同時在SPA中使用了$Huber loss$處理迴環中的異常點。在Freiburg hospital的例子中，迴環中的低分辨率和低最小得分設置產生了過高的假陽性率。這可以通過增大回環中的min score設置來提升精度，但是這會降低某些維度的結果質量（迴環數量減少，某些局部誤差容易變大）。同樣，沒有針對CPU性能調試Cartographer SLAM的參數，表5提供了算法消耗的時鐘時間（機器依舊是測試1中的工作站）。
表2 位姿精度

表3 位姿精度

表4 迴環精度

表5 算法時間性能

7. 結論

作者提出了一個2D SLAM系統，包含scan-to-submap的匹配，迴環檢測，和圖優化。獨立的submao軌跡使用基於柵格的local SLAM方法。在後端，所有的掃描幀scan都會使用像素級匹配方法與附近的submaps匹配去建立迴環約束。submap和scan位姿的約束圖會在後端間斷性持續優化。當同時用GPU進行加速合併已經完成的submaps和當前submap時，操作人員可以即時預覽最終的地圖效果。並且通過實驗驗證了本文算法可以在一般性能的硬件平臺實時運行。