cartographer原理簡述

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

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

1.前端

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

前端scan match策略簡介

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

根據trajectory_builder_2d.lua文件中的

use_online_correlative_scan_matching = false

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

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

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

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

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

2.回環(huán)檢測

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

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

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

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

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

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

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

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

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

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

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

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

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

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

注意，全局約束才構造回環(huán)邊

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

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

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

即搜索出此節(jié)點的初始位姿3。

然后與局部約束相同，將初始位姿3經過節(jié)點與submap之間的ceres優(yōu)化，最終得到回環(huán)約束位姿。

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

總結與補充

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

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

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

3.后端優(yōu)化

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

詳細介紹：參考網址

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

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

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

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

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

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

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

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

這與我們常說的位姿圖優(yōu)化確實有一點小小區(qū)別。

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

總結

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

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