cartographer 生成子图
红色代表接下來准备解释的函数。。
1、生成子图调用函数
在local_trajectory_builder_2d.cc InsertIntoSubmap函数 中
std::vector> insertion_submaps =
active_submaps_.InsertRangeData(range_data_in_local); submap_2d.cc Submap2D类 ActiveSubmaps2D类 中
2、ActiveSubmaps2D类
说明：
在插入第一帧激光数据时创建第一个活跃子图，除非在此初始化期间没有或只有一个子图存在，总是有两个子目录插入范围数据：a.用于匹配的旧子图，以及将用于下次匹配的新子图，他正在初始化。一旦一定数量的激光数据被插入后，新的子图被考虑初始化：旧的子图被改变，即新子图变为现在的旧子图，用于扫描匹配。而且一个新的子图被创建，忘记旧的子图。
两个submap之间有50%的重合，所以匹配的时候都用这个完成度大于50%的地图来进行匹配
构造函数：
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
: options_(options), range_data_inserter_(CreateRangeDataInserter()) {}1.其中，CreateRangeDataInserter 函数，用于创建了 概率网格范围数据插入器2D
switch (options_.range_data_inserter_options().range_data_inserter_type()) {
case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
return absl::make_unique(
options_.range_data_inserter_options()
.probability_grid_range_data_inserter_options_2d());
case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
return absl::make_unique(
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d());
default:
LOG(FATAL) << "Unknown RangeDataInserterType.";
} 2.ProbabilityGridRangeDataInserter2D类创建
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
const proto::ProbabilityGridRangeDataInserterOptions2D& options)
: options_(options),
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.hit_probability()))),
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.miss_probability()))) {}成员变量：
const proto::SubmapsOptions2D options_;
std::vector> submaps_;
std::unique_ptr range_data_inserter_;
ValueConversionTables conversion_tables_; 成员函数：
1、InsertRangeData函数
std::vector> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) 参数：激光数据
代码详解：1.submaps_ vector 保留了两个子图，一个用于匹配，一个用于构建。当submaps_ vector 为空或者是submaps_ vector 的最后submap插入的激光数据达到 设定的值options_.num_range_data 时 添加子图AddSubmap
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}2.给submaps_ vector 中的submap 添加激光数据 ，函数InsertRangeData
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_.get());
}3.当submaps_ vector 的第一个submap 插入激光数据为 2*options_.num_range_data 时，子图完成Finish
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish();
}2、AddSubmap
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) 参数：激光的 origin 数据
代码详解：
1.检查submaps_ vector 子图数量大于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());
}2.构建submap2d该类，由 origin，创建的Grid2D*，&conversion_tables_，创建的Grid2D* 使用 CreateGrid(origin).release())
submaps_.push_back(absl::make_unique(
origin,
std::unique_ptr(
static_cast(CreateGrid(origin).release())),
&conversion_tables_)); 3、CreateGrid函数
std::unique_ptr ActiveSubmaps2D::CreateGrid(
const Eigen::Vector2f& origin) 参数：激光的 origin （原点）
代码详解：
1.初始化栅格地图的大小100 ，获取分辨率的值
constexpr int kInitialSubmapSize = 100;
float resolution = options_.grid_options_2d().resolution();2.选择创建栅格还是三维重建
switch (options_.grid_options_2d().grid_type()) {
case proto::GridOptions2D::PROBABILITY_GRID:
return absl::make_unique(
MapLimits(resolution,
origin.cast() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
&conversion_tables_);
case proto::GridOptions2D::TSDF:
return absl::make_unique(
MapLimits(resolution,
origin.cast() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d()
.truncation_distance(),
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d()
.maximum_weight(),
&conversion_tables_);
default:
LOG(FATAL) << "Unknown GridType.";
} 3.二维栅格地图的构建 Eigen::Vector2d::Ones() 二维列向量[1,1] ProbabilityGrid构造
absl::make_unique(
MapLimits(resolution,
origin.cast() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
&conversion_tables_); 4. InsertRangeData Finish 在类Submap2D中
3、Submap2D类
构造函数：
Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid,
ValueConversionTables* conversion_tables);
explicit Submap2D(const proto::Submap2D& proto,
ValueConversionTables* conversion_tables); 1.由ActiveSubmaps2D类构造
Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid,
ValueConversionTables* conversion_tables)
: Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
conversion_tables_(conversion_tables) {
grid_ = std::move(grid);
}
2.由proto构造
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(proto.grid(), conversion_tables_);
} else if (proto.grid().has_tsdf_2d()) {
grid_ = absl::make_unique(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());
} 成员变量：
std::unique_ptr grid_;
ValueConversionTables* conversion_tables_; 成员函数：
1、Finish函数
void Submap2D::Finish() 1.检查Grid不为空，且完成标志为未完成
CHECK(grid_);
CHECK(!insertion_finished());2.计算裁剪栅格地图 ComputeCroppedGrid
grid_ = grid_->ComputeCroppedGrid();
3.将插入子图完成设置成true set_insertion_finished(true);
2、InsertRangeData
参数：参1：激光数据 ， 参2：概率网格范围数据插入器2D
代码详解：
1.检查Grid不为空，且完成标志为未完成
CHECK(grid_);
CHECK(!insertion_finished());2.用 概率网格范围数据插入器2D进行插入激光数据 Insert
range_data_inserter->Insert(range_data, grid_.get());3.插入激光数据+1
set_num_range_data(num_range_data() + 1);4、ProbabilityGrid类 class ProbabilityGrid : public Grid2D
explicit ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables);
explicit ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);表示概率的2D网格。
构造函数：
1.ActiveSubmaps2D类构造
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables)
: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
conversion_tables),
conversion_tables_(conversion_tables) {}2.proto构造
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_probability_grid_2d());
}3.变量
constexpr float kMinProbability = 0.1f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;成员变量：
ValueConversionTables* conversion_tables_;成员函数：
1.ComputeCroppedGrid函数
std::unique_ptr ProbabilityGrid::ComputeCroppedGrid() const 代码详情：
1.计算裁剪的限制 ComputeCroppedLimits
Eigen::Array2i offset;
CellLimits cell_limits;
ComputeCroppedLimits(&offset, &cell_limits);ComputeCroppedGrid 填写'offset'和'limits'来定义包含all的子区域已知栅格 offset 地图坐标系最小值
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);
}2.重新计算最大值坐标，max-裁剪的部分
const Eigen::Vector2d max =
limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());3.重新定义概率栅格地图，并且给栅格地图赋值
std::unique_ptr cropped_grid =
absl::make_unique(
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));
} 2、SetProbability函数
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
const float probability)将'cell_index'处的单元格的概率设置为给定的'概率'。 只有在栅格未知之前才允许。
代码详情：
1.获取 cell_index 的值 mutable_correspondence_cost_cells是vector*
uint16& cell =
(*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];ToFlatIndex作用是计算cell_index在一位数组的下标
int Grid2D::ToFlatIndex(const Eigen::Array2i& cell_index) const {
CHECK(limits_.Contains(cell_index)) << cell_index;
return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
}2.检测该栅格是未知
CHECK_EQ(cell, kUnknownProbabilityValue);3.将概率值转化为 1-32767 并设置
cell =CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));ProbabilityToCorrespondenceCost 功能
inline float ProbabilityToCorrespondenceCost(const float probability) {
return 1.f - probability;}CorrespondenceCostToValue 将correspondence_cost转换为[1,32767]范围内的uint16。
return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost,
kMaxCorrespondenceCost);kMinCorrespondenceCost 0.1f kMaxCorrespondenceCost 0.9f
BoundedFloatToValue 先将概率[0.1,0.9] 然后等比转化为[1,32767] 
3、GetProbability
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) 1.超出限制时返回最小的概率
if (!limits().Contains(cell_index)) return kMinProbability;2.先转化成 CorrespondenceCost，在转化成概率
return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
correspondence_cost_cells()[ToFlatIndex(cell_index)]));5、grid_2d
class Grid2D : public GridInterface // GridInterface空的 todo(kdaun) move mutual functions of Grid2D/3D here
1.成员变量：
MapLimits limits_;
std::vector
float min_correspondence_cost_;
float max_correspondence_cost_;
std::vector
// Bounding box of known cells to efficiently compute cropping limits.已知单元格的边界框可有效计算裁剪限制。
Eigen::AlignedBox2i known_cells_box_;
const std::vector
2.Grid2D构建
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),
min_correspondence_cost_(min_correspondence_cost),
max_correspondence_cost_(max_correspondence_cost),
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_);
}MapLimits 定义网格图的限制
成员变量 double resolution_; //分辨率 0.05
Eigen::Vector2d max_; 栅格左上角的真实位姿
CellLimits cell_limits_; //Grid的长、宽
该为真是点与Grid点转化的关系
ValueConversionTables
成员变量 // 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
std::map
std::unique_ptr
成员函数：
const std::vector
栅格图Grid2D构建的时，首先根据limits确定大小，并且值未知赋值给0。
3.ComputeCroppedLimits 计算裁剪限制
void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
CellLimits* const limits) const已知单元格的边界框可有效计算裁剪限制。
如果栅格是空的，则offset=(0,0) limits=(1,1)
*offset = known_cells_box_.min().array();
*limits = CellLimits(known_cells_box_.sizes().x() + 1, known_cells_box_.sizes().y() + 1);
4.GrowLimits
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
const std::vector*>& grids,
const std::vector& grids_unknown_cell_values) //根据需要增加地图以包含“点”。 这改变了意义这些坐标向前发展。 必须立即调用此方法在'FinishUpdate'之后，在对 'ApplyLookupTable'的任何调用之前。
GrowLimits(point, {mutable_correspondence_cost_cells()}, {kUnknownCorrespondenceValue});
1. 首先将point转化为Grid坐标系，然后判断该点是否在该坐标系下，不是则进入下面
while (!limits_.Contains(limits_.GetCellIndex(point))) {2. 计算x_offset,y_offset. 当前Grid的长宽的一半
const int x_offset = limits_.cell_limits().num_x_cells / 2;
const int y_offset = limits_.cell_limits().num_y_cells / 2;3.创建 MapLimits new_limits (limits_.resolution(),
limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset)，CellLimits(2倍的长宽))
4.stride Grid坐标系中 new_limits 的列数
offset =x_offset + stride * y_offset; 在新的Grid坐标系下原数据的起点
new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells;
5.将原来的Grid数据投到新的Grid上。
for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
1>新建 std::vector
2>赋值 根据二者关系赋值}
6.跟新边界的长宽 m_min += t; m_max += t; }//while循环结束
5.其他
1.FinishUpdate 完成更新顺序。
2.GetCorrespondenceCost 使用'cell_index'返回单元格的对应代价。
3.IsKnown 如果已知指定索引处的对应成本，则返回true。
4.ToFlatIndex 根据 (x,y)得出 一维数组的下标
6、ProbabilityGridRangeDataInserter2D
class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface
构造函数：
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
const proto::ProbabilityGridRangeDataInserterOptions2D& options)
: options_(options),
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.hit_probability()))),
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.miss_probability()))) {}成员变量:
const proto::ProbabilityGridRangeDataInserterOptions2D options_;
const std::vector hit_table_;
const std::vector miss_table_; 成员函数：
1、Insert 插入激光数据往栅格中
void ProbabilityGridRangeDataInserter2D::Insert(
const sensor::RangeData& range_data, GridInterface* const grid) const1.将Grid 静态 转化为 ProbabilityGrid
ProbabilityGrid* const probability_grid = static_cast(grid); 2.CastRays 函数进行跟新
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
CHECK_NOTNULL(probability_grid));3.完成跟新
probability_grid->FinishUpdate();2、CastRays 函数
void CastRays(const sensor::RangeData& range_data,
const std::vector& hit_table,
const std::vector& miss_table,
const bool insert_free_space, ProbabilityGrid* probability_grid) 1.根据激光数据，查看该概率栅格地图是否需要增长 GrowAsNeeded
GrowAsNeeded(range_data, probability_grid);2.射线投射的起点和终点的子像素精度因子 计算
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
constexpr int kSubpixelScale = 1000;3.相当于精度提高了kSubpixelScale倍，重新定义 MapLimits
const MapLimits superscaled_limits(
superscaled_resolution, limits.max(),
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
limits.cell_limits().num_y_cells * kSubpixelScale));4.将 激光的orgin 坐标投入到该Grid坐标系中
const Eigen::Array2i begin =
superscaled_limits.GetCellIndex(range_data.origin.head<2>());5. 激光数据有 hit miss 点 。假设是'起点'和'未命中'之间是自由空间。光线从'原点'开始， '返回'是障碍物所在的点。检测到 '未命中'是光线方向上的点，没有返回被检测到，并以配置的距离插入。
添加hit点， 遍历 hit点，并且投入到Grid坐标系中
for (const sensor::RangefinderPoint& hit : range_data.returns) {
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
}6.根据hit 点 添加miss点
for (const Eigen::Array2i& end : ends) {
std::vector ray =
RayToPixelMask(begin, end, kSubpixelScale);
for (const Eigen::Array2i& cell_index : ray) {
probability_grid->ApplyLookupTable(cell_index, miss_table);
}
} 7.最后，根据范围数据中的未命中计算和添加空光线。
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
std::vector ray = RayToPixelMask(
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
kSubpixelScale);
for (const Eigen::Array2i& cell_index : ray) {
probability_grid->ApplyLookupTable(cell_index, miss_table);
}
}