cartographer 生成子图 slam_2d

红色代表接下來准备解释的函数。。
1、生成子图调用函数在local_trajectory_builder_2d.ccInsertIntoSubmap函数中

std::vector> insertion_submaps = active_submaps_.InsertRangeData(range_data_in_local);

submap_2d.ccSubmap2D类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.InsertRangeDataFinish在类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.将插入子图完成设置成trueset_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);

kMinCorrespondenceCost0.1fkMaxCorrespondenceCost 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 correspondence_cost_cells_; //存的概率值用0-32767表示
float min_correspondence_cost_;
float max_correspondence_cost_;
std::vector update_indices_;
// Bounding box of known cells to efficiently compute cropping limits.已知单元格的边界框可有效计算裁剪限制。
Eigen::AlignedBox2i known_cells_box_;
const std::vector* value_to_correspondence_cost_table_;
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>>bounds_to_lookup_table_;
成员函数：
const std::vector* GetConversionTable(float unknown_result,float lower_bound, float upper_bound);
栅格图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.strideGrid坐标系中 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 new_cells(new_size, grids_unknown_cell_values[grid_index]);
2>赋值根据二者关系赋值}
6.跟新边界的长宽m_min += t; m_max += t; }//while循环结束
cartographer 生成子图

文章图片

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) const

1.将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.激光数据有 hitmiss 点。假设是'起点'和'未命中'之间是自由空间。光线从'原点'开始， '返回'是障碍物所在的点。检测到 '未命中'是光线方向上的点，没有返回被检测到，并以配置的距离插入。
添加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); } }

【cartographer 生成子图】