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

?

一、前言

在上一篇博客中，對 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的類 ActiveSubmaps2D 各個成員函數(shù)都進行了介紹，其主要功能如下圖所示：

通過調(diào)用 ActiveSubmaps2D::InsertRangeData() 函數(shù)向子圖 submaps_ 中插入數(shù)據(jù)，其會使得兩個連續(xù)的子圖之間的數(shù)據(jù)存在交集。如上圖的子圖1與子圖2存在交集，同時子圖2與子圖3也存在交集。

從類名可以輕易的分辨出，ActiveSubmaps2D 表示激活的子圖，其包含的成員變量 std::vector<std::shared_ptr<Submap2D>> submaps_ 表示的就是目前處于激活狀態(tài)的子圖(通常情況下是兩個子圖)，如果 submaps_ 中的第一個子圖插入數(shù)據(jù)足夠了，則會被標記為完成，然后從 submaps_ 中擦除。

Submap2D 與 ActiveSubmaps2D 的成員函數(shù)都是在 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中實現(xiàn)，既然分析完了 ActiveSubmaps2D，那么就來看看 Submap2D。不過在分析 Submap2D 之前，先要看看其父類 Submap。

在前面的博客中已經(jīng)提及過，Submap2D 繼承于 Submap，Submap 在 src/cartographer/cartographer/mapping/submaps.h 文件中被聲明，主要定義了一些純虛函數(shù)，以及一些成員變量。該些成員變量如下：

private:
  const transform::Rigid3d local_pose_; // 子圖原點在local坐標系下的坐標
  int num_range_data_ = 0; //子圖中數(shù)據(jù)的數(shù)目，初始為0
  bool insertion_finished_ = false; //是否為插入完成狀態(tài)，初始為否。

由于這些屬性是私有的，所以無法被其派生類 Submap2D 繼承，不過沒有關(guān)系，因為提供了對該些屬性訪問或者操作的 public 接口，如下：

  // Pose of this submap in the local map frame.
  // 在local坐標系的子圖的坐標
  transform::Rigid3d local_pose() const { return local_pose_; }

  // Number of RangeData inserted.
  // 插入到子圖中雷達數(shù)據(jù)的個數(shù)
  int num_range_data() const { return num_range_data_; }
  void set_num_range_data(const int num_range_data) {
    num_range_data_ = num_range_data;
  }

  bool insertion_finished() const { return insertion_finished_; }
  // 將子圖標記為完成狀態(tài)
  void set_insertion_finished(bool insertion_finished) {
    insertion_finished_ = insertion_finished;
  }

另外，需要注意到的是，Submap 的構(gòu)造函數(shù)需要傳入 local_submap_pose 變量，完成對成員變量 local_pose_ 的初始化，其表示子圖在 local 坐標系下的位姿。也就是說，每創(chuàng)建一個子圖，都需要指定好該子圖在 local 坐標系下的位姿。
?

二、Submap2D

1、Submap2D::Submap2D()

Submap2D 繼承于 Submap，其存在兩個私有屬性：

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

  // 轉(zhuǎn)換表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的數(shù)據(jù)
  ValueConversionTables* conversion_tables_;

后續(xù)對于這兩個屬性會進行詳細的分析，關(guān)于 Submap2D 的兩個重載構(gòu)造函數(shù)都會對這兩個屬性進行初始化。其第一個構(gòu)造函數(shù)，直接接收 grid 與 conversion_tables 參數(shù)，然后利用初始化列表直接賦值給 grid_ 與 conversion_tables_，代碼如下所示：

/**
 * @brief 構(gòu)造函數(shù)
 * 
 * @param[in] origin Submap2D的原點,保存在Submap類里
 * @param[in] grid 地圖數(shù)據(jù)的指針
 * @param[in] conversion_tables 地圖數(shù)據(jù)的轉(zhuǎn)換表
 */
Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
                   ValueConversionTables* conversion_tables)
    : Submap(transform::Rigid3d::Translation(
          Eigen::Vector3d(origin.x(), origin.y(), 0.))),
      conversion_tables_(conversion_tables) {
  grid_ = std::move(grid);
}

還需要傳遞一個參數(shù) origin，其表示子圖的原點，也是就子圖在 local 坐標系下的位姿。除上述構(gòu)造函數(shù)外，還有另外一個構(gòu)造函數(shù)，通過 proto 格式的數(shù)據(jù)構(gòu)建 ProbabilityGrid 或者 TSDF2D 對象指針賦值給 grid_。代碼就不再這里復(fù)制展示了。
?

2、Submap2D::InsertRangeData()

在 Submap2D 中，還有幾個成員函數(shù)：Submap2D::ToProto()， Submap2D::UpdateFromProto()，Submap2D::ToResponseProto() 都與 proto 相關(guān)，暫時不講解。先來看看看其中另外一個比較重要的函數(shù)Submap2D::InsertRangeData()：

$InsertRangeData()======================================================================================================================================================:$

$功能:$ 把點云數(shù)據(jù)插入到子圖之中

$輸入:$ 【參數(shù)①range_data】→需要被插入的點云數(shù)據(jù)?！緟?shù)②range_data_inserter】→負責(zé)數(shù)據(jù)插入的實例對象，為 RangeDataInserterInterface 的派生類。

$返回:$ 無
$================================================================================================================================================================:$

該函數(shù)實際上就是調(diào)用了 range_data_inserter->Insert(range_data, grid_.get()) 函數(shù)，將數(shù)據(jù)寫入到柵格地圖 grid_ 之中。該函數(shù)注釋如下：

// 將雷達數(shù)據(jù)寫到柵格地圖中
void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
  CHECK(grid_);
  CHECK(!insertion_finished());
  // 將雷達數(shù)據(jù)寫到柵格地圖中
  range_data_inserter->Insert(range_data, grid_.get());
  // 插入到地圖中的雷達數(shù)據(jù)的個數(shù)加1
  set_num_range_data(num_range_data() + 1);
}

從這里還可以看出每插入一幀數(shù)據(jù)，num_range_data 才會 +1，因為 range_data 中存儲的并不是一個點云數(shù)據(jù)，而是一幀。
?

3、Submap2D::Finish()

該函數(shù)比較簡單，其調(diào)用了 grid_->ComputeCroppedGrid() 函數(shù)，該函數(shù)后續(xù)再進行分析，然后設(shè)置 insertion_finished_ 變量，標記當(dāng)前子圖為完成狀態(tài)。
?

三、MapLimits

結(jié)合前面的分析，可以知道 Submap2D 中的 Grid2D 實例對象 grid_ 是十分重要的組成部分，回到之前 ActiveSubmaps2D::AddSubmap() 函數(shù)，存在如下代碼：

std::unique_ptr<Grid2D>(static_cast<Grid2D*>(CreateGrid(origin).release()))

由此可知，Grid2D 的構(gòu)建來自于 ActiveSubmaps2D::CreateGrid() 函數(shù)，該函數(shù)會構(gòu)建 Grid2D 派生類對象 ProbabilityGrid 或者 TSDF2D 的獨占指針。需要注意，在函數(shù)中可以看到如下代碼:

          MapLimits(resolution,
                    // 左上角坐標為坐標系的最大值, origin位于地圖的中間
                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
                                                resolution *
                                                Eigen::Vector2d::Ones(),
                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),

也就是是說，無論構(gòu)建 ProbabilityGrid 還是 TSDF2D 實例對象指針，都需要傳入MapLimits 對象作為實參。那么就來看看 MapLimits 代碼中是如何實現(xiàn)的，位于 src/cartographer/cartographer/mapping/2d/map_limits.h 文件中。從命名來看，地圖限制，其是限制了那些東西呢?

首先每個子圖 Submap2D 或者說都對應(yīng)的一個柵格(Grid)，后續(xù)每個柵格都會再進一步劃分，劃分之后以以 cell 為單位，如下圖所示，每個小方格都表示一個一個 call：

既然要把子圖 Submap2D 或者 Grid2D 劃分成 call 形式，那么肯定需要指定每個 Grid2D 應(yīng)該被劃分成多少個 cell。先來看看 MapLimits 的構(gòu)造函數(shù)。

1、MapLimits::MapLimits

  /**
   * @brief 構(gòu)造函數(shù)
   * 
   * @param[in] resolution 地圖分辨率
   * @param[in] max 左上角的坐標為地圖坐標的最大值
   * @param[in] cell_limits 地圖x方向與y方向的格子數(shù)
   */
  MapLimits(const double resolution, const Eigen::Vector2d& max,
            const CellLimits& cell_limits)
      : resolution_(resolution), max_(max), cell_limits_(cell_limits) {
    CHECK_GT(resolution_, 0.);
    CHECK_GT(cell_limits.num_x_cells, 0.);
    CHECK_GT(cell_limits.num_y_cells, 0.);z
  }

該構(gòu)造函數(shù)首先指定了地圖的分辨率，該分辨率表示由 options_.grid_options_2d().resolution() 確定。這里我們約定兩個坐標系，如下：

???????①地圖坐標系→該坐標系以物理單位作為衡量。
???????②像素坐標系→該坐標系以像素為單位

約定了上述兩個坐標系之后，那么所謂的分辨率就表示地圖坐標系與是像素坐標系的比值，簡單的說就是柵格地圖中一個像素代表地圖坐標系多個個物理單位(米)。

第二個參數(shù) max，表示的地圖坐標的最大值，第三個參數(shù) cell_limits 表示每個子圖，或者說每個柵格x,y方向上包含了多少個 cell。
?

2、MapLimits::GetCellIndex()

該函數(shù)從命名可以看出來，其是獲得 cell 在 gred 中的索引。代碼如下所示：

  // Returns the index of the cell containing the 'point' which may be outside
  // the map, i.e., negative or too large indices that will return false for
  // Contains().
  // 計算物理坐標點的像素索引
  Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
    // Index values are row major and the top left has Eigen::Array2i::Zero()
    // and contains (centered_max_x, centered_max_y). We need to flip and
    // rotate.
    return Eigen::Array2i(
        common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
        common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
  }

傳入的 point 是地圖坐標系的物理單位，計算方式也比較簡單，物理坐標除以分辨率即可，等價于把 地圖坐標 變換成 像素坐標。那么這里為什還要用 max_ 減去 point 呢? 如下所示：

/**
 * note: 地圖坐標系可視化展示
 * ros的地圖坐標系    cartographer的地圖坐標系     cartographer地圖的像素坐標系 
 * 
 * ^ y                            ^ x              0------> x
 * |                              |                |
 * |                              |                |
 * 0 ------> x           y <------0                y       
 * 
 * ros的地圖坐標系: 左下角為原點, 向右為x正方向, 向上為y正方向, 角度以x軸正向為0度, 逆時針為正
 * cartographer的地圖坐標系: 坐標系右下角為原點, 向上為x正方向, 向左為y正方向
 *             角度正方向以x軸正向為0度, 逆時針為正
 * cartographer地圖的像素坐標系: 左上角為原點, 向右為x正方向, 向下為y正方向
 */

其主要原因是因為 cartographer的地圖坐標系 與 cartographer地圖的像素坐標系 是不一樣的，像素坐標的原點是在左上角。根據(jù)對源碼的分析，像素坐標系中的一個像素代表地圖坐標的一個cell。
?

3、MapLimits::GetCellCenter()

該函數(shù)的作用可以與 MapLimits::GetCellIndex() 是相反的，其輸入一個像素索引，然后返回該像素對應(yīng)在地圖坐標系下的物理坐標：

  // Returns the center of the cell at 'cell_index'.
  // 根據(jù)像素索引算物理坐標
  Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
    return {max_.x() - resolution() * (cell_index[1] + 0.5),
            max_.y() - resolution() * (cell_index[0] + 0.5)};
  }

這里返回的是地圖 cell 中心坐標。源碼計算過程還是比較簡單的，就是 MapLimits::GetCellIndex() 的逆操作。
?

4、MapLimits::Contains()

該函數(shù)輸入一個像素坐標索引，其會判斷該像素是否存于柵格地圖內(nèi)部，代碼注釋如下：

  // Returns true if the ProbabilityGrid contains 'cell_index'.
  // 判斷給定像素索引是否在柵格地圖內(nèi)部
  bool Contains(const Eigen::Array2i& cell_index) const {
    return (Eigen::Array2i(0, 0) <= cell_index).all() &&
           (cell_index <
            Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
               .all();
  }

?

四、結(jié)語

關(guān)于 MapLimits 還有一些成員函數(shù)沒有講解，如 MapLimits::ToProto() 不過這已經(jīng)不影響后續(xù)的分析了。再了解了 ActiveSubmaps2D、Submap、Submap2D、MapLimits 之后，接下來就要看一個大頭部分：Grid2D 與 ProbabilityGrid

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

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