(02)Cartographer源码无死角解析-(42) 2D栅格地图→Submap、Submap2D、MapLimits

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

在上一篇博客中，对 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的类 ActiveSubmaps2D 各个成员函数都进行了介绍，其主要功能如下图所示：

通过调用 ActiveSubmaps2D::InsertRangeData() 函数向子图 submaps_ 中插入数据，其会使得两个连续的子图之间的数据存在交集。如上图的子图1与子图2存在交集，同时子图2与子图3也存在交集。

从类名可以轻易的分辨出，ActiveSubmaps2D 表示激活的子图，其包含的成员变量 std::vector<std::shared_ptr<Submap2D>> submaps_ 表示的就是目前处于激活状态的子图(通常情况下是两个子图)，如果 submaps_ 中的第一个子图插入数据足够了，则会被标记为完成，然后从 submaps_ 中擦除。

Submap2D 与 ActiveSubmaps2D 的成员函数都是在 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中实现，既然分析完了 ActiveSubmaps2D，那么就来看看 Submap2D。不过在分析 Submap2D 之前，先要看看其父类 Submap。

在前面的博客中已经提及过，Submap2D 继承于 Submap，Submap 在 src/cartographer/cartographer/mapping/submaps.h 文件中被声明，主要定义了一些纯虚函数，以及一些成员变量。该些成员变量如下：

private:
  const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
  int num_range_data_ = 0; //子图中数据的数目，初始为0
  bool insertion_finished_ = false; //是否为插入完成状态，初始为否。

由于这些属性是私有的，所以无法被其派生类 Submap2D 继承，不过没有关系，因为提供了对该些属性访问或者操作的 public 接口，如下：

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

  // Number of RangeData inserted.
  // 插入到子图中雷达数据的个数
  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_; }
  // 将子图标记为完成状态
  void set_insertion_finished(bool insertion_finished) {
    
    
    insertion_finished_ = insertion_finished;
  }

另外，需要注意到的是，Submap 的构造函数需要传入 local_submap_pose 变量，完成对成员变量 local_pose_ 的初始化，其表示子图在 local 坐标系下的位姿。也就是说，每创建一个子图，都需要指定好该子图在 local 坐标系下的位姿。
 

二、Submap2D

1、Submap2D::Submap2D()

Submap2D 继承于 Submap，其存在两个私有属性：

private:
  std::unique_ptr<Grid2D> grid_; // 地图栅格数据

  // 转换表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的数据
  ValueConversionTables* conversion_tables_;

后续对于这两个属性会进行详细的分析，关于 Submap2D 的两个重载构造函数都会对这两个属性进行初始化。其第一个构造函数，直接接收 grid 与 conversion_tables 参数，然后利用初始化列表直接赋值给 grid_ 与 conversion_tables_，代码如下所示：

/**
 * @brief 构造函数
 * 
 * @param[in] origin Submap2D的原点,保存在Submap类里
 * @param[in] grid 地图数据的指针
 * @param[in] conversion_tables 地图数据的转换表
 */
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);
}

还需要传递一个参数 origin，其表示子图的原点，也是就子图在 local 坐标系下的位姿。除上述构造函数外，还有另外一个构造函数，通过 proto 格式的数据构建 ProbabilityGrid 或者 TSDF2D 对象指针赋值给 grid_。代码就不再这里复制展示了。
 

2、Submap2D::InsertRangeData()

在 Submap2D 中，还有几个成员函数：Submap2D::ToProto()， Submap2D::UpdateFromProto()，Submap2D::ToResponseProto() 都与 proto 相关，暂时不讲解。先来看看看其中另外一个比较重要的函数Submap2D::InsertRangeData()：

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

$功能:$ 把点云数据插入到子图之中

$输入:$ 【参数①range_data】→需要被插入的点云数据。【参数②range_data_inserter】→负责数据插入的实例对象，为 RangeDataInserterInterface 的派生类。

$返回:$ 无
$================================================================================================================================================================:$

该函数实际上就是调用了 range_data_inserter->Insert(range_data, grid_.get()) 函数，将数据写入到栅格地图 grid_ 之中。该函数注释如下：

// 将雷达数据写到栅格地图中
void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
    
    
  CHECK(grid_);
  CHECK(!insertion_finished());
  // 将雷达数据写到栅格地图中
  range_data_inserter->Insert(range_data, grid_.get());
  // 插入到地图中的雷达数据的个数加1
  set_num_range_data(num_range_data() + 1);
}

从这里还可以看出每插入一帧数据，num_range_data 才会 +1，因为 range_data 中存储的并不是一个点云数据，而是一帧。
 

3、Submap2D::Finish()

该函数比较简单，其调用了 grid_->ComputeCroppedGrid() 函数，该函数后续再进行分析，然后设置 insertion_finished_ 变量，标记当前子图为完成状态。
 

三、MapLimits

结合前面的分析，可以知道 Submap2D 中的 Grid2D 实例对象 grid_ 是十分重要的组成部分，回到之前 ActiveSubmaps2D::AddSubmap() 函数，存在如下代码：

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

由此可知，Grid2D 的构建来自于 ActiveSubmaps2D::CreateGrid() 函数，该函数会构建 Grid2D 派生类对象 ProbabilityGrid 或者 TSDF2D 的独占指针。需要注意，在函数中可以看到如下代码:

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

也就是是说，无论构建 ProbabilityGrid 还是 TSDF2D 实例对象指针，都需要传入MapLimits 对象作为实参。那么就来看看 MapLimits 代码中是如何实现的，位于 src/cartographer/cartographer/mapping/2d/map_limits.h 文件中。从命名来看，地图限制，其是限制了那些东西呢?

首先每个子图 Submap2D 或者说都对应的一个栅格(Grid)，后续每个栅格都会再进一步划分，划分之后以以 cell 为单位，如下图所示，每个小方格都表示一个一个 call：

既然要把子图 Submap2D 或者 Grid2D 划分成 call 形式，那么肯定需要指定每个 Grid2D 应该被划分成多少个 cell。先来看看 MapLimits 的构造函数。

1、MapLimits::MapLimits

  /**
   * @brief 构造函数
   * 
   * @param[in] resolution 地图分辨率
   * @param[in] max 左上角的坐标为地图坐标的最大值
   * @param[in] cell_limits 地图x方向与y方向的格子数
   */
  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
  }

该构造函数首先指定了地图的分辨率，该分辨率表示由 options_.grid_options_2d().resolution() 确定。这里我们约定两个坐标系，如下：

       ①地图坐标系→该坐标系以物理单位作为衡量。
       ②像素坐标系→该坐标系以像素为单位

约定了上述两个坐标系之后，那么所谓的分辨率就表示地图坐标系与是像素坐标系的比值，简单的说就是栅格地图中一个像素代表地图坐标系多个个物理单位(米)。

第二个参数 max，表示的地图坐标的最大值，第三个参数 cell_limits 表示每个子图，或者说每个栅格x,y方向上包含了多少个 cell。
 

2、MapLimits::GetCellIndex()

该函数从命名可以看出来，其是获得 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地图的像素坐标系 是不一样的，像素坐标的原点是在左上角。根据对源码的分析，像素坐标系中的一个像素代表地图坐标的一个cell。
 

3、MapLimits::GetCellCenter()

该函数的作用可以与 MapLimits::GetCellIndex() 是相反的，其输入一个像素索引，然后返回该像素对应在地图坐标系下的物理坐标：

  // Returns the center of the cell at 'cell_index'.
  // 根据像素索引算物理坐标
  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()

该函数输入一个像素坐标索引，其会判断该像素是否存于栅格地图内部，代码注释如下：

  // Returns true if the ProbabilityGrid contains 'cell_index'.
  // 判断给定像素索引是否在栅格地图内部
  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();
  }

四、结语

关于 MapLimits 还有一些成员函数没有讲解，如 MapLimits::ToProto() 不过这已经不影响后续的分析了。再了解了 ActiveSubmaps2D、Submap、Submap2D、MapLimits 之后，接下来就要看一个大头部分：Grid2D 与 ProbabilityGrid