cartographer原理簡述

參考:https://blog.csdn.net/weixin_36976685/article/details/84994701

Cartographer 能產生一個精度為5cm的2D柵格地圖。

1.前端

Cartographer在前端匹配環節區別與其它建圖算法的區別主要是使用了Submap這一概念，每當或得一次laser scan的數據后，便把此時所在位置看做一個節點，與當前最近建立的submap去進行匹配(前端的scan match)，得到一個最優位姿，用來作為ceres的初始化位姿initial_ceres_pose。前端的scan match策略為:scan_matching::RealTimeCorrelativeScanMatcher,

前端scan match策略簡介

/** 首先由PoseExtrapolator根據imu和里程計通過濾波的方式預測一個初始姿態pose_prediction，然后在這個初始姿態附近放一個搜索框（注意，每一個角度也要枚舉）* 對搜索框中的每一個（x,y,theta）進行枚舉，打分score。* 打分機制：********************************************************************************************* 首先將原始點云進行體素濾波，然后將點云轉換為柵格坐標的點云，然后運用（x,y,theta）將點云進行旋轉，旋轉后點云中的每一個點* 所落入的柵格都有一個occupied概率值，將這一個點云的所有點得到的概率值相加，然后除以點云的點個數，然后再乘以一個因子* 得到最終的打分結果* 得分最高的搜索框中的位姿（x*,y*,theta*）作為最優搜索位姿* ***************************************************************************************************** 這個最優位姿被叫做initial_ceres_pose，以后要用來作為ceres優化的初始位姿 */

根據trajectory_builder_2d.lua文件中的

use_online_correlative_scan_matching = false

我們可以知道，默認是不需要前端scan match的。那么initial_ceres_pose就是簡單的用里程計和imu的濾波得到的pose_prediction。

用前端scan match策略或者直接里程計和imu的濾波得到了ceres的初始優化位姿后，當然是使用了ceres庫進行match優化,應該是僅僅優化位姿而不優化laser scan。 優化得到位姿的同時在不斷插入新數據幀的同時該Submap也得到了更新。一定量的數據組合成為一個Submap，當不再有新的scan插入到Submap時，就認為這個submap已經創建完成，接著會去創建下一個submap。

因此你可以通過前端，得出節點與submap的相對位姿。然后submap在當前軌跡中的位姿是submap建立的時候一開始第一個節點相對于軌跡坐標系的位姿。二者相乘，可以得到前端的最終結果————節點的map位姿。即節點相對于節點所在的軌跡坐標系的位姿。注意，是在他自己那條軌跡、即自己那個map中的坐標哦。

然后又知道軌跡坐標系在世界坐標系中的位姿，因此可以得到某個節點相對于世界的位姿，即絕對位姿。

解釋一下上邊，我喜歡把map坐標系作為軌跡坐標系，因為一個軌跡對應一個map嘛。

2.回環檢測

Cartographer通過創建大量的submap來實現大場景建圖，submap在短時間內的準確度是可靠的，但長時間會存在累積誤差，為了消除累積誤差，需要通過回環檢測來構建回環,最終是為了優化所有submap的位姿。

首先回環優化，我們需要檢測到回環，再進行優化 。這里為了減少計算量，提高實時回環檢測的效率，Cartographer應用了**branch and bound(分支定界)**優化方法進行優化搜索，如果得到一個足夠好的匹配（即Tij），已經檢測到了回環的存在。

我們從建立約束講起。由于回環檢測是一種全局約束。而其實后端同樣構建了局部約束。

后端的匹配策略為：scan_matching::FastCorrelativeScanMatcher，詳細介紹一下：

首先一個節點進來，我要 計算它與其他子圖間的約束。

因此我對此前所有軌跡中建立的所有子圖進行循環，計算這個節點與循環中的各個子圖的約束，根據節點和循環中的各個子圖之間的情況不同，可能構建的是全局約束，也可能是局部約束

（1）局部約束：節點和子圖在同一條軌跡中，或二者分別在兩條軌跡中，但是節點距離這兩條軌跡之間的上一次全局約束不遠，則進行局部約束。

前端我們可以得到節點相對于世界的位姿，也可以得到某個子圖的世界位姿，因此可以得到這個節點在這個子圖中的位姿。也就是這個節點相對于這個子圖的相對位姿。把這個位姿稱為初始位姿1。為什么要用世界坐標系作為橋梁是因為這個子圖和這個節點并不一定在一個map中，即不一定在同一條軌跡坐標系中。然后用submap在自己那條軌跡坐標系中的位姿乘以初始位姿1，得到初始位姿2，即此節點在此子圖所在的大map中的位姿。這樣，初始位姿2才能成為分枝定界方法的真正初始位姿。

在初始位姿2附近，利用分枝定界方法，獲取一個最優位姿。

分枝定界的樹結構一共七層，序號從上到下的索引分別為6-0。分枝定界加快回環檢測的速度。
分枝定界是 節點的位姿在submap中的分枝定界，而不是在submap所在map中的分枝定界

分枝定界求出的位姿被稱為初始位姿3，這個初始位姿3及其攜帶的點云要被輸入ceres與此子圖進行優化匹配match。
注意：只是這個節點和這個子圖之間的局部優化匹配，而不是全局位姿圖的優化。

最終得到ceres的輸出結果，即節點在map中的最優位姿。然后這個結果經過map到submap的轉換，最終得到節點在submap中的相對位姿**。這個位姿并不是我們所說的回環邊，其實就是一個普通約束，子圖和節點的普通邊

以上約束叫做：局部約束。節點和submap離得并不遠。

（2）全局約束：如果有 全局定位采樣脈沖的話,則進行全域上的約束匹配——需要節點和子圖二者不在同一軌跡，并在時間上和之前的全局約束關系相隔較遠，才能觸發采用全局約束。

注意，全局約束才構造回環邊

前面說過，我們在建立局部約束的時候，使用了初始位姿2作為分枝定界的初始位姿，再其附近進行局部范圍搜索。

但是現在建立全局約束的時候，直接在一個超大的范圍內進行分枝定界搜索，并不需要計算一個特殊的初始位姿2，而直接把初始位姿設置為地圖limits的中心點，可以理解為map的中心點。（因為carto中一條軌跡的map就是用map_limits表示的）

然后在這個超大的范圍內進行分枝定界，通過此節點所攜帶的點云與此submap的匹配程度，進行分枝定界求解。找到這個map中最能與此submap匹配的位姿。

即搜索出此節點的初始位姿3。

然后與局部約束相同，將初始位姿3經過節點與submap之間的ceres優化，最終得到回環約束位姿。

通常說的回環邊有兩類（里程計邊、scan match邊），這一類屬于scan match得到的回環邊。

總結與補充

通過上述兩種方法，即全局約束和局部約束，構建了一些scan match邊。不管是局部還是全局，都是實現的某個節點與某個submap之間的連接。

補充一下搜索范圍：
‘’’
局部搜索的范圍：±7m，±30°
全局搜索的范圍：10^6柵格范圍（乘以分辨率0.05為長度），±180°
‘’’

提出一個小疑問：局部約束算回環嗎？歡迎質疑

3.后端優化

對于全局優化來說，節點和節點之間是里程計邊，Tij可以認為是里程計的測量數據，構成圖優化的measurement。節點和某些子圖之間的局部約束也可以當做是約束邊，是一種scan match邊。但是對于回環邊，就是上述回環檢測章節所講述到的scan match邊，也是節點和submap之間的約束。

詳細介紹：參考網址

如下圖所示有兩類頂點，三角表示子圖submap，圓圈表示節點。
由于傳感器的一次掃描數據可能插入多個不同的子圖，所以同一個節點可能與多個子圖之間存在一定的匹配關系（即節點在子圖中的相對位姿），這個匹配關系就是所謂的約束。

之前說過，具有約束關系的節點和子圖不一定是在同一條軌跡中的。
我正在tracking的這條軌跡中的節點可能會和其他軌跡中的submap產生約束聯系，加入最終的global位姿優化。
這些約束可以被分為局部約束和全局約束

其實節點之間也不是孤立的，它們都是在上一個節點的基礎上累加一些運動得到的。這些運動的測量就體現在了里程計以及IMU的積分定位、前端的掃描匹配等方面。 各個節點前后串連起來就得到了一條軌跡。

自從上次后端的全局優化之后，每向優化問題中加入90個節點以后，進入后端的全局優化過程。
這就是隔一段時間進行一次優化的機制

submap和node之間有約束、node和node之間的聯系是什么呢？：

（1）如果沒有里程計數據，則node和node之間的相對位姿是經過local slam獲得的，因為local slam獲得了local pose，即相對于地圖map的相對位姿，所以用節點1->map->節點2構建兩個節點的相對位姿。

（2）如果有里程計數據，則兩個節點之間還可以加入里程計數據作為相對位姿約束。

node和子圖、node和node的約束都弄明白了以后，使用ceres進行全局優化。

這與我們常說的位姿圖優化確實有一點小小區別。

當然可以同時優化位姿和路標點，只需要在backup_2d.lua文件中將use_landmark參數設置為true即可。

總結

以上是生活随笔為你收集整理的cartographer原理简述的全部內容，希望文章能夠幫你解決所遇到的問題。

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