1 框架解讀
1.1 左側數據源：
- 激光雷達：兩次濾波會根據配置文件選擇不同的options，對傳感器數據進行兩次濾波的作用是減少了一定的數據量
- 里程計、IMU等：爲了計算出重力的方向
1.2 Local SLAM（前端）：
通過Ceres非線性優化庫來求解。Scan Matching通過匹配雷達數據得到的位姿之後，將當前幀的激光（Laser Scan）插入到子圖（Submap）中，如果當前幀是沒有運動的或者運動緩慢的甚至處於靜止狀態的，那麼可以將這種數據去除（Dropped）。這樣一幀一幀的數據處理之後，我們不斷更新，就得到了很多的子圖。後續當激光數據再次來到時，通過CSM的方法將當前幀的數據插入到子圖中。
1.3 Global SLAM（後端）：
主要就是迴環檢測方法。採用分支-定界加速的方法，構建迴環，消除子圖之間的匹配誤差。
2 論文相關工作
2.1 幾種常用的激光數據掃描匹配方法的對比
- scan to scan幀間匹配：ICP或者PL-ICP，這種方法會陷入局部極值的問題，針對大場景計算相對位姿，誤差累積會非常明顯
- scan to map幀與地圖的匹配：這種方法可以限制誤差的累積。CSM的相關匹配，其實就是暴力搜索，構建的是勢然場模型，不會陷入局部極值。CSM可以理解爲在較粗糙的層面下開始的，之後再加一個梯度優化，再進行更精細的搜索，比如使用G-N方法，該方法需要良好的初值，我們可以使用高頻的LiDAR數據，或者3D情況下使用IMU來提供。
- pixel-accurate scan精確像素匹配：精度高，但是計算量很龐大
2.1 處理剩餘的局部誤差累計的方法
-
粒子濾波
基於網格的SLAM來說，隨着地圖的增大，它很快就變成資源密集型的。
-
圖優化
通過節點與邊的約束，找到一個最優的配置
3 系統概述
之前的一系列介紹，都旨在引出cartographer算法。主要說明下面的第二點，論文提到的軟實時約束。可以理解爲並非真正的實時約束，因爲它的效率是跟不上的，我們在線跑cartographer的時候會發現它的約束格網是跟不上我們的機器人的行駛速度的。同時，我們更關注的是後端處理的軌跡優化，所以我們會把一些參數設置得更大，實時只是給出了一個初始值位姿。
然而我們在離線跑bag包的時候，我們會發現後端優化也需要一定的過程，比如截圖的.lua文件中，表示每90個節點進行一個優化。
第三部分提到了一個分支-定界加速的方法，還有一個預計算的網格。
4 Local 2D SLAM
-
scans：子圖的構成其實就是一幀一幀激光數據掃描匹配對齊的迭代過程，每一幀的激光數據轉換到子圖當中，其實就是一個旋轉量R加一個平移t，也就說說T就是通過旋轉加平移將激光的框架轉換到子圖的框架下。
-
submaps：幾個連續的激光scan構成了一個子圖。這些子圖是柵格地圖M。其中離散的柵格地圖可以理解分辨率，本文采用的是5cm的分辨率。針對2D的格子大小就是5*5大小的格子，然後每一個格子有一個自己對應的概率值，位於最小值Pmin和最大值Pmax的空間中：
- 當柵格的概率值小於Pmin時，表示這個點沒有障礙物
- 當柵格的概率值位於Pmin和Pmax之間時，表示一種不確定性
- 當柵格的概率值大於Pmax時，表示這個點是有障礙物的
clamp函數就是一個區間限定函數。這裏也提出了預計算網格（precomputed grids）計算原理。當激光點從原點打在一個柵格上時，被擊中的柵格，我們進行該格子hit+1的操作，然後激光點和原點進行連線，連線穿過的格子，我們進行該格子miss+1的操作。對於之間沒有hit或者miss的格子，我們再爲其分配一個概率值。後續再有激光打在柵格上時，我們再重新計算一組hit和miss的值。
-
CSM（最右邊部分）：在每一幀激光數據插入子圖submap前，會使用基於ceres的scan matching對位姿進行優化。這個scan matching負責找到一個掃描位姿，使子圖中掃描點scan的概率最大化。這個問題可以轉換爲一個最小二乘的問題。該公式中的M函數是一個局部構圖中概率值的一個平滑版本。這是一個局部的更優化，需要提供一個良好的初值（前文提到，使用高頻LiDAR數據，或者3D情況下使用IMU提供）
5 Closing loops
這部分是本文的重點，其中的分支-定界加速和優化問題是cartographer爲什麼能夠達到實時優化的效果。
迴環檢測的目的：在大場景中通過創建小的子圖，來抑制誤差的累積。雖然短時間內連續激光數據組成的子圖誤差累積可以達到很小，但是不同的子圖之間仍然可能會有很大的誤差，所以仍然需要回環檢測來優化我們的位姿。
這個優化位姿本質上可以理解爲將位姿之間的差值轉化爲最小二乘問題，這裏面提到了一個稀疏姿態調整（Sparse Pose Adjustment, SPA）公式。其中m上角標是子圖位姿，s上角標是激光掃描的位姿，第三個是兩者之間的協方差，第四個是兩者之間的相對位置關係（變換）。
E表示的是他們之間的殘差，在這個部分有一個魯棒性迴歸的損失函數（Huber robust error function），它相對於均方誤差來說，對異常值並不敏感。其中的a可表示觀測值和預測值之間的差值（殘差），當殘差值很小的時候，可以進行平方的操作，當殘差值很大的時候，直接加入到系統中會將系統引導到一個錯誤的方向，所以進行一個線性化的操作，這樣避免位系統引入較大的殘差項，這在實際應用中主要應用在對稱的環境中，減小誤差匹配帶來的問題。
下面公式中的W是搜索窗口，Mnearest表示的是M將其參數舍入到最近的網格點得到的，其實就是從這個個scan點集周圍開始搜索，然後將網格點擴展成爲了像素。這個像素的意思就是說，在前端CSM的基礎上進行了後端迴環檢測的進一步匹配。所以將精確度達到一個像素的級別。
左邊的符號理解爲信度和，是將當前幀插入到子圖中，遍歷所有位姿得到的最高分。這個值越高，表示越匹配。
我們的目標就是找到這個最高分，但是這樣會導致計算量很大，所以本文采用分支-上界法。下面說一下要是想在這個scan點集周圍選定一個搜索窗口W尋找最佳的位姿。需要對以下幾個參數進行設置：
- 搜索中的角度（angular step）
- 搜索的最遠距離（points at the maximum range）
- 搜索的步長（search window）
- 分辨率（width of one pixel）
這部分可以結合算法1來看。從算法1來看，最上面有3層的暴力for循環，這樣的話要是想在大場景下實時地應用肯定是不行的。所以加入分支-定界進行加速，右邊是給出的一個例子。搜索中間x和y都是7m，角度是30°。W上面加一橫線表示搜索空間的集合。
主要思想是將可能性子集表示爲樹中的節點，其中根節點表示所有可能的解決方案。每個節點的子節點構成其父節點的一個分區，因此它們一起表示相同的可能性集合。通過得分進行判斷，爲了得到一個具體的算法，我們必須確定選擇節點、分支和上界計算的方法。
5.1 節點選擇
算法的效率取決於兩個東西：
-
好的解決方案：
採用深度優先搜索算法。類似於樹的先序遍歷，這種搜索算法所遵循的搜索策略是儘可能“深”地搜索一個圖。快速計算多個葉節點的分值。這一算法在算法3中體現。
-
好的上界：
由於不想把差的匹配添加到迴環檢測中，所以引入了一個分數閾值，低於這個閾值就對這個部分的最優解不感興趣，但是在實際過程中這個閾值往往是不會被超越的，所以我們把希望寄託在好的解決方案上。
5.2 分支規則
下面來看一下它是如何進行分支與上界（best_score）的計算。樹中的每一個節點都用一個整數的元組c來表示，其中每一個父節點可以分爲4個子節點。其中的角度是不變的，x和y都分別加入了新的偏移量。
結合右邊的圖來看，先假設從最頂層C0開始，計算所有C0層的得分，從大到小依次進行排序。然後再看C1層，計算C1層所有節點得分，再依次從大到小排序。然後重複到底層，計算葉子節點得分，然後將葉子節點中的最高得分記爲最高得分（best_score）。將這個最高得到和上一層的非父節點進行相比，如果這個分值比這些非父節點都大，那麼就進行剪枝，就這樣不斷地返回，就得到最高得分（best_score）。
下圖中左邊是常規的分支定界算法，右邊是本文提到的基於DFS的分支定界算法。
6 實驗部分
這部分主要是通過跑一些離線數據集，來進行實驗的。
7 結論
- 針對二維的SLAM算法，cartographer除了前端的scan match匹配算法，重點在後端的一個迴環檢測優化。採用分支定界的方法進行加速，對整個構圖進行了優化，證明了高精度實時構圖的可能性。