1.整體代碼構成

在整體的原有框架中代碼主要分為兩個部分：cartographer和cartographer_ros。
cartographer主要負責處理來自雷達、IMU和里程計的數據并基于這些數據進行地圖的構建，是cartographer理論的底層實現。建圖過程主要分為前端Local累加式建圖與后端Global位姿圖構建與回環匹配。
cartographer_ros則基于ros的通信機制獲取傳感器的數據并將它們轉換成cartographer中定義的格式傳遞給cartographer處理，與此同時也將cartographer的處理結果發布用于顯示或保存，在低層算法和機器人操作系統ROS之間提供了交互的接口。

2.底層算法構成

基于單線激光雷達的cartographer建圖過程可簡要表示如下圖:

其中輸入項中Range Data激光數據為必須，其余均為非必須。
整體的建圖可分為Local前端部分和Global后端回環部分。

2.1 Local部分:

Local SLAM主要負責地圖的實時累加式建立，主要在local_trajectory_builder_2d中實現。該部分通過一幀幀的Laser Scan建立并維護一系列的Submap，即一系列小規模的柵格地圖。當再有新的Laser Scan時，會通過Fast-CSM和Ceres Scan Matching的方法將其插入到子圖中的最佳位置。但是submap的累加式建圖會產生誤差累積的問題，該問題由Global部分解決。
前端Local部分輸入的傳感器數據可以有四個：Range Data(激光雷達，攝像頭等)，Odometry Pose(里程計數據)，IMU Data，FixedFramePose(當前版本尚未最終實現)。
里程計與數據與IMU數據共同進入PoseExtraPolator(姿態推算器)，得到里程計與IMU得到的位置估計，然后推算出下一時刻的位置姿態，然后給到Scan Matching中作為掃描匹配的初值。此處里程計和IMU的數據并不是必須的。
在LocalTrajectoryBuilder的AddRangeData中，Range Data數據經過體素濾波，之后將各個點存入returns或者misses集，然后進入scanMatching，如果使用了IMU，則將姿態推算器獲得的當前姿態推算作為優化初始值。這里ScanMatching使用基于ceres優化的方法獲得觀測最優位置估計，經過Motion Filter濾除運動過小的幀，之后調用InsertIntoSubmap()函數插入以構建submap。

2.1.1 AddRangeData()

在AddRangeData()中，對一幀點云的每個點數據，篩出小于指定距離的點，并逐個記錄為returns；對于大于指定距離的點，將該方向上一定距離的某點記入misses點集。此處可將多幀數據一并存入并在后續工作中作一幀處理，默認僅累計一幀的數據。之后對這一累積幀數據進行體素濾波。

2.1.2 TransformToGravityAlignedFrameAndFilter()

在該函數中，輸入的點云數據經過修剪篩去了z軸閾值之外的點。之后通過VoxelFilter()分別對returns點集和misses點集進行體素濾波。在每個體素內僅保留一個點。

2.1.3 AddAccumulatedRangeData()

經過體素濾波的點云在該函數中完成。從姿態推算器中獲取當前點云幀的姿態初值，進入ScanMatch()的模塊以優化點云幀的位姿。匹配完成后通過InsertIntoSubmap()把點云幀插入子圖。

2.1.4 ScanMatch()

首先對點云進行一個自適應體素濾波，然后獲得當前活躍的兩個子圖中的較舊的一個子圖，之后當前幀將與其進行匹配以優化當前幀位姿。
$★$Real-time CSM
首先通過real-time CSM對激光幀數據在submap的柵格地圖上進行一次粗匹配，產生一個大體較好的位姿值并返回一個得分數。
Real-time CSM的核心思想來源于論文Real-time correlative scan matching，文中提出了一種概率驅動的掃描匹配方法。
$$p(z_t|x_t,m_{t?1})$$
以上稱為觀測模型。
其中，$x_t$，$x_{t?1}$就是當前時刻和上一時刻的位姿，前者為未知的帶求解的量，$z_t$為當前scan里的所有數據的集合即雷達觀測數據，相當于是當前一幀雷達數據中的所有點，包括角度和距離；$m_{t?1}$為現有的地圖信息。
雷達觀測數據里包含了很多點數據，而這些數據相互之間并沒有關聯，所以可以假設他們是完全獨立的。由此可以將式子分解：
$$p(z_t|x_t,m_{t?1})=\prod _{i}p(z_t^i|x_t,m_{t?1})\propto \sum _i\log p(z_t^i|x_t,m_{t?1}))$$
上面的式子將整個一幀觀測數據的概率拆分成了當前幀的數據的每一個點的概率和。
在算法的實現中，程序采用以下方法：
a.在上一時刻位姿的周圍設定一個搜索窗口Search Window，將激光幀在一個范圍內按設定的旋轉步長生成一系列旋轉后的激光幀，將這些候選幀映射入柵格。
b.之后再將這些激光幀按一定步長在x、y軸上平移，進一步生成一系列的候選幀。這些候選幀覆蓋了該窗口內的所有可能位姿。
c.最后遍歷這個候選的子集，計算每個候選與地圖匹配的概率和，找到那個概率和最高的就是我們要找的當前機器人的位姿。
歷遍所有可能位姿的做法是暴力的，然而由于窗口較小，實時性沒有受到很大影響。

$★$Ceres scan match
之后將該位姿再進行一次與子圖的匹配ceres_scan_match，以獲得最終的進一步優化位姿。最后返回優化的位姿。采用最小二乘法解決這個問題：
$\underset{\xi }{\mathrm{argmin}}{\sum }_{k=1}^K(1?M_{smooth}(T_{\xi }{h}_{\xi }){)}^2$ (CS)
這里scanMatching方法對每個柵格點的概率進行雙三次樣條插值平滑，然后構建優化問題。式子中M即概率，概率值為0到1的一個數字，因此M這項越大，1-M就越小，這個優化函數目標是獲取一個概率最高的位置。
因為這是一個局部優化問題，所以需要用imu提供一個好的角度初始值。如果沒有，也可以用高概率的scan matcher 或者pixel-accurate來獲得值作為初始值，但是計算很復雜。

2.1.5 InsertIntoSubmap()

首先MotionFilter檢測此次位姿運動的大小，當小于某閾值時則不加入submap。之后使用InsertRangeData()根據優化好的位姿將點云幀分別插入當前活躍的兩個子圖中。
submap柵格地圖如下圖所示，其中打叉的灰色點為hit，無打叉的灰色點為miss。

更新子圖時將每個hit的點與原點連線上的點全部設置為miss，根據以下公式對子圖上hit的柵格概率odd進行更新。對于miss的網格點的修改同理。
$odds(p)=\frac{p}{1?p}$
$M_{new}(x)=clamp(odd{s}^{?1}(odds(M_{old}(x))?odds(p_{hit})))$
其中clamp是區間限定函數，大于max，返回max;小于min,返回min。該更新公式可由貝葉斯法則推導獲得。
依據以上方法，新scan被加入當前激活的兩個子圖。

2.2 Global部分:

Global的回環優化每隔一定數量激光幀以后進行。因為一個submap是有少數的幾個連續scan點集生成的，所以誤差很小，子圖內部不需要繼續優化。如果檢測到當前的掃描幀scan附近存在位置足夠接近的歷史子圖submap，則scan-match可以將子圖和掃描幀進行匹配，其中粗匹配過程在搜索窗口內使用fast-CSM分支定界獲得粗匹配結果，如果找到好的匹配，則使用（CS）式進行精細匹配調整，之后其作為約束添加到優化（SPA）中。

2.2.1 global_trajectory_builder

在Global軌跡構建器中組織起了整體SLAM建圖過程，其內部將建圖分為了2d與3d兩部分。在2d建圖時，構建器通過CreateGlobalTrajectoryBuilder2D()構建了2d情況下的整體建圖過程。
構建地圖時，程序首先構建了前端建圖的local_trajectory_builder_(Local軌跡構建器)和用于回環全局優化的pose_graph_(位姿圖)。對每一幀激光幀，程序首先調用了Local軌跡構建器的AddRangeData()方法，將激光幀逐幀加入一個個子圖，以完成實時累加式地圖的建立（2.1）；之后構建位姿圖，使用AddNode()，把前述匹配好的激光幀作為Node加入位姿圖中進行后續全局匹配。
位姿圖的構成如下圖所示：

圖中Node表示各個激光幀，Submap為各個子圖，虛線表示構建的位姿約束。其中與Submap m相關的約束表示激光幀在Submapm中檢測到了匹配并構建了新的約束（回環）。
全局回環優化可分為位姿圖的構建以及位姿圖的優化兩大部分。

2.2.2AddNode() in pose_graph_2d

在 pose_graph_2d中的AddNode()實現將激光幀與附近其他submap掃描匹配形成約束。這些約束在后面的位姿圖優化時將組成位姿圖的“邊”參與到優化的過程中去。在pose_graph中，為了保證位姿圖相關概念名詞的一致性，故將參與約束構建的激光幀稱為TrajectoryNode (軌跡節點)，簡稱Node (節點)。
在該部分的實現中，AddNode()首先將輸入的Node加入到pose_graph維護的trajectory_nodes_變量里。通過檢測之前被Node插入的submap是否曾被pose_graph觀測到，可以幫助pose_graph內部維護submap狀態的隊列submap_data及時進行更新。之后，調用ComputeConstraintsForNode()開始計算關于該Node的約束Constraint。

2.2.3 ComputeConstraintsForNode()

在該函數中計算與新添加的這個節點相關的所有約束關系并優化。
調用optimization_problem_的AddTrajectoryNode(),該節點被加入到優化問題中進行優化。
對于節點之前插入兩個submap的過程，可以認為天然的形成了Node與前后兩張submap的兩條約束，所以之后直接將這兩條約束加入到pose_graph中維護約束的隊列constraints_里。
之后，歷遍歷史中的 submap，通過ComputeConstraint()計算新的 Node 與每個 submap 的約束。
如果通過標記發現有新的剛結束構建的 submap，則需要將其狀態設置為finished，并調用ComputeConstraintsForOldNodes()為他計算新的 submap 和所有舊的節點的約束。
在每次調用函數加入節點時都會進行計數，以保證每隔若干個節點進行一次全局優化。當計數達到設定值時，調用DispatchOptimization()進行位姿圖優化計算。

2.2.4 ComputeConstraint()

在該函數中計算一個node和一個submap的約束。輸入變量即為Node和submap的id
首先需要確認該submap已經結束構建，對未結束構建的submap不進行約束計算;
（此處通過MaybeAddGlobalConstraint似乎可以實現全局定位）
對于初始位姿差距過大的node與submap，直接跳過約束的計算。
在約束計算的層層調用與各種條件判別中，較為重要的即為基于分支定界BBA的粗匹配和基于Ceres scan match的精匹配。

2.2.5 Fast- CSM&分支定界Branch And Bound

在全局匹配激光幀與子圖時，由于搜索窗口更大，匹配次數更多，純粹使用前述Real Time CSM暴力搜索十分消耗時間。Fast- CSM使用分支定界的辦法加速了這一過程。
分支定界法（branch and bound）是一種求解整數規劃問題的最常用算法。把全部可行的解空間不斷分割為越來越小的子集（稱為分支），并為每個子集內的解的值計算一個下界或上界（稱為定界）。在每次分支后，對凡是界限超出已知可行解值那些子集不再做進一步分支。這樣，解的許多子集（即搜索樹上的許多結點）就可以不予考慮了，從而縮小了搜索范圍。
分支定界法深度優先地查找最佳值，減小了計算所有情況的計算開銷。
在Fast- CSM中，根據分支定界的思想，地圖通過分辨率折半的過程被分作了多層，從最上層（最粗分辨率）開始用Real Time CSM的方法計算粗匹配最優匹配解。
在每層分辨率下，待匹配的Node根據給定的搜索窗口與步長創建一系列經過不同旋轉、平移的候選匹配集。每一個候選位置的Node分別與當前分辨率下的submap進行匹配，獲得每一個候選位置匹配的得分并進行排序獲得得分最高的位置候選。
之后在更低一層的submap上對最佳位置附近進行展開，再次進行候選集生成、逐個匹配獲取最佳值的過程。通過分支定界的方法最后找到最細分辨率下最佳的候選相對位姿。

該圖為轉載侵刪

2.2.6 Ceres_scan_match

與Local部分相同，粗定位之后，調用Ceres的非線性優化對粗匹配的結果進行微調：
$\underset{\xi }{\mathrm{argmin}}{\sum }_{k=1}^K(1?M_{smooth}(T_{\xi }{h}_{\xi }){)}^2$(CS)
這里scanMatching方法同樣對每個柵格點的概率進行雙三次樣條插值平滑，然后構建優化問題。優化后建立的約束被加入到pose_graph的constraint中，等待全局優化。

2.2.7 全局最終優化DispatchOptimization()

在全局優化中主要調用了optimization_problem_的Solve()對整個位姿圖進行匹配。
在優化中，將節點信息子圖信息（節點）和約束信息添加進ceres優化問題，隨后調用優化庫進行全局優化求解。
通過前述的Node與Submap之間的約束計算，約束圖被逐漸構建出來。圖中每個節點Node（激光幀）都與其插入的子圖相連，若存在回環約束則該Node還與與之匹配的Submap存在約束連接。稀疏圖優化有以下公式：
$\underset{{\Xi }^m,{\Xi }^s}{\mathrm{argmin}}\frac{1}{2}{\sum }_{ij}\rho (E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij}))$ (SPA)
式中優化的變量是子圖位姿和激光幀位姿，優化的目標是在每個子圖位姿和激光幀位姿間最小化協方差和相對位姿的殘差。
經過優化后每個激光幀和子圖的關系都得到了修正，尤其是在回環時當構建了激光幀和很早的子圖的約束時，全局優化可以有效解決累積誤差帶來的問題。

總結

以上是生活随笔為你收集整理的Cartographer代码建图实现流程解析的全部內容，希望文章能夠幫你解決所遇到的問題。

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