cartographer代码学习-概率栅格地图（ActiveSubmaps2D与Submaps2D）

概率栅格地图是二维激光SLAM的特点，能够将环境通过地图的形式表达出来。

ActiveSubmaps2D作为概率栅格地图中的重要成分，这个对象主要在LocalTrajectoryBuilder2D这里被使用
第一次调用：

active_submaps_(options.submaps_options())

传入一个submap的配置参数。具体的参数在trajectory_builder_2d.lua内：

submaps = {
    num_range_data = 90,          -- 一个子图里插入雷达数据的个数的一半
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
      resolution = 0.05,
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      -- 概率占用栅格地图的一些配置
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地图的一些配置
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },

最上面几个是外部参数，其中range_data_inserter_type参数决定了在建立地图写入器的时候选择使用哪个地图写入器：

// 创建地图数据写入器
std::unique_ptr<RangeDataInserterInterface>
ActiveSubmaps2D::CreateRangeDataInserter() {
  switch (options_.range_data_inserter_options().range_data_inserter_type()) {
    // 概率栅格地图的写入器
    case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
      return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
          options_.range_data_inserter_options()
              .probability_grid_range_data_inserter_options_2d());
    // tsdf地图的写入器
    case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
      return absl::make_unique<TSDFRangeDataInserter2D>(
          options_.range_data_inserter_options()
              .tsdf_range_data_inserter_options_2d());
    default:
      LOG(FATAL) << "Unknown RangeDataInserterType.";
  }
}

下面细分两块概率栅格地图参数以及tsdf地图参数。
第二次调用：

if (active_submaps_.submaps().empty())

判断地图是否为空。
第三次调用：

// 使用active_submaps_的第一个子图进行匹配
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();

取出一个子地图进行扫面匹配。
第四次调用：

// 将点云数据写入到submap中
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

将点云写入栅格地图中。

ActiveSubmaps2D的具体实现是在submap2D.cc文件中：

// ActiveSubmaps2D构造函数
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
    : options_(options), range_data_inserter_(CreateRangeDataInserter()) {}

这里实际上是使用了一个CreateRangeDataInserter()函数，这个函数创建了一个地图写入器。

ActiveSubmaps2D类中这里面还有一个函数：InsertRangeData

// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图
  // 因为这时第一个子图应该已经处于完成状态了
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

这个函数就是用来处理点云到子图的函数。cartographer会同时维护两个子图。当激光雷达的插入次数达到一定的次数时，会停止该子图的插入，然后新建一个新的子图。原来的一个子图会通过Addsubmap函数插入到地图中。
submap子图的思路如下：
算法配置文件默认参数为90。
初始时只有一个子图，当第一个地图达到90帧时，建立第二个子图；
当第一个地图到达180帧时，第二个子图到达90帧，此时将第一个子图的指针删除，同时建立第三个子图，将指针指向第二个与第三个子图。注意这里第一个子图本身是没有被删除的。删除的是指向它的一个指针。
当第二个子图到达180帧时，重复第二步
…
可以看到子图是以每180次激光数据的处理为一个新的更新的。

新建子图的代码如下：

// 以当前雷达原点为地图原件创建地图
std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
    const Eigen::Vector2f& origin) {
  // 地图初始大小,100个栅格
  constexpr int kInitialSubmapSize = 100;
  float resolution = options_.grid_options_2d().resolution(); // param: grid_options_2d.resolution
  switch (options_.grid_options_2d().grid_type()) {
    // 概率栅格地图
    case proto::GridOptions2D::PROBABILITY_GRID:
      return absl::make_unique<ProbabilityGrid>(
          MapLimits(resolution,
                    // 左上角坐标为坐标系的最大值, origin位于地图的中间
                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
                                                resolution *
                                                Eigen::Vector2d::Ones(),
                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
          &conversion_tables_);
    // tsdf地图
    case proto::GridOptions2D::TSDF:
      return absl::make_unique<TSDF2D>(
          MapLimits(resolution,
                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
                                                resolution *
                                                Eigen::Vector2d::Ones(),
                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
          options_.range_data_inserter_options()
              .tsdf_range_data_inserter_options_2d()
              .truncation_distance(),               // 0.3
          options_.range_data_inserter_options()
              .tsdf_range_data_inserter_options_2d()
              .maximum_weight(),                    // 10.0
          &conversion_tables_);
    default:
      LOG(FATAL) << "Unknown GridType.";
  }
}

可以看出来它支持两种格式的地图：概率栅格地图以及TSDF格式地图。
对于概率地图，它以机器人当前位姿为原点建立一个一定大小的栅格地图。默认是100x100个栅格。当前原点的世界坐标会存入submap类中。对于概率地图，其需要两个参数：MapLimits以及conversion_tables_，这里的conversion_tables_是指转换表，其将0.1-0.9的概率值转换成0-65530的整数形式进行存储，方便进行运算。

总体来说，ActiveSubmaps2D函数执行了三个事情：
1、调用构造函数 CreateRangeDataInserter()新建ProbabilityGridRangeDataInserter2D类的对象
2、调用submaps()函数获取指向submap2D的shared_ptr指针的vector
3、调用雷达写入数据的InsertRangeData函数，同时该函数会调用addsubmap()函数以及submap2D::InsertRangeData函数。addsubmap()调用CreateGrid()函数创建概率地图。同时该函数调用地图相关的几个类函数：MapList、Grid2D以及ProbabilityGrid。

上述操作第三步调用了submap2D的操作，其继承了Submap，对于submap，其有三个私有变量：

const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
int num_range_data_ = 0;
bool insertion_finished_ = false;

local_pose_代表子图原点在local坐标系下的坐标，num_range_data_为插入激光数据的数量，insertion_finished_代表这张子图是否执行完成。
对于submap的初始化而言，它需要传入一个local_submap_pose作为子图的原点：

Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}

除此之外就是几个简单的赋值以及返回参数的函数，所以submap这块的内容相对而言内容不多，还是比较简单的。

而submap2D主要继承了submap，同时将其没有具体实现的函数进行了实现。此外，对于submap2D的初始化需要的参数主要包含以下部分：

/**
 * @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);
}

可以看到这里的参数其实最终都是传递到submap的。此外，这个类主要也是包含了几个简单的函数调用以及实现，不作具体展开，到这里基本ActiveSubmaps2D与Submaps2D的内容告一段落。