Cartographer学习——2D栅格地图构建
前言：
到目前为止，对于点云数据的预处理过程已经介绍完毕，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。
在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。下面就来看看 Cartographer 中时如何构建的。
关于2D栅格地图的构建主要涉及到如下几个类，如下图所示，先大致看一下，后续会分别对其进行详细的讲解：
一、ActiveSubmaps2D
1. 初始化类对象
首先我们来看看 ActiveSubmaps2D 这个类，其主要负责与 LocalTrajectoryBuilder2D 的交互，同时内部再通过 Submap2D、Submap 、Grid2D 、ProbabilityGrid 、ProbabilityGrid 、ProbabilityGridRangeDataInserter2D 这几个类完成地图的保存与插入。
ActiveSubmaps2D的类对象active_submaps_在src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h头文件的LocalTrajectoryBuilder2D类的定义中声明。
class LocalTrajectoryBuilder2D { private: ... ActiveSubmaps2D active_submaps_; }
在src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc源文件下的LocalTrajectoryBuilder2D的构造函数中进行赋值。
/** * @brief 构造函数 * * @param[in] options * @param[in] expected_range_sensor_ids 所有range类型的话题 */ LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D( const proto::LocalTrajectoryBuilderOptions2D& options, const std::vector<std::string>& expected_range_sensor_ids) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), real_time_correlative_scan_matcher_( options_.real_time_correlative_scan_matcher_options()), ceres_scan_matcher_(options_.ceres_scan_matcher_options()), range_data_collator_(expected_range_sensor_ids) {}
其根据src/cartographer/configuration_files/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, }, }, },
2. 成员变量分析
const proto::SubmapsOptions2D options_; std::vector<std::shared_ptr<Submap2D>> submaps_; std::unique_ptr<RangeDataInserterInterface> range_data_inserter_; // 转换表, 第[0-32767]位置, 存的是[0.9, 0.1~0.9]的数据 ValueConversionTables conversion_tables_;
options_ 主要存储匹配信息，submaps_ 用于存储多个子图，range_data_inserter_ 与 ValueConversionTables 后续进行详细分析。下面来分析 ActiveSubmaps2D 的成员函数。
3. 成员函数分析
3.1 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(); }
对于 ActiveSubmaps2D::InsertRangeData() 函数从命名可以看出，其主要功能为插入雷达数据到子图中，流程如下：
(01) 如果submaps_ 不存在任何一个子图，或者 submaps_ 中最后一个子图数据的数量达到了与 options_.num_range_data()=90(默认配置)，则调用 ActiveSubmaps2D::AddSubmap() 函数新建一个子图。
(02) 将一帧雷达数据 range_data 写入到所有子图之中 submaps_ 。不过注意，通常 submaps_ 最多只包含两个子图。具体原因稍后讲解。
(03) 如果 submaps_ 中第一个子图中插入的数据数量达到了两倍 options_.num_range_data()，则把该子图标记为完成。
(04) 调用 ActiveSubmaps2D::submaps() 函数，使用共享指针返回 submaps_ 中的所有子图。
3.2 AddSubmap()
// 新增一个子图,根据子图个数判断是否删掉第一个子图 void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { // 调用AddSubmap时第一个子图一定是完成状态,所以子图数为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_)); }
上面提到，如果submaps_ 不存在任何一个子图，或者 submaps_ 中最后一个子图数据的数量达到了与 options_.num_range_data()=90(默认配置)，则调用 ActiveSubmaps2D::AddSubmap() 函数新建一个子图。
新建子图调用的函数就是 ActiveSubmaps2D::AddSubmap()，其目的是构件一个 Submap2D 独占指针对象，然后添加到 submaps_ 之后，不过有几个点是需要注意的：
如果 submaps_ 中包含的子图数量，即 submaps_.size() 大于等于 2，那么会删除掉 submaps_ 中的第一个地图。所以与前面的内容就呼应起来了，submaps_ 中最多存在两个子图。因为若 submaps_ 已经存在两个及两个以上的子图时，新建一个子图的同时会删除一个子图，所以依旧为两个子图。
3.3 CreateRangeDataInserter()
// 创建地图数据写入器 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."; } }
ActiveSubmaps2D 可以支持 概率栅格地图 与 tsdf地图，通过 ActiveSubmaps2D::CreateRangeDataInserter() 函数，根据配置信息可以构建 ProbabilityGridRangeDataInserter2D 与 TSDFRangeDataInserter2D 对象。
本人使用的是 ProbabilityGridRangeDataInserter2D，所以后续以其为例进行讲解。他们都派生自 RangeDataInserterInterface()，主要实现如下纯虚函数，用于插入雷达数据(后续会做详细讲解)。
// Inserts 'range_data' into 'grid'. virtual void Insert(const sensor::RangeData& range_data,GridInterface* grid) const = 0;
3.4 CreateGrid()
代码比较简单，这里就不做分析
// 以当前雷达原点为地图原点创建地图 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."; } }
3.5 ActiveSubmaps2D()
// ActiveSubmaps2D构造函数 ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) : options_(options), range_data_inserter_(CreateRangeDataInserter()) {}
很简单，就是将指向Submap2D 的 shared_ptr指针的vector进行返回，也即得到了所有子图
3.7 FinishSubmap()
这个函数没有找到定义，应该是只做了声明，没有去实现，先放下，后续再观察。
4. 重点函数分析
在 ActiveSubmaps2D 的这些成员函数中，不难看出，对于地图的相关处理都集中在成员函数 InsertRangeData() 函数，其还调用了另一个重要函数AddSubmap()，为了方便理解，根据下图进行讲解一下：
其上的每个方形代表一个子图 Submap2D，根据上面的分析知道 submaps_ 最多同时存在两个子图，这里假设现在 submaps_ 中存储的子图为 submaps_1与submaps_2。
那么submaps_1中插入的数据定然比子图2多 options_.num_range_data()，因为只有submaps_1达到 options_.num_range_data() 数据集时，submaps_2才被创建，后续在插入的数据是，会同时插入到子图1与子图2，也就是说，这两个子图的数据是存在交集的。
当子图2数据达到了 options_.num_range_data()，也就是此时submaps_1的数据为2倍的 options_.num_range_data()，会把submaps_1标记为完成状态，同时从 submaps_ 中删除该子图，这样submaps_2代替了之前子图1的位置，同时会再创建submaps_3添加到 submaps_ 之中。也就是说，此时 submaps_ 中包含了submaps_2与submaps_3，然后再继续往submaps_2,3插入数据，所以submaps_2与submaps_3也存在交集，依次循环下去。
最后了，会保证两个相邻的子图之间是存在共同数据的，其目的是为了点云匹配时，避免两个子图间出现断层的现象。具体的细节后续再做更加详细的分析。
二、Submap
在上一节中，对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。
class Submap2D : public Submap
Submap 在 src/cartographer/cartographer/mapping/submaps.h 文件中被声明，主要定义了一些纯虚函数，以及一些成员变量。代码如下：
/** * @brief 独立的子地图, 3个功能 * * 保存在local坐标系下的子图的坐标 * 记录插入到子图中雷达数据的个数 * 标记这个子图是否是完成状态 */ class Submap { public: // 构造函数, 将传入的local_submap_pose作为子图的坐标原点 Submap(const transform::Rigid3d& local_submap_pose) : local_pose_(local_submap_pose) {} virtual ~Submap() {} virtual proto::Submap ToProto(bool include_grid_data) const = 0; virtual void UpdateFromProto(const proto::Submap& proto) = 0; // Fills data into the 'response'. virtual void ToResponseProto( const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const = 0; // 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; } private: const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标 int num_range_data_ = 0; //子图中数据的数目，初始为0 bool insertion_finished_ = false; //是否为插入完成状态，初始为否。 };
需要注意到的是，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_，代码如下所示：
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_。代码如下：
// 根据proto::Submap格式的数据生成Submap2D Submap2D::Submap2D(const proto::Submap2D& proto, ValueConversionTables* conversion_tables) : Submap(transform::ToRigid3(proto.local_pose())), conversion_tables_(conversion_tables) { if (proto.has_grid()) { if (proto.grid().has_probability_grid_2d()) { grid_ = absl::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_); } else if (proto.grid().has_tsdf_2d()) { grid_ = absl::make_unique<TSDF2D>(proto.grid(), conversion_tables_); } else { LOG(FATAL) << "proto::Submap2D has grid with unknown type."; } } set_num_range_data(proto.num_range_data()); set_insertion_finished(proto.finished()); }
2. Submap2D::InsertRangeData()
在 Submap2D 中，还有几个成员函数：Submap2D::ToProto()， Submap2D::UpdateFromProto()，Submap2D::ToResponseProto() 都与 proto 相关，暂时不讲解。
先来看看看其中另外一个比较重要的函数Submap2D::InsertRangeData()：
// 将雷达数据写到栅格地图中 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); }
功能: 把点云数据插入到子图之中
输入: 【参数①range_data】→需要被插入的点云数据。【参数②range_data_inserter】→负责数据插入的实例对象，为 RangeDataInserterInterface 的派生类。
返回: 无
该函数实际上就是调用了 range_data_inserter->Insert(range_data, grid_.get()) 函数，将数据写入到栅格地图 grid_ 之中。从这里还可以看出每插入一帧数据，num_range_data 才会 +1，因为 range_data 中存储的并不是一个点云数据，而是一帧。
3. Submap2D::Finish()
// 将子图标记为完成状态 void Submap2D::Finish() { CHECK(grid_); CHECK(!insertion_finished()); grid_ = grid_->ComputeCroppedGrid(); // 将子图标记为完成状态 set_insertion_finished(true); }
该函数比较简单，其调用了 grid_->ComputeCroppedGrid() 函数，该函数后续再进行分析，然后设置 insertion_finished_ 变量，标记当前子图为完成状态。
4. Submap2D::grid()
const Grid2D* grid() const { return grid_.get(); }
该函数比较简单，其就是直接返回grid_对象的原始指针。
四、MapLimits
1. 类的调用分析
在上一节中，可以知道 Submap2D 中的 Grid2D 实例对象 grid_ 是十分重要的组成部分，回到之前 ActiveSubmaps2D::AddSubmap() 函数，存在如下代码：
// 新建一个子图, 并保存指向新子图的智能指针 submaps_.push_back(absl::make_unique<Submap2D>( origin, std::unique_ptr<Grid2D>( static_cast<Grid2D*>(CreateGrid(origin).release())), &conversion_tables_));
由此可知，Grid2D 的构建来自于 ActiveSubmaps2D::CreateGrid() 函数，该函数会构建 Grid2D 派生类对象 ProbabilityGrid 或者 TSDF2D 的独占指针。需要注意，在函数中可以看到如下代码:
// 概率栅格地图 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_);
也就是说，无论构建 ProbabilityGrid 还是 TSDF2D 实例对象指针，都需要传入MapLimits 对象作为实参。那么就来看看 MapLimits 代码中是如何实现的，位于 src/cartographer/cartographer/mapping/2d/map_limits.h 文件中。从命名来看，地图限制，其是限制了那些东西呢?
首先每个子图 Submap2D 或者说都对应的一个栅格(Grid)，后续每个栅格都会再进一步划分，划分之后以 cell 为单位，如下图所示，每个小方格都表示一个 cell：
既然要把子图 Submap2D 或者 Grid2D 划分成 cell 形式，那么肯定需要指定每个 Grid2D 应该被划分成多少个 cell。先来看看 MapLimits 这个类，在src/cartographer/cartographer/mapping/2d/map_limits.h中声明。
class MapLimits { public: /** * @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.); } explicit MapLimits(const proto::MapLimits& map_limits) : resolution_(map_limits.resolution()), max_(transform::ToEigen(map_limits.max())), cell_limits_(map_limits.cell_limits()) {} // Returns the cell size in meters. All cells are square and the resolution is // the length of one side. double resolution() const { return resolution_; } // Returns the corner of the limits, i.e., all pixels have positions with // smaller coordinates. // 返回左上角坐标, 左上角坐标为整个坐标系坐标的最大值 const Eigen::Vector2d& max() const { return max_; } // Returns the limits of the grid in number of cells. const CellLimits& cell_limits() const { return cell_limits_; } // 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)); } // 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)}; } // 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(); } private: double resolution_; Eigen::Vector2d max_; // cartographer地图坐标系左上角为坐标系的坐标的最大值 CellLimits cell_limits_; }; //其中CellLimits定义如下： // 地图x方向与y方向的格子数 struct CellLimits { CellLimits() = default; CellLimits(int init_num_x_cells, int init_num_y_cells) : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {} explicit CellLimits(const proto::CellLimits& cell_limits) : num_x_cells(cell_limits.num_x_cells()), num_y_cells(cell_limits.num_y_cells()) {} int num_x_cells = 0; int num_y_cells = 0; };
2. 类的构造函数
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.); }
该构造函数首先指定了地图的分辨率，该分辨率表示由 options_.grid_options_2d().resolution() 确定。这里我们约定两个坐标系，如下：
①地图坐标系→该坐标系以物理单位作为衡量。
②像素坐标系→该坐标系以像素为单位
约定了上述两个坐标系之后，那么所谓的分辨率就表示地图坐标系与像素坐标系的比值，简单的说就是栅格地图中一个像素代表地图坐标系多少个物理单位(米)。
第二个参数 max，表示的地图坐标的最大值，第三个参数 cell_limits 表示每个子图或者说每个栅格x,y方向上包含了多少个 cell。
3. MapLimits::GetCellIndex()
该函数从命名可以看出来，其是获得 cell 在像素坐标系中的索引。代码如下所示：
// 计算物理坐标点的像素索引 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。
4. MapLimits::GetCellCenter()
该函数的作用可以与 MapLimits::GetCellIndex() 是相反的，其输入一个像素索引，然后返回该像素对应在地图坐标系下的物理坐标：
// 根据像素索引算物理坐标 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() 的逆操作。
5. MapLimits::Contains()
该函数输入一个像素坐标索引，其会判断该像素是否存于栅格地图内部，代码注释如下：
// 判断给定像素索引是否在栅格地图内部 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(); }
6. resolution() 、max() 、cell_limits()
double resolution() const { return resolution_; } // Returns the corner of the limits, i.e., all pixels have positions with // smaller coordinates. // 返回左上角坐标, 左上角坐标为整个坐标系坐标的最大值 const Eigen::Vector2d& max() const { return max_; } // Returns the limits of the grid in number of cells. const CellLimits& cell_limits() const { return cell_limits_; }
这三个函数就是返回对应的成员变量 resolution_ max_ cell_limits_的值。
7. ToProto()
inline proto::MapLimits ToProto(const MapLimits& map_limits) { proto::MapLimits result; result.set_resolution(map_limits.resolution()); *result.mutable_max() = transform::ToProto(map_limits.max()); *result.mutable_cell_limits() = ToProto(map_limits.cell_limits()); return result; }
另外，在src/cartographer/cartographer/mapping/2d/map_limits.h 文件中，还定义了上面这个函数，其就是将map_limits的成员变量的相关属性转换成proto格式的数据，进行打包返回。
五、ValueConversionTables
下图为carto官方的背包实验建立的地图。
该栅格地图可以理解为一个灰度图，颜色越白的像素，说明该像素对应地图坐标系中cell被占用的概率越小，这里的占用可以理解为障碍物。
也就是说，越黑的区域是障碍物的概率概率就越大，如上图中墙壁是黑色的，等价于障碍物。
另外，在地图的外面，也就是上图中的灰色区域，表示没有探测的区域，其对应的cell区域有可能没有被占用，也可能占用了。
虽然最终的目的是需要形成这样一副栅格地图，但是源码中又是如何实现的呢？其核心就是src/cartographer/cartographer/mapping/2d/grid_2d.h文件中类Grid2D的成员变量：
// 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 std::vector<uint16> correspondence_cost_cells_;
该变量记录着一个Gird中所有cell对应地图的栅格值(被占用的概率越大，该值越大)，根据该成员变量，就可以绘画出一块Gird区域的栅格地图。
这里需要注意的一个点，correspondence_cost_cells_存储的是元素是uint16的类型，那么说明其取值范围为[0，2^16] = [0,65536]。
当然，最大值虽然是65536，但是不一定所有的数值都用到了。
回到src/cartographer/cartographer/mapping/2d/submap_2d.cc文件中的ActiveSubmaps2D::AddSubmap()函数，可见如下代码：
// 新建一个子图, 并保存指向新子图的智能指针 submaps_.push_back(absl::make_unique<Submap2D>( origin, std::unique_ptr<Grid2D>( static_cast<Grid2D*>(CreateGrid(origin).release())), &conversion_tables_));
可知创建一个子图，需要三个参数，
第一个参数origin表示激光雷达的原点在local坐标系下的位置，
第二个参数为ProbabilityGird类型的智能指针对象，
第三个参数为ValueConversionTables 类对象。
下面就来看一下ValueConversionTables这个类：
类的声明如下：
class ValueConversionTables { public: const std::vector<float>* GetConversionTable(float unknown_result, float lower_bound, float upper_bound); private: std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */, float /* upper_bound */>, std::unique_ptr<const std::vector<float>>> bounds_to_lookup_table_; };
1. 类的成员变量
std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */, float /* upper_bound */>, std::unique_ptr<const std::vector<float>>> bounds_to_lookup_table_;
bounds_to_lookup_table_是一个map容器，它的key是一个包含三个元素的tuple，value是一个装有float对象的vector容器指针。
2. ValueConversionTables::GetConversionTable()
/** * @brief 获取转换表, 这个函数只会调用1次 * * @param[in] unknown_result 0.9 未知时的值 * @param[in] lower_bound 0.1 最小correspondence_cost * @param[in] upper_bound 0.9 最大correspondence_cost * @return const std::vector<float>* */ const std::vector<float>* ValueConversionTables::GetConversionTable( float unknown_result, float lower_bound, float upper_bound) { // 将bounds作为key std::tuple<float, float, float> bounds = std::make_tuple(unknown_result, lower_bound, upper_bound); auto lookup_table_iterator = bounds_to_lookup_table_.find(bounds); // 如果没有bounds这个key就新建 if (lookup_table_iterator == bounds_to_lookup_table_.end()) { // 新建转换表 auto insertion_result = bounds_to_lookup_table_.emplace( bounds, PrecomputeValueToBoundedFloat(0, unknown_result, lower_bound, upper_bound)); return insertion_result.first->second.get(); } // 如果存在就直接返回原始指针 else { return lookup_table_iterator->second.get(); } }
首先用unknown_result(未知结果), lower_bound(最小边界值), upper_bound(最大边界值)这三个形参构建一个tuple对象bounds，将其作为key，然后在map中对这个key进行查找，如果查找成功，就返回指向vector容器的原始指针；如果查找不成功，将bounds作为key，将PrecomputeValueToBoundedFloat(0, unknown_result, lower_bound, upper_bound)作为value，插入到map对象bounds_to_lookup_table_,并且返回该转换表，这样下次就可以获取该转换表了。下面就对PrecomputeValueToBoundedFloat()这个函数进行解析：
2.1 PrecomputeValueToBoundedFloat()
/** * @brief 新建转换表 * * @param[in] unknown_value 0 * @param[in] unknown_result 0.9 * @param[in] lower_bound 0.1 * @param[in] upper_bound 0.9 * @return std::unique_ptr<std::vector<float>> 转换表的指针 */ std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat( const uint16 unknown_value, const float unknown_result, const float lower_bound, const float upper_bound) { auto result = absl::make_unique<std::vector<float>>(); // num_values = 65536 size_t num_values = std::numeric_limits<uint16>::max() + 1; // 申请空间 result->reserve(num_values); // 将[0, 1~32767]映射成[0.9, 0.1~0.9] // vector的个数为65536, 所以存的是2遍[0-32767]的映射 for (size_t value = 0; value != num_values; ++value) { result->push_back(SlowValueToBoundedFloat( static_cast<uint16>(value) & ~kUpdateMarker, // 取右边15位的数据, 0-32767 unknown_value, unknown_result, lower_bound, upper_bound)); } return result; }
先创建一个指向vector容器的智能指针result，将uint16无符号整型取值最大值+1=65536赋给num_values，然后调用reserve()对result进行内存申请，内存空间中元素的个数为num_values。最后使用for循环对result中每一个元素进行赋值，这里需要注意由于num_values为65536，所以存的是2遍[0-32767]的映射，赋值中使用到一个函数为SlowValueToBoundedFloat()，下面对这个函数进行解析：
2.2 SlowValueToBoundedFloat()
/** * @brief 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] * * @param[in] value [0, 32767]的值, 0 对应0.9 * @param[in] unknown_value 0 * @param[in] unknown_result 0.9 * @param[in] lower_bound 0.1 下界 * @param[in] upper_bound 0.9 上界 * @return float 转换后的数值 */ float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value, const float unknown_result, const float lower_bound, const float upper_bound) { CHECK_LE(value, 32767); if (value == unknown_value) return unknown_result; const float kScale = (upper_bound - lower_bound) / 32766.f; return value * kScale + (lower_bound - kScale); }
如果value=unknown_value=0时，其对应概率为unknown_result=0.9。1~32767对应概率为0.1~0.9，处理方式使用线性插值的方法y=kx+b。
3. 总结
总的来说，ValueConversionTables中实现了成员函数GetConversionTable()，该函数会根据传入的三个实参unknown_result=0.9, lower_bound=0.1, upper_bound=0.9构建转换表，该转换表可以把[0, 1~32767] 映射到 [0.9, 0.1~0.9]。
之所以这样做是因为直接计算会比较耗时，所以这里先预先计算出来，用空间换时间，后续通过索引的方式，就可以把[0, 1~32767] 转换成 [0.9, 0.1~0.9]。
六、Grid2D
通过前面的介绍，了解到所有submap2D对象共用ActiveSubmaps2D中转换表ValueConversionTables conversion_tables_。该转换表的功能是通过索引的方式把[0, 1~32767] 转换成 [0.9, 0.1~0.9]的数值。该转换表在创建子图submap2D时，会传递给ProbabilityGrid。关于ProbabilityGrid的内容后续再进行了解，ProbabilityGrid构造函数又会把转换表传递给Grid2D。Gird2D声明于src/cartographer/cartographer/mapping/2d/grid_2d.h文件中。下面对这Grid2D这个类进行分析：
1. 类的成员变量
private: MapLimits limits_; // 地图大小边界, 包括x和y最大值, 分辨率, x和y方向栅格数 // 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 std::vector<uint16> correspondence_cost_cells_; float min_correspondence_cost_; //correspondence_cost_cells_ 中的最小值 float max_correspondence_cost_; //correspondence_cost_cells_ 中的最大值 std::vector<int> update_indices_; // 记录已经更新过的索引 // Bounding box of known cells to efficiently compute cropping limits. Eigen::AlignedBox2i known_cells_box_; // 栅格的bounding box, 存的是像素坐标 // 将[0, 1~32767] 映射到 [0.9, 0.1~0.9] 的转换表 const std::vector<float>* value_to_correspondence_cost_table_;
其中correspondence_cost_cells_是最重要的一个变量，其记录着Gird2D中所有cell对应的栅格值。
2. 类的构造函数
类的构造函数有两个，先来看第一个，代码如下：
/** * @brief 构造函数 * * @param[in] limits 地图坐标信息 * @param[in] min_correspondence_cost 最小correspondence_cost 0.1 * @param[in] max_correspondence_cost 最大correspondence_cost 0.9 * @param[in] conversion_tables 传入的转换表指针 */ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, float max_correspondence_cost, ValueConversionTables* conversion_tables) : limits_(limits), correspondence_cost_cells_( limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, kUnknownCorrespondenceValue), // 0 min_correspondence_cost_(min_correspondence_cost), // 0.1 max_correspondence_cost_(max_correspondence_cost), // 0.9 // 新建转换表 value_to_correspondence_cost_table_(conversion_tables->GetConversionTable( max_correspondence_cost, min_correspondence_cost, max_correspondence_cost)) { CHECK_LT(min_correspondence_cost_, max_correspondence_cost_); }
①形参：传入地图限制这个类，最小最大的空闲概率以及转换表的类指针。
②对成员变量进行初始化，其中新建转换表用到了ValueConversionTables这个类的成员函数GetConversionTable()，这个函数在上面在上一节已经介绍。
③最后比较min_correspondence_cost_是否小于max_correspondence_cost_
再来看它的第二个构造函数，代码如下：
Grid2D::Grid2D(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : limits_(proto.limits()), correspondence_cost_cells_(), min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), value_to_correspondence_cost_table_(conversion_tables->GetConversionTable( max_correspondence_cost_, min_correspondence_cost_, max_correspondence_cost_)) { CHECK_LT(min_correspondence_cost_, max_correspondence_cost_); if (proto.has_known_cells_box()) { const auto& box = proto.known_cells_box(); known_cells_box_ = Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), Eigen::Vector2i(box.max_x(), box.max_y())); } correspondence_cost_cells_.reserve(proto.cells_size()); for (const auto& cell : proto.cells()) { CHECK_LE(cell, std::numeric_limits<uint16>::max()); correspondence_cost_cells_.push_back(cell); } }
使用proto格式的数据进行类的构造，代码简单，这里不做解释。
3. Grid2D::limits()
const MapLimits& limits() const { return limits_; }
返回地图范围边界这个成员变量。
4. Grid2D::FinishUpdate()
// 插入雷达数据结束 void Grid2D::FinishUpdate() { while (!update_indices_.empty()) { DCHECK_GE(correspondence_cost_cells_[update_indices_.back()], kUpdateMarker); // 更新的时候加上了kUpdateMarker, 在这里减去 correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker; update_indices_.pop_back(); } }
该函数的主要作用就是需要保证update_indices_最后一个元素作为索引，其对应的cell栅格值需要大于kUpdateMarker，然后该栅格值减去kUpdateMarker，最后将update_indices_最后一个元素删除。
这里暂时留下一个疑问，那就是这样做的目的与作用是什么？→疑问1（原作者提出）
5. Grid2D::GetCorrespondenceCost()
float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { if (!limits().Contains(cell_index)) return max_correspondence_cost_; return (*value_to_correspondence_cost_table_) [correspondence_cost_cells()[ToFlatIndex(cell_index)]]; }
6. Grid2D::GetGridType()
GridType ProbabilityGrid::GetGridType() const { return GridType::PROBABILITY_GRID; } GridType TSDF2D::GetGridType() const { return GridType::TSDF; }
7. Grid2D::GetMinCorrespondenceCost() Grid2D::GetMaxCorrespondenceCost()
float GetMinCorrespondenceCost() const { return min_correspondence_cost_; } float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; }
8. Grid2D::IsKnown()
// 指定的栅格是否被更新过 bool IsKnown(const Eigen::Array2i& cell_index) const { return limits_.Contains(cell_index) && correspondence_cost_cells_[ToFlatIndex(cell_index)] != kUnknownCorrespondenceValue; }
9. Grid2D::ComputeCroppedLimits()
该函数的两个形参均为指针常量，也就是说offset和limits本身存储的地址是不能改变的，但是其指向的目标是可以改变的，这里所谓的剪切，实际上就是对offset和limits重新进行赋值。
其赋值的依据来自于成员变量Eigen::AlignedBox2i known_cells_box_，该成员变量存储的是多个cell对应的像素坐标，实际上是offset记录known_cells_box_中x,y的最小值，limits记录x,y最大值与最小值的差值。
// 根据known_cells_box_更新limits void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, CellLimits* const limits) const { if (known_cells_box_.isEmpty()) { *offset = Eigen::Array2i::Zero(); *limits = CellLimits(1, 1); return; } *offset = known_cells_box_.min().array(); *limits = CellLimits(known_cells_box_.sizes().x() + 1, known_cells_box_.sizes().y() + 1); }
简单地说，基于像素坐标系，offset表示一个box左上角坐标，limits记录该box的宽高＋1.从这里可以看出，关于limits和offset的更新主要依赖于known_cells_box_，那么known_cells_box_又是如何变换更新的呢？
暂时先记着→疑问2
10. Grid2D::GrowLimits()
该函数存在两个重载，如下图所示（实际上第一个重载函数最终调用了第二个重载函数）：
void Grid2D::GrowLimits(const Eigen::Vector2f& point) { GrowLimits(point, {mutable_correspondence_cost_cells()}, {kUnknownCorrespondenceValue}); } void Grid2D::GrowLimits(const Eigen::Vector2f& point, const std::vector<std::vector<uint16>*>& grids, const std::vector<uint16>& grids_unknown_cell_values) {
从函数的命名规则可以看到，该函数的目的是为了增大MapLimits limits_，其主要包含着一个子图（或者说Grid）大小边界，如x和y的最大值，分辨率，x和y方向的栅格数。对地图的扩大，本质上是对这些属性的修改，源码实现逻辑如下，先来看下图：
假若传入的地图点（物理坐标）point,其没有被包含在上图的蓝色区域，也就是源码的limits_之中， 则会对蓝色地图进行扩大，扩大成绿色区域，从上图可以看出由蓝色地图扩大成绿色地图，其x,y都分别扩大了2倍，那么源码中又是如何实现的呢?
（1）传入的参数point地图的物理坐标，转换成像素坐标之后，判断其对应的cell是否位于目前的地 图坐标系内，如果不在，则说明现在的地图太小了，需要扩大。
（2）地图的扩大首先是对MapLimits limits_进行调整，首先地图的原点与分辨率不变，然后地图x,y 的最大值max增加原本最大值的一半，并且把地图x，y方向cell的数量都扩大至原来的两倍。
（3）老坐标系的原点在新坐标系下的一维像素坐标offset,也就是上图中红色点O在紫色点O坐标系的 一维坐标。
（4）根据求得的offset，将老地图的栅格值拷贝到新地图上，新地图保存栅格值的变量为 new_cells,拷贝完成之后执行*grids[grid_index]=new_cells,用新地图替换掉原来的老地图。
（5）更新完limits_之后，因为地图变了，所以需要对known_cells_box_进行调整，需要把这些坐标恢复到老地图中的位置， 直接平移x_offset, y_offset 个单位即可。
总结 其主要思路是先把地图扩大（x_offset,y_offset)个单位，然后将地图的最大值增加原本最大值的一半，然后把老地图的栅格值拷贝到新地图上。
另外需要注意的是，地图的增加实在while循环中，只有point位于地图内，才会停止扩充地图。代码注释如下：
// 根据坐标决定是否对地图进行扩大 void Grid2D::GrowLimits(const Eigen::Vector2f& point, const std::vector<std::vector<uint16>*>& grids, const std::vector<uint16>& grids_unknown_cell_values) { CHECK(update_indices_.empty()); // 判断该点是否在地图坐标系内 while (!limits_.Contains(limits_.GetCellIndex(point))) { const int x_offset = limits_.cell_limits().num_x_cells / 2; const int y_offset = limits_.cell_limits().num_y_cells / 2; // 将xy扩大至2倍, 中心点不变, 向四周扩大 const MapLimits new_limits( limits_.resolution(), limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), CellLimits(2 * limits_.cell_limits().num_x_cells, 2 * limits_.cell_limits().num_y_cells)); const int stride = new_limits.cell_limits().num_x_cells; // 老坐标系的原点在新坐标系下的一维像素坐标 const int offset = x_offset + stride * y_offset; const int new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells; // grids.size()为1 for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) { std::vector<uint16> new_cells(new_size, grids_unknown_cell_values[grid_index]); // 将老地图的栅格值复制到新地图上 for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { new_cells[offset + j + i * stride] = (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells]; } } // 将新地图替换老地图, 拷贝 *grids[grid_index] = new_cells; } // end for // 更新地图尺寸 limits_ = new_limits; if (!known_cells_box_.isEmpty()) { // 将known_cells_box_的x与y进行平移到老地图的范围上 known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); } } }
11. Grid2D::correspondence_cost_cells() 等
// 返回不可以修改的栅格地图数组的引用 const std::vector<uint16>& correspondence_cost_cells() const { return correspondence_cost_cells_; } const std::vector<int>& update_indices() const { return update_indices_; } const Eigen::AlignedBox2i& known_cells_box() const { return known_cells_box_; } // 返回可以修改的栅格地图数组的指针 std::vector<uint16>* mutable_correspondence_cost_cells() { return &correspondence_cost_cells_; } std::vector<int>* mutable_update_indices() { return &update_indices_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
七、ProbabilityGrid
带着上面两个疑问，看下在 ProbabilityGrid 这个类中是否能够找到相关的答案。ProbabilityGrid 是 Grid2D 的派生类，该类声明于 src/cartographer/cartographer/mapping/2d/probability_grid.h 文件中，可见其成员变量还是比较简单的，仅仅一个转换表 ValueConversionTables* conversion_tables_ 而已。下面就来看看 probability_grid.cc 文件中其成员函数的实现。
1. 类的构造函数
其存在两个构造函数，一个是根据参数 const MapLimits& limits 与 ValueConversionTables* conversion_tables 实例化对象，另外一个是根据 proto::Grid2D 的配置参数构造对象。但是过程都比较简单，主要就是构建了一个 Grid2D 对象，然后把转换表赋值给了成员变量 conversion_tables_。
第一个构造函数在调用父类构造函数的时候，传入了参数 kMinCorrespondenceCost 与 kMaxCorrespondenceCost，着两个值都为常量，定义如下：
constexpr float kMinProbability = 0.1f; // 0.1 constexpr float kMaxProbability = 1.f - kMinProbability; // 0.9 constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 0.1 constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 0.9
kMinCorrespondenceCost 与 kMaxCorrespondenceCost 分别记录 Grid2D::correspondence_cost_cells_ 的最小值与最大值，当然指的是通过转换表映射之后的结果。
1.1 ProbabilityGrid::ProbabilityGrid()
/** * @brief ProbabilityGrid的构造函数 * * @param[in] limits 地图坐标信息 * @param[in] conversion_tables 转换表 */ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits, ValueConversionTables* conversion_tables) : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, conversion_tables), conversion_tables_(conversion_tables) {}
1.2 ProbabilityGrid::ProbabilityGrid()
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { CHECK(proto.has_probability_grid_2d()); }
2. ProbabilityGrid::SetProbability()
从函数名可以直到，该函数的作用是设置栅格地图的概率值，第一个形参 cell_index 为 cell 的索引，第二个形参 probability 表示该 cell 被占用的概率。
其首先是通过 cell_index 获得该索引对应的cell，其是通过 mutable_correspondence_cost_cells() 函数获得，所以可以对该cell 的栅格值进行修改。不过由于 probability 表示该 cell 被占用的机率，所以这里调用了 ProbabilityToCorrespondenceCost() 函数，实际上就是 1-probability，获得未被占用的概率。然后再调用 CorrespondenceCostToValue() 函数，该函数的作用是把 [ 0.9 , 0.1 ∽ 0.9 ] 的数值映射至 [ 0 , 1 ∽ 32767 ]，可以看做是转换表的逆操作。
因为对该 cell 被重新设置，说明其已经被探索到，即其是已知的了，所以通过 mutable_known_cells_box 函数把 cell 的二维索引(也可以看作是像素坐标)添加至 known_cells_box_ 之中。代码注释如下：
// Sets the probability of the cell at 'cell_index' to the given // 'probability'. Only allowed if the cell was unknown before. // 将 索引 处单元格的概率设置为给定的概率, 仅当单元格之前处于未知状态时才允许 void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, const float probability) { // 获取对应栅格的引用 uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)]; CHECK_EQ(cell, kUnknownProbabilityValue); // 为栅格赋值 value cell = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability)); // 更新bounding_box mutable_known_cells_box()->extend(cell_index.matrix()); }
3. ProbabilityGrid::ApplyLookupTable()
从该函数的命名来看，叫做应用查询表，那么其是如何应用的，且作用是什么呢？该函数的主要逻辑如下：
(1) 其传入的参数 cell_index 与上一函数相同，通过该索引可以获得 correspondence_cost_cells_ 中对应的 cell；另一参数 table 就是查询表，通过该查询表可以可以把 [ 0 , 1 ∽ 32767 ] 的数值映射到 [ 0.9 , 0.1 ∽ 0.9 ]。
(2) 该函数首先判断一下查询表 table 的大小，需要保证其为 kUpdateMarker=32768，否则报错。然后把二维 cell_index 变换成一维索引 flat_index。然后调用 mutable_correspondence_cost_cells() 函数，再借助 flat_index 获得其对应的 cell。判断一下 *cell >= kUpdateMarker 是否成立，如果成立表示该 cell 已经更新过了，无需再次更新，同时返回 false。
(3) 该 cell 没有更新过，则通过 mutable_update_indices() 函数把 flat_index 添加到 update_indices_ 之中，表示该 cell 已经更新过了，然后再对其进行更新。更新的操作为 ： *cell = table[*cell]。这里没有看懂→疑问3
(4) 如果当前的 cell 更新了，说明该 cell 已经探索到，属于已知的 cell，通过 mutable_known_cells_box() 函数把其像素坐标添加到 known_cells_box_ 之中。
该函数的代码注释如下：
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'cell_index' if the cell has not already // been updated. Multiple updates of the same cell will be ignored until // FinishUpdate() is called. Returns true if the cell was updated. // 如果单元格尚未更新,则将调用 ComputeLookupTableToApplyOdds() 时指定的 'odds' 应用于单元格在 'cell_index' 处的概率 // 在调用 FinishUpdate() 之前，将忽略同一单元格的多次更新。如果单元格已更新，则返回 true // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. // 如果这是对指定单元格第一次调用 ApplyOdds(),则其值将设置为与 'odds' 对应的概率 // 使用查找表对指定栅格进行栅格值的更新 bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector<uint16>& table) { DCHECK_EQ(table.size(), kUpdateMarker); const int flat_index = ToFlatIndex(cell_index); // 获取对应栅格的指针 uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index]; // 对处于更新状态的栅格, 不再进行更新了 if (*cell >= kUpdateMarker) { return false; } // 标记这个索引的栅格已经被更新过 mutable_update_indices()->push_back(flat_index); // 更新栅格值 *cell = table[*cell]; DCHECK_GE(*cell, kUpdateMarker); // 更新bounding_box mutable_known_cells_box()->extend(cell_index.matrix()); return true; }
4. ProbabilityGrid::GetProbability()
该函数较为简单，其就是传入一个cell索引值，然后获得该 cell 被占用的概率。通过 correspondence_cost_cells() 函数，结合 cell 一维索引获得的是 cell 对应的未被占用的栅格值，其范围是 [ 0 , 1 ∽ 32767 ] ，所以还要调用 ValueToCorrespondenceCost() 函数，把其映射到[0.9, 0.1∽0.9] 上。映射之后的概率表示未被占用的概率，所用最后还调用了 CorrespondenceCostToProbability() 把其转换成 cell 被占用的概率。
// Returns the probability of the cell with 'cell_index'. // 获取 索引 处单元格的占用概率 float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { if (!limits().Contains(cell_index)) return kMinProbability; return CorrespondenceCostToProbability(ValueToCorrespondenceCost( correspondence_cost_cells()[ToFlatIndex(cell_index)])); }
这里额外来看一下 install_isolated/include/cartographer/mapping/probability_values.h 文件中的图下代码：
// c++11: extern c风格 extern const std::vector<float>* const kValueToProbability; extern const std::vector<float>* const kValueToCorrespondenceCost; // Converts a uint16 (which may or may not have the update marker set) to a // probability in the range [kMinProbability, kMaxProbability]. inline float ValueToProbability(const uint16 value) { return (*kValueToProbability)[value]; } // Converts a uint16 (which may or may not have the update marker set) to a // correspondence cost in the range [kMinCorrespondenceCost, // kMaxCorrespondenceCost]. inline float ValueToCorrespondenceCost(const uint16 value) { return (*kValueToCorrespondenceCost)[value]; }
其上的 kValueToProbability 与 kValueToCorrespondenceCost 是两个转换表，第一个表示转换成被占用的概率，第二个表示未被占用的概率。这两个转换表在 src/cartographer/cartographer/mapping/probability_values.cc 文件中定义。
5. ProbabilityGrid::ComputeCroppedGrid()
通过前面一系列的分析，有了知识储备之后，再来分析该函数就比较简单了。在前面提到过，可以把子图看作一个 Grid，从函数名可以看出，该函数的主要目的就是对 Grid 进行剪切，那么如何剪切呢？无论其如何剪切，都必须保证已经探索过的 cell 被保留下来，源码的主体流程如下：
(1) 首先调用 ComputeCroppedLimits() 函数，然后把 offset 与 cell_limits 的地址作为实参进行传递。该函数是在父类 Grid2D 中实现的，前面已经讲解过，其主要是获得 known_cells_box_ 最小值(xy)，即其高宽。简单的说，就把 known_cells_box_ 看作 Grid(或子图) 上的一个框，这个框包含了所有目前探索过 cell 对应的像素坐标。
(2) 通过 ComputeCroppedLimits() 函数后，offset 与 limits 共同描述了 known_cells_box_ 的区域， 根据该区域重新定义地图。offset 表示旧栅格地图原点(左上角)到 known_cells_box_ 原点(左上角)的偏移值，新地图首先要减去该偏移值，然后使用 cell_limits 定义新地图的栅格数。这些就获得了剪切之后的新地图 cropped_grid。
(3) 通过 GetProbability(xy_index + offset) 函数获取旧地图 cell 被占用的概率，然后赋值给新地图 cropped_grid，本质上是对 cropped_grid::correspondence_cost_cells_ 变量进行操作。最后返回新地图 cropped_grid 的智能指针。
代码注释如下：
// 根据bounding_box对栅格地图进行裁剪到正好包含点云 std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const { Eigen::Array2i offset; CellLimits cell_limits; // 根据bounding_box对栅格地图进行裁剪 ComputeCroppedLimits(&offset, &cell_limits); const double resolution = limits().resolution(); // 重新计算最大值坐标 const Eigen::Vector2d max = limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); // 重新定义概率栅格地图的大小 std::unique_ptr<ProbabilityGrid> cropped_grid = absl::make_unique<ProbabilityGrid>( MapLimits(resolution, max, cell_limits), conversion_tables_); // 给新栅格地图赋值 for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { if (!IsKnown(xy_index + offset)) continue; cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset)); } // 返回新地图的指针 return std::unique_ptr<Grid2D>(cropped_grid.release()); }
6. ProbabilityGrid::DrawToSubmapTexture()
从函数命名来看，其功能是在子图上绘画文本。关于地图栅格数据的存储，使用 std::string 的形式，源码中创建了变量
std::string cells。其每2字节描述描述一个cell：
①第一个字节→栅格值value
②第二个字节→alpha透明度
(1) 调用父类 ComputeCroppedLimits(&offset, &cell_limits) 函数，对栅格地图进行剪裁。创建一个 std::string 对象 cells。然后对剪切之后地图所有cell进行遍历。
(2) 如果根据 known_cells_box_ 剪切出来的 cell 不在原地图之中，则把该 cell 的两个字节都设置为0，表示未知。
(3) 如果根据 known_cells_box_ 剪切出来的 cell 在原地图之中，那么首先获取该cell被占用的概率值，通过 ProbabilityToLogOddsInteger() 函数把其映射到 [1, 255] 之间，然后再映射到[-127, 127]，使用变量 delta 表示。关于 ProbabilityToLogOddsInteger() 函数后面单独分析。
(4) delta 的范围在 [-127, 127] 之间，总的来说 delta>0时，越接近127，表示 cell 被占用的机率越大。总的来说 delta<0时，越接近-127，表示 cell 没有被占用的机率越大。
(5) ①→当 delta > 0 时，alpha=0，value = delta。 ②→当 delta <= 0 时，alpha=-delta，value=0。
从这里可以看出当 delta > 0 时, 其直接被设置为栅格值value，delta < 0 时, 栅格值value被设置为0。对于透明度alpha，当 delta > 0 时，alpha=0，表示不透明。当 delta <= 0 时，透明度设置为 -delta。也就是说最终透明度 alpha 使用一个正值来表示，越接近 127，则透明度越高。
(6) 总的来说当栅格cell被占用时，value>0，alpha=0。当栅格cell未被占用时，value=0，alpha>0。另外如果 value=alpha=0时，透明度会被设置成1。
(7) 把所有cell的栅格信息以 value 与 alpha 的形式存储到 std::string cells 之后，会调用 common::FastGzipString 对栅格地图数据进行压缩，压缩结果存储于 texture 之中。
(8) 填充地图信息，如宽高cell数，分辨率等等，最后调用了 *texture->mutable_slice_pose() 函数进行赋值，这里没有看明白，先记一下。
关于 ProbabilityGrid::DrawToSubmapTexture() 函数的注释如下:
// 获取压缩后的地图栅格数据 bool ProbabilityGrid::DrawToSubmapTexture( proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const { Eigen::Array2i offset; CellLimits cell_limits; // 根据bounding_box对栅格地图进行裁剪 ComputeCroppedLimits(&offset, &cell_limits); std::string cells; // 遍历地图, 将栅格数据存入cells for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { if (!IsKnown(xy_index + offset)) { cells.push_back(0 /* unknown log odds value */); cells.push_back(0 /* alpha */); continue; } // We would like to add 'delta' but this is not possible using a value and // alpha. We use premultiplied alpha, so when 'delta' is positive we can // add it by setting 'alpha' to zero. If it is negative, we set 'value' to // zero, and use 'alpha' to subtract. This is only correct when the pixel // is currently white, so walls will look too gray. This should be hard to // detect visually for the user, though. // 我们想添加 'delta'，但使用值和 alpha 是不可能的 // 我们使用预乘 alpha，因此当 'delta' 为正时，我们可以通过将 'alpha' 设置为零来添加它。 // 如果它是负数，我们将 'value' 设置为零，并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确，因此墙壁看起来太灰。 // 但是，这对于用户来说应该很难在视觉上检测到。 // delta处于[-127, 127] const int delta = 128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset)); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度 cells.push_back(value); cells.push_back((value || alpha) ? alpha : 1); } // 保存地图栅格数据时进行压缩 common::FastGzipString(cells, texture->mutable_cells()); // 填充地图描述信息 texture->set_width(cell_limits.num_x_cells); texture->set_height(cell_limits.num_y_cells); const double resolution = limits().resolution(); texture->set_resolution(resolution); const double max_x = limits().max().x() - resolution * offset.y(); const double max_y = limits().max().y() - resolution * offset.x(); *texture->mutable_slice_pose() = transform::ToProto( local_pose.inverse() * transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); return true; }
7. ProbabilityToLogOddsInteger()
现在呢，我们回过头来看一下 ProbabilityGrid::DrawToSubmapTexture() 中调用的 ProbabilityToLogOddsInteger() 函数，该函数实现于 src/cartographer/cartographer/mapping/submaps.h 文件中，其主要原理可以阅读 Cartographer 论文：Real-Time Loop Closure in 2D LIDAR SLAM
// Converts the given probability to log odds. // 对论文里的 odds(p)函数 又取了 log inline float Logit(float probability) { return std::log(probability / (1.f - probability)); } const float kMaxLogOdds = Logit(kMaxProbability); const float kMinLogOdds = Logit(kMinProbability); // Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds, // kMaxLogOdds] is mapped to [1, 255]. inline uint8 ProbabilityToLogOddsInteger(const float probability) { const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) * 254.f / (kMaxLogOdds - kMinLogOdds)) + 1; CHECK_LE(1, value); CHECK_GE(255, value); return value; }
首先来看 Logit 这个函数，这里设代码 probability / ( 1. f − probability ) = odds ，这里的 odds 与论文中一致，其输入probability 表示 cell被占用的概率，odds 表示被占用概率与未被占用概率的比值，
可想而知：odds 越大，表示cell被占用的概率越大。
如果cell被占用的概率与未被占用的概率都未0.5相等时，odds 为1。
如果cell未被占用的机率很大很大，那么 odds 是趋向于0的一个数。
也就是说，当占用机率大于未被占用机率时，odds>1；占用小于未被占用机率时，odds<1；
如果占用机率等于未被占用机率都为0.5时 odds=1；在根据 log函数(e为底) 的性质，可以把 odds 映射到 [ − ∞ , + ∞ ]，为了方便讲解，映射之后的结果记为 odds’。
很显然，odds’ 直接使用是不合适的，因为其区间为 [ − ∞ , + ∞ ]，论文中使用了一个巧妙的办法，那就 probability 最大值为0.9，那么未被占用的概率最小为0.1，现在回过头来看 src/cartographer/cartographer/mapping/probability_values.h 文件中定义的如下变量：
constexpr float kMinProbability = 0.1f; // 0.1 constexpr float kMaxProbability = 1.f - kMinProbability; // 0.9 constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 0.1 constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 0.9
就比较好理解了，ProbabilityToLogOddsInteger()函数由于输入 probability 只能在 [01,0.9] 之间取值，所以可以把 odds’ 夹紧到区间 [kMinLogOdds,kMaxLogOdds]。最后再把区间映射到 [1,255] 区间上。这样就达到了 ProbabilityToLogOddsInteger() 函数的最终目的。
8. 疑问2的作用
known_cells_box_ 的作用是记录一块区域，该区域把探测过，更新过的 cell 都包揽进去了，再对 cell 更新的同时需要对 known_cells_box_ 也进行更新。
八、ProbabilityGridRangeDataInserter2D
1. 前言
目前存在两个疑问是待解决的：
疑问1→为什么Gird::FinishUpdate()函数中，栅格值为什么需要减去KUpdateMarker？
疑问3→ProbabilityGrid::ApplyLookupTable()函数中对于cell的更新比较奇怪，代码为*cell = table[*cell]。
也就是说，接下来分析的过程中需要注意Grid2D:FinishUpdate()与 ProbabilityGrid::ApplyLookupTable()这两个函数的调用。虽然对 ActiveSubmaps2D、Submap2D、 Submap、Grid2D、ProbabilityGrid都进行了具体的分析，那么这些类又是如何串联起来的?或者说他们是如何相互配合共同工作的呢?
这就是接下来需要讲解的内容了！把这些类都串起来的核心函数就是 ProbabilityGridRangeDatalnserter2D::Insert()，在对其进行讲解之前来回顾一下前面的内容。函数间的调用关系如下：
LocalTrajectoryBuilder2D::AddRangeData() //添加点云数据 LocalTrajectoryBuilder2D::AddAccumulatedRangeData()//添加累计，经过处理之后的点云数据 LocalTrajectoryBuilder2D::InsertIntoSubmap() //将处理后的雷达数据写入submap
最终找到了 InsertintoSubmap()函数，其实现于 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 文件中,该函数就是LocalTrajectoryBuilder2D 与2D 栅格地图的桥梁。 其核心功能是调用了 ActiveSubmaps2D::InsertRangeData()函数，然后构建了 InsertionResult 格式的数据返回。其中ActiveSubmaps2D::InsertRangeData()函数在前面已经做过简单的讲解，主要核心代码如下：
AddSubmap(range_data.origin.head<2>() //如果没有子图则回新建子图 submap->InsertRangeData(range_data, range_data_inserter_.get()); //将雷达数据插入到相邻的两个子图之中 submaps_.front()->Finish(); //如果第一个子图插入的点云帧数足够，则把该子图标记为完成状态
上述中， submap->InsertRangeData()函数的核心是执行了如下代码：
range_data_inserter->Insert(range_data, grid_.get());
这里的 range_data_inserter,是在ActiveSubmaps2D 构造函数的初始化列表调用 CreateRangeDatalnserter()函数创建的。CreateRangeDatalnserter()函数实现于submap_2d.cc文件中:
// 创建地图数据写入器 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."; } }
我们配置的是PROBABILITY_GRID_INSERTER_2D, 所以构建的是 ProbabilityGridRangeDatalnserter2D 类对象,该类继承于RangeDatalnserterinterface。
总的来说，submap->InsertRangeData()最终调用到的是 ProbabilityGridRangeDatalnserter2D::Insert()函数,该函数在src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc文件中实现。 ProbabilityGridRangeDatalnserter2D 存在三个私有成员变量,这里先记录一下：
private: const proto::ProbabilityGridRangeDataInserterOptions2D options_; const std::vector<uint16> hit_table_; const std::vector<uint16> miss_table_;
2. 疑问1和疑问3的引入
对于疑问1与疑问3的解答，在 ProbabilityGridRangeDatalnserter2D 的构造函数中可以找到答案，先来看看 ProbabilityGridRangeDatalnserter2D 的构造函数：
// 写入器的构造, 新建了2个查找表 ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D( const proto::ProbabilityGridRangeDataInserterOptions2D& options) : options_(options), // 生成更新占用栅格时的查找表 // param: hit_probability hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( Odds(options.hit_probability()))), // 0.55 // 生成更新空闲栅格时的查找表 // param: miss_probability miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( Odds(options.miss_probability()))) {} // 0.49
从这里来看，可以知道其构造函数主要就是根据hit_probability与miss_probability 创建了两个查询表 hit_table_与 miss_table_。其都是调用 ComputeLookupTableToApplyCorrespondenceCostodds()函数。该函数实现于src/cartographer/cartographer/mapping/probability_values.cc 文件中。
提示：为了大家透彻的理解该函数，这里先说一下该函数的目的。首先，该函数会返回一个更新查询表，该表有什么作用呢? 前面分析过程中，已经知道 ProbabilityGrid::ApplyLookupTable()函数中， 会对cell进行更新，实际上也就是对correspondence_cost_cells_的更新。
那么是如何更新的？ 在疑问3提到更新的代码*cell =table[*cell] 比较怪异，不是很明白。起始呢，算法的原理是这样的，先来看论文中更新的公式：
其上的这里这里以hit_probability=options.hit_probability()=0.55为例，那么上式中的 Phit=0.55, Mold(x)表示cell未更新之前的被占用的概率值，Mnew（)表示更新之后的cell被占用的概率值。 odds^-1表示的就是odds的逆操作，在上一篇博客中提到odds表示被占用概率与未被占用概率的比值，论文中的公式如下：
关于odds与odds-1源码实现如下：
// 通过概率计算Odd值 论文里的 odds(p)函数 inline float Odds(float probability) { return probability / (1.f - probability); } // 通过Odd值计算概率值 论文里的 odds^-1 函数 inline float ProbabilityFromOdds(const float odds) { return odds / (odds + 1.f); }
现在再回过头来看公式(01)就比较好理解了，因为odds越大，表示cell被占用的概率越大。如果cell 被占用的概率与未被占用的概率都为0.5相等时，odds为1。如果cell未被占用的机率很大很大,那么 odds是趋向于0的一个数。也就是说,原始 cell 对应的 odds(Mold(x))乘以 hit_probability=0.55 对应的odds(Phit)>1,那么其结果Mnew(x)肯定是增加的，从而达到更新的效果。
总结 总的来说, 使用hit_probability=0.55>0.5 对cell 进行更新,则cell 对应被占用的概率越来越大。如果使用 miss_probability=options.miss_probability()=0.49<0.5对cell进行更新，则cell对应被占用的概率越来越小。
这里的确了解了cell更新的原理，但是似乎并没有解答疑问1与疑问3。虽然论文中的公式是这样的， 但是源码并非是这样的实现的。下面就从源码的角度来解答疑问1与疑问3吧。
3. ComputeLookupTableToApplyCorrespondenceCostodds()
从上面的分析中，知道 ProbabilityGridRangeDatalnserter2D的核心是调用了 ComputeLookupTableToApplyCorrespondenceCostodds()函数。
根据上面公式虽然可以对cell的栅格值进行更新，但是每次计算都涉及到了重复的乘除法计算。所以，Cartographer 源码实现过程中，又使用到了用空间换效率的方式。
目的不是要对cell进行更新吗?那么是不是可以提前计算出cell 更新的所有可能值呢?答案是可以的，因为cell就是[0,32768]中的一个数值，那么把[0,32768]更新后的所有值都保存下来即可。
/** * @brief 将点云写入栅格地图 * * @param[in] range_data 要写入地图的点云 * @param[in] grid 栅格地图 */ void ProbabilityGridRangeDataInserter2D::Insert( const sensor::RangeData& range_data, GridInterface* const grid) const { ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid); CHECK(probability_grid != nullptr); // By not finishing the update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). // param: insert_free_space CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), probability_grid); probability_grid->FinishUpdate(); }
源码中就是这样做的，具体实现方式如下：
(01)首先创建一个std:vector<uint16>result变量，用于保存结果，这里的结果就是转换表。也就是 cell 更新之后所有可能的取值。
(02)result存储第一个元素比较特殊，其存储的是形参odds对应的栅格值，这里可以理解为把形参 odds 以uint16的方式进行存储。不过注意，在存储之时，该值还加上了kUpdateMarker=32768。
(03)计算cell更新时从1到32768的所有可能更新后的结果, 然后存储于result 之中,源码中的 Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))等价于公式(1)的 odds(Mold (x))，源码中的形参odds等价于公式(1)中的odds(phit)。
源代码的注释如下：
// 将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来 std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds( float odds) { std::vector<uint16> result; result.reserve(kValueCount); // 32768 // 当前cell是unknown情况下直接把odds转成value存进来 result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost( ProbabilityFromOdds(odds))) + kUpdateMarker); // 加上kUpdateMarker作为一个标志, 代表这个栅格已经被更新了 // 计算更新时 从1到32768的所有可能的 更新后的结果 for (int cell = 1; cell != kValueCount; ++cell) { result.push_back( CorrespondenceCostToValue( ProbabilityToCorrespondenceCost(ProbabilityFromOdds( odds * Odds(CorrespondenceCostToProbability( (*kValueToCorrespondenceCost)[cell]))))) + kUpdateMarker); } return result; }
根据上面分析，可以知道，根据返回的更新查询表result,可以对cell进行更新，这里更新的时候 +kUpdateMarker,所以后面后续再调用Grid2D::FinishUpdate()函数中，cell 栅格需要减去 kUpdateMarker，以复原。这样就对疑问1进行了解答。对于疑问3的解答，先不要着急，我们先来来看看ProbabilityGridRangeDatalnserter2D::Insert()函数。
4. ProbabilityGridRangeDatalnserter2D::Insert()
(01)利用c++多态机制，调用static_cast函数把参数Gridlnterface* const grid 转换成 ProbabilityGrid类型。
(02)调用CastRays()函数,该函数实现于probability_grid_range_data_inserter_2d.cc 文件中。该函数是把点云数据插入到子图(或者说grid)的核心函数，后续回单独对其进行详细的讲解。
(03)调用Grid2D::FinishUpdate()函数，根据前面的分析我们知道每个更新过的cell都需要减去 kUpdateMarker 以复原。
/** * @brief 将点云写入栅格地图 * * @param[in] range_data 要写入地图的点云 * @param[in] grid 栅格地图 */ void ProbabilityGridRangeDataInserter2D::Insert( const sensor::RangeData& range_data, GridInterface* const grid) const { ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid); CHECK(probability_grid != nullptr); // By not finishing the update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). // param: insert_free_space CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), probability_grid); probability_grid->FinishUpdate(); }
5. probability_grid->ApplyLookupTable
上面通过对 ComputeLookupTableToApplyCorrespondenceCostOdds()的解读，解答了疑问1,现在还剩下疑问3→Probability Grid:ApplyLookupTable()函数中对于cell的更新比较奇怪,代码为: *cell = table[*cell]。
该函数被 src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc文件中的CastRays(）函数调用，且被调用了三次。这里先不对CastRays()函数进行具体分析，下一篇 博客再进行详细的讲解。其三次调用probability_grid->ApplyLookupTable（)的代码如下所示：
// 更新hit点的栅格值 probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); // 从起点到end点之前, 更新miss点的栅格值 probability_grid->ApplyLookupTable(cell_index, miss_table); // 从起点到misses点之前, 更新miss点的栅格值 probability_grid->ApplyLookupTable(cell_index, miss_table);
从这里可以看出，其主要是根据hit_table或者 miss_table 这两个更新查询表对cell 进行更新。这里需要注意的是,使用hit_table 更新,其cell 对应被占用的概率越大。使用miss_table更新,其 cell 对应被占用的概率越小， 即未被占用的概率越大。其原因是因为构建 hit_ table_ 与miss_table 的参数分别是options.hit_probability（)与options.miss_robability(),前者大于0.5,后者小于0.5。 那么问题3的答案就出现了，根据前面的分析：hit_table_与 miss_table_的索引就是代表未更新之前的cell,索引对应位置存储的就是更新后的cell,所以通过执行代码*cell=table[*cell]。即可完成对 cell的更新。
九、RayToPixelMask()与贝汉明(Bresenham)算法
1. 前言
在上一节中，解答了如下两个疑问：
疑问1：→为什么Gird::FinishUpdate()函数中，栅格值为什么需要减去KUpdateMarker？
疑问3：→ProbabilityGrid::ApplyLookupTable()函数中对于cell的更新比较奇怪，代码为*cell = table[*cell]。
但是在分析ProbabilityGridRangeDatalnserter2D::Insert()函数的时候，其调用了一个十分重要的函数CastRays()，对于该函数并没有进行具体的分析，接下来就是对该函数进行讲解了。不过在这之前，需要了解一下贝汉明(Bresenham)算法。
需要提前理解的是，把点云数据插入到栅格地图，其本质上就是对栅格地图的更新。更新步骤为：根据最新采集到的雷达点，在与地图匹配后，把雷达点插入到当前地图中，其本质是对栅格地图概率的更新。
这里的更新包括两部分，其一是对hit点的概率更新，其二是对CastRay完成激光射线状地图更新， 即对一条直线所经过的栅格进行概率值更新。
对于上述过程应该还是很好理解的，hit表示激光Q发射光子打中的点，说明其是障碍物，那么源码中使用 hit_table_表格对cell 进行更新。从雷达传感器原点到hit点连接的射线其经过的区域记为 miss,对应的cell栅格表示其没有障碍物，所以需要使用miss_table_表格对cell进行更新。下图是雷达扫描的模型仿真：
图中红色部分表示之前探索过的区域，灰色部分表示目前正在探索的区域。可以看到一条条由机器人（雷达原点)发射而出的一条条射线。这些射线经过的区域为miss,其终点位置为hit。对于一种特殊情况，那就是大范围区域没有障碍物，但是雷达只能探索到有限距离，该情况后续我们再进行具体分析。
那么显然, 对于hit 点的更新十分简单,直接调用 ProbabilityGrid.ApplyLookupTable() 函数更新即可。但是对于hit 射线经过经过区域miss 所对应的cell 都要进行更新，显然会复杂一些。
对与miss 区域cell的更新，主要解决的问题，就是一条由雷达原点发射出来的点云射线，其会经过哪些cell或者说栅格，如下图所示：
若机器上处于上图绿色圆圈Robot栅格位置，打出一个光子，即一个点云数据，为上图红色圆圈hit栅格位置，那么要求的，就是这两个栅格中心连接的直线，所经过的栅格区域，也就是上图的miss。
2. 贝汉明(Bresenham)算法→基本说明
这里说的贝汉明(Bresenham)算法，并非原版的贝汉明(Bresenham)算法，而是Cartographer中改版过后的贝汉明(Bresenham)算法。先来看下图：
图3
1、灰浅色小方格(pixel坐标系) → 每个小方格代表一个像素、
2、灰深色大方格(subpixel坐标系) → 每个大方格边长为5个像素
3、蓝色方格 → 起始端点(pixel坐标系)
4、红色方格 → 结束端点(pixel坐标系)
5、黄色圆点 → 交点
6、subpixel_scale → 每个大方格的边长，上图中等于5
7、dx → 两端点x距离，像素为单位(pixel坐标系)
8、dy → 两端点y距离，像素为单位(pixel坐标系)
本来根据贝汉明(Bresenham)算法的原理，我们需要求解出下图中的紫色像素方格的坐标：
图4
但是这样存在一个问题，那就是按一个像素进行填充，如果地图的分辨率较高的话，那么用肉眼可能根本没有办法看见了，所以Cartographer中利用subpixel_scale参数构建一个新的坐标系，这里我们称为subpixel坐标系，记原来的像素坐标系为pixel坐标系。
根据图3、可以理解为对subpixel坐标系进行细分，就成了pixel坐标系。Cartographer求得射线经过的区域是基于subpixel坐标系的，其结果如下图所示（同样为紫色区域）：
图5
核心思路 假设上图中蓝色大方格为0，下一个方格为1（紫色），根据上图可以直接看出，其位于方格0的右边，那么问题来了，这是如何计算的呢？其核心就是sub_yx(x=1,2,3,4...)与subpixel_scale 进行比较，比较的结果有三种情况：
1、subpixel_scale > sub_yx : 选择当前方格右边的方格 2、subpixel_scale = sub_yx : 选择当前方格右上边(对角)的方格 3、subpixel_scale < sub_yx : 选择当前方格上边的方格
对于情况3会特殊一点，为什么特殊，根据上图我们带入一下就知道了：
(1)首先蓝色大方格0对应的subpixel_scale >sub_y1,所以应该选择当前方格右边的紫色方格1;
(2)又因为subpixel_scale<sub_y2，所以选择紫色方格1上边的紫色方格2；该为情况3,所以需要再一次判断sub_y2'，因为subpixel_scale>sub_y2'，这次为情况一，选择右边的紫色方格3（如果这次还为情况3，则需要再次判断sub_y2”）
(3)现在已经选择了紫色方格3，又因为subpixel_scale=sub_yx,所以选择右上边（对角）的紫色方格 4。
(4)在选择紫色方格5与紫色方格6的时候又是特殊情况，所以subpixel_scale 需要与sub_y5与 sub_y5'进行比较。
总结 如果出现情况3,则可能会存在sub_y2'、sub_y2”、sub_y2'''，并且sub_y2-sub_y2'=sub_y2'- sub_y2”=sub_y2”-sub_y'''=subpixel_scale。其实很好理解，当始终点线段斜率较大的时候，就会经常出现上述情况。
这里列举一个典型的例子，始终点线段斜率无限大（但不垂直x轴），此时，紫色方格的x坐标是相同的，只需要把y坐标一个加上去即可，每增加subpixel坐标系y轴的一个单位，等价于增加 subpixel_scale个像素。
3. 源码实现→x轴排序
根据前面的分析，已经知道了 Cartographer 实现贝汉明（Bresenham)算法的大致流程，但是源码中又是如何实现的呢？虽然我们对图5进行了详细的讲解，但是那仅仅是实现的一种方式而已，源码中虽然原理上差不过，不过还是存在一些差异的。
在CastRays()中调用了一个RayToPixelMask()函数,该函数就是贝汉明(Bresenham)算法的核心实现，其需要传递三个参数，分别为两个端点像素坐标，以及缩放尺度subpixel_scale，其源码中设置为 subpixel_scale=kSubpixelScale=1000。 该函数实现于src/cartographer/cartographer/mapping/internal/2d/ray__to_pixel_mask.cc文件中，首先执行如下代码：
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x // coordinate. // 保持起始点的x小于终止点的x,需要注意，这里这样没有对y处理， // 也就是scaled_begin.y() 与 scaled_end.y() 谁大谁小还是不确定的 if (scaled_begin.x() > scaled_end.x()) { return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale); } CHECK_GE(scaled_begin.x(), 0); CHECK_GE(scaled_begin.y(), 0); CHECK_GE(scaled_end.y(), 0); std::vector<Eigen::Array2i> pixel_mask;
所以对于RayToPixelMask 连个端点无先后顺序，因为其内部会根据x轴进行排序。小的为 scaled_begin,大的为scaled_end。
还创建了一个pixel_mask变量,用于存储像素坐标系，不过注意，其是基于subpixel系的坐标。等价于图5中紫色方格的坐标。如紫色方格1的坐标为（2,1)。注意并不是(10,5)，因为是基于subpixel 坐标系的。紫色方格2的坐标为（2,2），其余的依此类推。
4. 源码实现→垂直线段
首先其对垂直线段[scaled_begin,scaled_end]进行了处理，该情况的实现还是比较简单的，x坐标不变，把y一个一个增进去即可，不过依然要除以，存储pixel_mask中返回的坐标是基于subpixel 坐标系的，代码注释如下：
// Special case: We have to draw a vertical line in full pixels, as // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate. // 起点与终点x相等,说明该射线是垂直的，即斜率不存在的情况 if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) { // 把起始点subpixel系下的坐标赋值给current，这里需要注意的是, // 把y值较小的点任务是起始点 Eigen::Array2i current( scaled_begin.x() / subpixel_scale, std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale); //把起始点subpixel系下的坐标current添加至pixel_mask之中 pixel_mask.push_back(current); // y值大的点为终止点，计算其subpixel系下的y坐标 const int end_y = std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale; // 因为 current 存储的是subpixel系下坐标，所以++操作，是subpixel系下增加了一个单位 // 当然，也可以认为是 pixel 系下增加了subpixel_scale 个单位 for (; current.y() <= end_y; ++current.y()) { //如果当前的subpixel系下坐标与上一次添加到pixel_mask的坐标相同，则不会重复添加。 if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); } //需要注意，其存储的是subpixel系下的坐标。 return pixel_mask; }
5. 源码实现→第一个像素
5.1 预备工作
先来看代码解释，如下，后续再结合图像进行讲解：
// 下边就是 breshman 的具体实现 //为了后续计算方便，避免考虑绝对像素坐标，直接以偏移值的方式进行计算 //终止点相对于起始点x轴偏移的pixel数(基于pixel坐标系),因为前面做了排序，所以其>0 const int64 dx = scaled_end.x() - scaled_begin.x(); //终止点相对于起始点y轴偏移的pixel数(基于pixel坐标系), //不过未做排序，其可能>0，也可能<0，需要分情况讨论 const int64 dy = scaled_end.y() - scaled_begin.y(); // 提前计算好的一个数值，其表示在subpixel系下y轴每增加一个单位， // 其对应的一维像素坐标系下，y需要偏移多少个单位(像素),这里*2， // 是为了方便中心点的表示，详细解释在下面 const int64 denominator = 2 * subpixel_scale * dx; // The current full pixel coordinates. We scaled_begin at 'scaled_begin'. // 计算起始点subpixel系下坐标，且添加至pixel_mask之中 Eigen::Array2i current = scaled_begin / subpixel_scale; pixel_mask.push_back(current); // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in // the denominator. // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale) // | | | | // +-+-+-+ // | | | | // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale) // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale) // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale) /*上面的类容翻译过来，就是说为了表示 subpixel(一个subpixel含1000个像素) 的中心, 所以这里乘以2，因为这样：如果使用 (2 * subpixel_scale) 表示一个subpixel的边长，或者说顶点 那么 1 * subpixel_scale 就表示中心，那么 0=0*subpixel_scale 就表示最小值(点) */ // The center of the subpixel part of 'scaled_begin.y()' assuming the // 'denominator', i.e., sub_y / denominator is in (0, 1). // sub_y表示一个点在pixel系下的坐标，相对于该点在subpixel系下坐标的偏移 // 该偏移以像素维单位，这里的*2与前面保持一致，为方便表示， // 另外 sub_y/denominator 是处于(0,1)之间的，可以理解为y坐标的偏移率。 int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx; // The distance from the from 'scaled_begin' to the right pixel border, to be // divided by 2 * 'subpixel_scale'. // 这里要以subpixel的一个方格为参考来理解，其求得的是起始点在pixel系下的坐标， // 相对于该点在subpixel系下x坐标的偏移的2倍，以像素为单位 const int first_pixel = 2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1; // The same from the left pixel border to 'scaled_end'. // 其求得的是终止点在pixel系下的坐标，相对于该点在subpixel系下x坐标的偏移，以像素为单位 const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1; // The full pixel x coordinate of 'scaled_end'. //计算出结束端subpixel系下的x坐标 const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
上述代码主要出现的变量为：
dx、 dy、 denominator, current, sub_y、 first_pixel, last_pixel, end_x
对于dx与dy 直接其与图5中的dx,dy(黑色加粗字体)一致，这里的denominator 不好理解可以直接除以一个2*dx,则变成了 subpixel_scale，后续紫色方格的选择，源码中是与denominator进行对比，其本质上就是与subpixel_scale 进行对比。current较为简单，与前面的一致。
变量sub_y
对于上述代码中的sub_y 理解起来或许还是比较困难的，如下所示：
int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
同上面分析 denominator一致， 同样除以一个2*dx,那么剩下的就是代码就是(scaled_begin.y()% subpixel_scale） + 1, 这个还是比较好理解的，如下图所示：
上图中记sub_yf=scaled_begin.y()% subpixel_scale.源码中的+1比较令人疑惑，暂时先记一下。那么也就是说
变量first_pixel
const int first_pixel =2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
首先来看看其上的scaled_begin.x()% subpixel_scale，记为sub_xf，对应到图上如下所示：
那么 first_pixel可以记为：
变量end_x
const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
如下图end表示终点在subpixel系得坐标，end_x表示该点的x坐标，如果以下图为例，其坐标为(6，4).
5.2 整理思路
上面的分析，主要得到如下几个结论：
其中sub_xf与sub_yf如下图所示：
接着源码中还存在如下代码：
sub_y += dy * first_pixel;
表示其对初始的sub_y进行了更新，这里结合(1)(2)对其进行展开：
为了方便理解，等式两边都除以2*dx，结果如下：
其中dy/dx表示线段的斜率，方便理解作图如下：
根据上图，就很好理解了，sub_y/ 2*dx 表示的， 就是图5中的sub_y1，不过显然，上图中起点到终点的连线与蓝色方格的交点1(上图黄色圆圈)画错了位置，所以我们需要再调整一下，下图应该才是与源码的实现全贴合：
图6
其与我们前面绘画的主要有2个不同点，首先是起点与终点的连线需要向上平移一个单位，然后需要再增加一个紫色的方格3'。这样就好了。
6. 源码实现→分情况讨论
整理了思路之后，且绘画出了上图6，了解到sub_y/ 2*dx 就是我们要求解的sub_y1，如下图所示：
接下来就是需要分析源码中是如何求解sub_y2、sub_y2'、sub_y3、sub_y3'....。在前面已经提到过。因为源码中对scaled_begin.x()与scaled_end.x()进行了排序,所以dx一直都是大于的。但是存在dy>0或者dy<0两种情形。上面列举的例子都是dy>0的情况。
6.1 dy > 0
对于dy > 0的情形，根据前面的简介，其又可以分为如下三种情况(x为上图的1,2,2',...):
1、sub_yx < subpixel_scale : 选择当前方格右边的方格 2、sub_yx = sub_yxsubpixel_scale : 选择当前方格右上边(对角)的方格 3、sub_yx > subpixel_scale : 选择当前方格上边的方格
并且提到情况3是一种特殊的情况（会进入到一种循环状态），需要注意的是，源码中sub_y与 denominator的比较等价与上述 sub_yx与subpixel_scale的比较。现在来理解源码就比较简单了。 代码注释如下：
// dy > 0 时的情况subpixel系下的坐标至多被添加一次 if (dy > 0) { while (true) { //只有current与上一次存储在pixel_mask中坐标不一样时，才进行存储。 //这样可以保证，每个坐标至多被存储一次 if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); // 情况3: 本质上就是 sub_yx > subpixel_scale，且是一个循环 // 可以 把 sub_y 与 denominator 都除以一个2*dx进行理解 while (sub_y > denominator) {//sub_y<denominator时结束循环 sub_y -= denominator; //等价价于 sub_yx移动至sub_y(x+1) ++current.y(); //选择当前方格上边的方格 //保证每个坐标至多添加一次 if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current); } ++current.x();//x轴移动到下一个方格(基于subpixel系) //情况2: 本质上就是 sub_yx==subpixel_scale， if (sub_y == denominator) { //选择当前方格右上边(对角)的方格 sub_y -= denominator; ++current.y(); } //遍历到最后一个方格跳出循环 if (current.x() == end_x) { break; } // 如果情况2与情况3都没有处理，则该处表示情况1 // Move from one pixel border to the next. sub_y += dy * 2 * subpixel_scale; }
相信大家结合图示再进行理解，就十分简单了。
6.2 dy > 0
该情形与 dy > 0的情况基本一致，所以这里就不再重复讲解了。
7. GrowAsNeeded()
在讲解 CastRays() 函数之前，先来看看同样位于 probability_grid_range_data_inserter_2d.cc 文件中的 GrowAsNeeded() 函数。
// 根据点云的bounding box, 看是否需要对地图进行扩张 void GrowAsNeeded(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) { // 找到点云的bounding_box，获得目前探索过的区域 Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); // Padding around bounding box to avoid numerical issues at cell boundaries. //对区域进行填充，保证雷达点云数据的点都位于该区域类 constexpr float kPadding = 1e-6f; for (const sensor::RangefinderPoint& hit : range_data.returns) { bounding_box.extend(hit.position.head<2>()); } for (const sensor::RangefinderPoint& miss : range_data.misses) { bounding_box.extend(miss.position.head<2>()); } // 对地图进行扩充，新扩充的点云数据对应的cekk坐标被包含在扩充之后的地图之中 probability_grid->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones()); probability_grid->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones()); }
8. CastRays()
/** * @brief 根据雷达点对栅格地图进行更新 * * @param[in] range_data * @param[in] hit_table 更新占用栅格时的查找表 * @param[in] miss_table 更新空闲栅格时的查找表 * @param[in] insert_free_space * @param[in] probability_grid 栅格地图 */ void CastRays(const sensor::RangeData& range_data, const std::vector<uint16>& hit_table, const std::vector<uint16>& miss_table, const bool insert_free_space, ProbabilityGrid* probability_grid) { // 根据雷达数据调整地图范围，保证点云都在地图之内 GrowAsNeeded(range_data, probability_grid); //获得地图的相关信息，如最大最小范围，栅格数，分辨率等 const MapLimits& limits = probability_grid->limits(); //获得subpixel系的分辨率 const double superscaled_resolution = limits.resolution() / kSubpixelScale; //新建subpixel系的地图限制 const MapLimits superscaled_limits( superscaled_resolution, limits.max(), CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, limits.cell_limits().num_y_cells * kSubpixelScale)); // 雷达原点在地图中的像素坐标, 作为画线的起始坐标 const Eigen::Array2i begin = superscaled_limits.GetCellIndex(range_data.origin.head<2>()); // Compute and add the end points. //把每个点云数据对应的像素坐标作为结束点，这里只对hit点进行了栅格更新。 std::vector<Eigen::Array2i> ends; ends.reserve(range_data.returns.size()); for (const sensor::RangefinderPoint& hit : range_data.returns) { // 计算hit点在地图中的像素坐标, 作为画线的终止点坐标 ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>())); // 更新hit点的栅格值 probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); } // 如果不插入free空间就可以结束了，也就是不对miss区域进行栅格更新 if (!insert_free_space) { return; } // Now add the misses，把前面hit线段经过的栅格进行更新。 for (const Eigen::Array2i& end : ends) { std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, kSubpixelScale); for (const Eigen::Array2i& cell_index : ray) { // 从起点到end点之前, 更新miss点的栅格值 probability_grid->ApplyLookupTable(cell_index, miss_table); } } // Finally, compute and add empty rays based on misses in the range data. // range_data 中包含了两种点云数据：RangeData::PointCloud.returns RangeData::PointCloud.misses // RangeData::PointCloud.returns 就是hit点， // RangeData::PointCloud.misses 前面是表示雷达没有打中障碍物的点，或者说超出了测量测量范围的数据。然后使用一个值进行代替的数据 for (const sensor::RangefinderPoint& missing_echo : range_data.misses) { //求得线段经过的栅格 std::vector<Eigen::Array2i> ray = RayToPixelMask( begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()), kSubpixelScale); for (const Eigen::Array2i& cell_index : ray) { // 从起点到misses点之前, 更新miss点的栅格值 probability_grid->ApplyLookupTable(cell_index, miss_table); } } }
该函数也比较简单，需要注意一点的是 const sensor::RangeData& range_data 中的 PointCloud misses 数据，该些点云都是没有打中障碍物或者说超出了测量测量范围的数据。
十、总体流程分析
下面来梳理一下2D栅格地图构建过程，以及地图的发布与使用。
1. 构建过程
对于构建过程，首先来看一下关于栅格相关类的构建过程过程：
(01) LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D() 构造函数，该构造函数会对于成员变量 ActiveSubmaps2D active_submaps_; 进行初始化。
(02) ActiveSubmaps2D::ActiveSubmaps2D 构造函数，该构造函数会调用成员函数 ActiveSubmaps2D::CreateRangeDataInserter() 对成员变量 std::unique_ptr<RangeDataInserterInterface> range_data_inserter_进行初始化，这里利用了C++多态机制，range_data_inserter_ 实际上是一个 ProbabilityGridRangeDataInserter2D 类型的智能指针。
(03) ActiveSubmaps2D::ActiveSubmaps2D 除了在构造函数中创建 ProbabilityGridRangeDataInserter2D 实例对象，且后续调用 ActiveSubmaps2D::AddSubmap() 函数时，还会构建 Submap2D 对象，添加到其成员变量 submaps_ 之中。
(04) ActiveSubmaps2D::AddSubmap() 函数中构建 Submap2D 对象时，需要给 Submap2D 构造函数传递两个类对象，分别为 MapLimits 与 CellLimits。
总结 以上就是关于2D栅格地图核心类的构建过程。
2. 调用过程
LocalTrajectoryBuilder2D::AddRangeData() //处理点云数据 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() //累加点云数据 LocalTrajectoryBuilder2D::InsertIntoSubmap() //把点云数据插入子图 ActiveSubmaps2D::InsertRangeData()//把输插入到活跃的子图 Submap2D::InsertRangeData() ProbabilityGridRangeDataInserter2D::Insert() //输出插入到栅格地图 //src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc CastRays()//对栅格地图进行更新 //src/cartographer/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc RayToPixelMask() //核型算法（贝汉明→Bresenham）
3. 地图保存
目前已经知道了2D栅格地图相关类的构建过程，以及如何点云数据插入且更新栅格地图相关函数的调用过程，
下面来看看地图是如何被保存与使用的。回到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中，存在如下代码：
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(){ // 将校正后的雷达数据写入submap std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(time, range_data_in_local, filtered_gravity_aligned_point_cloud,pose_estimate, gravity_alignment.rotation()); ...... // 返回结果 return absl::make_unique<MatchingResult>( MatchingResult{time, pose_estimate, std::move(range_data_in_local), std::move(insertion_result)}); }
可知插入地图的结果：
// 将点云插入到地图后的result struct InsertionResult { std::shared_ptr<const TrajectoryNode::Data> constant_data; std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 最多只有2个子图的指针 };
被包含在结构体 MatchingResult 中返回了。该返回的结果在 src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc 文件的 重载函数 AddSensorData() 中被接收，如下所示：
void AddSensorData( const std::string& sensor_id, //订阅的话题 const sensor::TimedPointCloudData& timed_point_cloud_data) override { ...... // 通过前端进行扫描匹配, 然后返回匹配后的结果 std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result = local_trajectory_builder_->AddRangeData( sensor_id, timed_point_cloud_data); ...... // 这里的InsertionResult的类型是 TrajectoryBuilderInterface::InsertionResult insertion_result = absl::make_unique<InsertionResult>(InsertionResult{node_id, matching_result->insertion_result->constant_data, std::vector<std::shared_ptr<const Submap>>( matching_result->insertion_result->insertion_submaps.begin(), matching_result->insertion_result->insertion_submaps.end())}); // 将结果数据传入回调函数中, 进行保存 if (local_slam_result_callback_) { local_slam_result_callback_( trajectory_id_, matching_result->time, matching_result->local_pose, std::move(matching_result->range_data_in_local), std::move(insertion_result)); } }
其上的变量 insertion_result 就是插入点云输入插入地图之后返回的结果。该变量会被回调函数 GlobalTrajectoryBuilder::LocalSlamResultCallback local_slam_result_callback_ 调用。
该回调函数实际上就是 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 中的一个 lambda表达式 如下：
[this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr< const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { // 保存local slam 的结果数据 5个参数实际只用了4个 OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); }
从而可知道其最终调用到了 MapBuilderBridge::OnLocalSlamResult() 函数。该函数会把这些数据存储在成员变量 local_slam_data_ 中，如下所示：
std::unordered_map<int,std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);
如果想获取该数据，需要调用 MapBuilderBridge::GetLocalTrajectoryData() 函数。该函数在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 文件中的 void Node::PublishLocalTrajectoryData() 函数中被调用：
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) { ...... }
也就是说，到此为止就完成了子图话题的发布。
4. 结语
通过该篇博客的总结，详细理解了 Cartographer 中2D栅格地图，后续就是讲解点云扫描匹配了，不用多说，其肯定是 Cartographer 最核心的部分。回到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中，可见代码如下：
// local map frame <- gravity-aligned frame // 扫描匹配, 进行点云与submap的匹配 std::unique_ptr<transform::Rigid2d> pose_estimate_2d = ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
参考大佬:Cartographer源码解析——2D栅格地图构建_cartgrapher解析-CSDN博客
