講解關(guān)于slam一系列文章匯總鏈接:史上最全slam從零開始，針對于本欄目講解(02)Cartographer源碼無死角解析-鏈接如下:
(02)Cartographer源碼無死角解析- (00)目錄_最新無死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?

?

一、前言

通過前面一系列的博客，PoseExtrapolator 進(jìn)行了比較細(xì)致的分析。到目前為止，對于點云數(shù)據(jù)的預(yù)處理過程可以說時十分了解了，如：點云數(shù)據(jù)多傳感器時間同步、運動畸變校正、重力校正、體素濾波等。做完這一系列的預(yù)備工作之后，實際上呢，就可以進(jìn)行點云的掃描匹配了。

在講解掃描匹配之前，先來看看 Cartographer 2D 的柵格地圖，其不像3D點云地圖有很多成熟的庫可以調(diào)用，具有統(tǒng)一的標(biāo)準(zhǔn)。大多數(shù) 2D Slam 的柵格地圖都是需要自己編寫代碼進(jìn)行構(gòu)建的。下面就來看看 Cartographer 中時如何構(gòu)建的。

關(guān)于2D柵格地圖的構(gòu)建主要涉及到如下幾個類，后續(xù)會分別對齊進(jìn)行詳細(xì)的講解：

ActiveSubmaps2D 
Submap2D    Submap 子父類關(guān)系
Grid2D    ProbabilityGrid  子父類關(guān)系
RangeDataInserterInterface   ProbabilityGridRangeDataInserter2D//子父類關(guān)系

?

二、類間關(guān)系

先來看看源碼中時如何把這些類，或者類對象關(guān)聯(lián)起來的

1、ActiveSubmaps2D

在 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函數(shù)中，可以找到如下代碼：

  // 將校正后的雷達(dá)數(shù)據(jù)寫入submap
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

該處對 InsertIntoSubmap() 函數(shù)的調(diào)用，起到了與類 ActiveSubmaps2D 的交互。因為 LocalTrajectoryBuilder2D::InsertIntoSubmap() 函數(shù)中，使用到類 LocalTrajectoryBuilder2D 的成員對象：

ActiveSubmaps2D active_submaps_;

該成員對象在 LocalTrajectoryBuilder2D 構(gòu)造函數(shù)的初始化列表中被賦予初值，初始化列表可以看到如下代碼：

active_submaps_(options.submaps_options())

其首先根據(jù)配置文件中的 submaps 信息，構(gòu)建 ActiveSubmaps2D 對象，然后賦值給 active_submaps_。配置文件路徑為：

src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_2d.lua

2、Submap2D 與 Submap

類 ActiveSubmaps2D 包含成員變量 Submap2D ，如下所示：

std::vector<std::shared_ptr<Submap2D>> submaps_;

另外 Submap2D 為 Submap 的派生類，其都實現(xiàn)于文件 src/cartographer/cartographer/mapping/2d/submap_2d.cc 之中，Submap2D 存在兩個重載構(gòu)造函數(shù)，其都會構(gòu)件 Grid2D 實例對象，然后賦值給成員變量：

  std::unique_ptr<Grid2D> grid_; // 地圖柵格數(shù)據(jù)

3、Grid2D

Grid2D 繼承自 src/cartographer/cartographer/mapping/grid_interface.h 文件中的 GridInterface，GridInterface 比較簡單，僅僅幾句代碼而已。另外 Grid2D 包含成員變量 MapLimits limits_。另外，Grid2D 同時也是一個基類，如 ProbabilityGrid 與 TSDF2D 都為其派生類。

4、ProbabilityGrid

通過 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的 ActiveSubmaps2D::AddSubmap() 函數(shù)，可以得知在構(gòu)建 Grid2D 對象的時候，其調(diào)用的 ActiveSubmaps2D::CreateGrid() 函數(shù)，該函數(shù)會根據(jù)不同的配置信息構(gòu)件 ProbabilityGrid 或者是 TSDF2D 對象。

5、ProbabilityGridRangeDataInserter2D

調(diào)用 Submap2D::InsertRangeData() 函數(shù)，其需要傳遞一個 RangeDataInserterInterface 類型的指針對象，從命名可以看出其為一個接口，源碼中實際上傳入的實參對象由 ActiveSubmaps2D::CreateRangeDataInserter() 函數(shù)決定，其派生類 ProbabilityGridRangeDataInserter2D 與 TSDFRangeDataInserter2D 主要是負(fù)責(zé)數(shù)據(jù)插入的功能。ActiveSubmaps2D::CreateRangeDataInserter() 函數(shù)也是在 ActiveSubmaps2D 的構(gòu)造函數(shù)中被調(diào)用。
?

三、ActiveSubmaps2D

首先我們來看看 ActiveSubmaps2D 這個類，其主要負(fù)責(zé)與 LocalTrajectoryBuilder2D 的交互，同時內(nèi)部再通過 Submap2D、Submap 、Grid2D 、ProbabilityGrid 、ProbabilityGrid 、ProbabilityGridRangeDataInserter2D 這幾個類完成地圖的保存與插入。

前面已經(jīng)介紹過，ActiveSubmaps2D 的實例對象在 LocalTrajectoryBuilder2D 構(gòu)造函數(shù)中根據(jù)：

src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_2d.lua

配置文件中的如下參數(shù)進(jìn)行構(gòu)件：

  -- 子圖相關(guān)的一些配置
  submaps = {
    num_range_data = 90,          -- 一個子圖里插入雷達(dá)數(shù)據(jù)的個數(shù)的一半
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", -- 地圖的種類, 還可以是tsdf格式
      resolution = 0.05,   --分辨率
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",  --使用2D柵格概率圖插入數(shù)據(jù)
      -- 概率占用柵格地圖的一些配置
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地圖的一些配置
      tsdf_range_data_inserter = {
		......
		......
      },
    },

這些參數(shù)的具體作用，后續(xù)再做詳細(xì)的講解。ActiveSubmaps2D 主要有如下幾個成員變量：

  const proto::SubmapsOptions2D options_;
  std::vector<std::shared_ptr<Submap2D>> submaps_;
  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
  
  // 轉(zhuǎn)換表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的數(shù)據(jù)
  ValueConversionTables conversion_tables_; 

options_ 主要存儲匹配信息，submaps_ 用于存儲多個子圖，range_data_inserter_ 與 ValueConversionTables 后續(xù)進(jìn)行詳細(xì)分析。下面來分析 ActiveSubmaps2D 的成員函數(shù)。
?

四、InsertRangeData()

對于 ActiveSubmaps2D::InsertRangeData() 函數(shù)從命名可以看出，其主要功能為插入雷達(dá)數(shù)據(jù)到子圖中，流程如下：

$(01)$ 如果submaps_ 不存在任何一個子圖，或者 submaps_ 中最后一個子圖數(shù)據(jù)的數(shù)量達(dá)到了與 options_.num_range_data()=90(默認(rèn)配置)，則調(diào)用 ActiveSubmaps2D::AddSubmap() 函數(shù)新建一個子圖。

$(02)$ 將一幀雷達(dá)數(shù)據(jù) range_data 寫入到所有子圖之中 submaps_ 。不過注意，通常 submaps_ 最多只包含兩個子圖。具體原由稍后講解原由。

$(03)$ 如果 submaps_ 中第一個子圖中場插入的數(shù)據(jù)數(shù)量達(dá)到了兩倍 options_.num_range_data()，則把該子圖標(biāo)記為完成。

$(04)$ 調(diào)用 ActiveSubmaps2D::submaps() 函數(shù)，使用共享指針返回 submaps_ 中的所有子圖。

代碼注釋如下：

// 將點云數(shù)據(jù)寫入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第二個子圖插入節(jié)點的數(shù)據(jù)等于num_range_data時,就新建個子圖
  // 因為這時第一個子圖應(yīng)該已經(jīng)處于完成狀態(tài)了
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 將一幀雷達(dá)數(shù)據(jù)同時寫入兩個子圖中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  // 第一個子圖的節(jié)點數(shù)量等于2倍的num_range_data時,第二個子圖節(jié)點數(shù)量應(yīng)該等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

?

五、AddSubmap()

上面提到，如果submaps_ 不存在任何一個子圖，或者 submaps_ 中最后一個子圖數(shù)據(jù)的數(shù)量達(dá)到了與 options_.num_range_data()=90(默認(rèn)配置)，則調(diào)用 ActiveSubmaps2D::AddSubmap() 函數(shù)新建一個子圖。

新建子圖調(diào)用的函數(shù)就是 ActiveSubmaps2D::AddSubmap()，其目的是構(gòu)件一個 Submap2D 獨占指針對象，然后添加到 submaps_ 之后，不過有幾個點是需要注意的：

如果 submaps_ 中包含的子圖數(shù)量，即 submaps_.size() 大于等于 2，那么會擦除掉 submaps_ 中的第一個地圖。所以與前面的內(nèi)容就呼應(yīng)起來了，submaps_ 中最多存在兩個子圖。因為若 submaps_ 已經(jīng)存在兩個及兩個以上的子圖時，新建一個子圖的同時會刪除一個子圖，所以依舊為兩個子圖。

代碼注釋如下：

// 新增一個子圖,根據(jù)子圖個數(shù)判斷是否刪掉第一個子圖
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  // 調(diào)用AddSubmap時第一個子圖一定是完成狀態(tài),所以子圖數(shù)為2時就可以刪掉第一個子圖了
  if (submaps_.size() >= 2) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    CHECK(submaps_.front()->insertion_finished());
    // 刪掉第一個子圖的指針
    submaps_.erase(submaps_.begin());
  }
  // 新建一個子圖, 并保存指向新子圖的智能指針
  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));
}

?

六、CreateRangeDataInserter()

ActiveSubmaps2D 可以支持 概率柵格地圖 與 tsdf地圖，通過 ActiveSubmaps2D::CreateRangeDataInserter() 函數(shù)，根據(jù)配置信息可以構(gòu)建 ProbabilityGridRangeDataInserter2D 與 TSDFRangeDataInserter2D 對象。本人使用的是 ProbabilityGridRangeDataInserter2D，所以后續(xù)以其為例進(jìn)行講解。他們都派生自 RangeDataInserterInterface()，主要實現(xiàn)如下純虛函數(shù)，用于插入雷達(dá)數(shù)據(jù)(后續(xù)會做詳細(xì)講解)。

  // Inserts 'range_data' into 'grid'.
  virtual void Insert(const sensor::RangeData& range_data,GridInterface* grid) const = 0;

?

七、CreateGrid()

ActiveSubmaps2D::CreateGrid() 函數(shù)，主要是根據(jù)雷達(dá)傳感器的原點構(gòu)建 ProbabilityGrid 或者 TSDF2D 對象，主要作用進(jìn)行地圖保存，其都繼承于 Grid2D。另外需要注意，在構(gòu)建 ProbabilityGrid 或者 TSDF2D 時，會構(gòu)建一個 MapLimits 對象當(dāng)作實參傳入到構(gòu)造函數(shù)。
?

八、結(jié)語

在 ActiveSubmaps2D 的這些成員函數(shù)中，不難看出，對于地圖的相關(guān)處理都集中在成員函數(shù) InsertRangeData() 函數(shù)，其還調(diào)用了另一個重要函數(shù)AddSubmap()，為了方便理解，根據(jù)下圖進(jìn)行講解一下：

其上的每個方形代表一個子圖 Submap2D，根據(jù)上面的分析知道 submaps_ 最多同時存在兩個子圖，這里假設(shè)現(xiàn)在 submaps_ 中存儲的子圖為子圖1與子圖2。那么子圖1中插入的數(shù)據(jù)定然比子圖2多 options_.num_range_data()，因為只有子圖一達(dá)到 options_.num_range_data() 數(shù)據(jù)集時，子圖二才被創(chuàng)建，同時會刪除 submaps_ 最前面的子圖(這里假設(shè)為子圖0，未在上圖畫出)。后續(xù)在插入的數(shù)據(jù)是，會同時插入到子圖1與子圖2，也就是說，這兩個子圖的數(shù)據(jù)是存在交集的。

當(dāng)子圖2數(shù)據(jù)達(dá)到了 options_.num_range_data()，也就是此時 子圖1的數(shù)據(jù)為2倍的 options_.num_range_data()，會把子圖1標(biāo)記為完成狀態(tài)，同時從 submaps_ 中刪除該子圖，這樣子圖2代替了之前子圖1的位置，同時會再創(chuàng)建子圖3添加到 submaps_ 之中。也就是說，此時 submaps_ 中包含了子圖2與子圖3，然后再繼續(xù)往子圖2,3插入數(shù)據(jù)，所以子圖2與子圖3也存在交集，依次循環(huán)下去。

最后了，會保證兩個相鄰的子圖之間是存在共同數(shù)據(jù)的，其目的是為了點云匹配時，在兩個子圖間出現(xiàn)斷層的現(xiàn)象。具體的細(xì)節(jié)后續(xù)再做更加詳細(xì)的分析。

?
?
?文章來源地址http://www.zghlxwxcb.cn/news/detail-659230.html

到了這里，關(guān)于(02)Cartographer源碼無死角解析-(41) 2D柵格地圖→ActiveSubmaps2D的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！