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_dataif (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.resolutionswitch (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.3options_.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);
}