前言：
到目前为止，对于点云数据的预处理过程已经介绍完毕，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。
在讲解扫描匹配之前，先来看看 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()) {}构造函数对其成员变量options_, range_data_inserter_进行赋值，其中对ange_data_inserter_赋值的时侯会调用CreateRangeDataInserter()这个函数。
3.6 submaps()
// 返回指向 Submap2D 的 shared_ptr指针 的vector
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::submaps() const {
return std::vector<std::shared_ptr<const Submap2D>>(submaps_.begin(),
submaps_.end());
}很简单，就是将指向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 SubmapSubmap 在 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]更新后的所有值都保存下来即可。源码中就是这样做的，具体实现方式如下：
(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进行更新。下图是雷达扫描的模型仿真：
图1
图1 红色部分表示之前探索过的区域，灰色部分表示目前正在探索的区域。可以看到一条条由机器人（雷达原点)发射而出的一条条射线。这些射线经过的区域为miss,其终点位置为hit。对于一种特殊情况，那就是大范围区域没有障碍物，但是雷达只能探索到有限距离，该情况后续我们再进行具体分析。
那么显然, 对于hit 点的更新十分简单,直接调用 ProbabilityGrid.ApplyLookupTable() 函数更新即可。但是对于hit 射线经过经过区域miss 所对应的cell 都要进行更新，显然会复杂一些。
对与miss 区域cell的更新，主要解决的问题，就是一条由雷达原点发射出来的点云射线，其会经过哪些cell或者说栅格，如下图所示： 
图2
若机器上处于上图绿色圆圈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);
1046
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
