内容列表
红色代表接下來准备解释的函数。。
1、生成子图调用函数
说明：
- 活跃子图最多只有两个，刚初始化时只有一个，随后一直保持两个
- 只与第一个子图匹配，插入时活跃子图都插入
- 第一个子图插入个数等于2倍的值时，活跃子图完成，第二个子图等于1倍的值时，插入新子图。
在local_trajectory_builder_2d.cc InsertIntoSubmap函数 中
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
active_submaps_.InsertRangeData(range_data_in_local);
- 该函数： submap_2d.cc Submap2D类 ActiveSubmaps2D类 中
2、ActiveSubmaps2D类
说明：
在插入第一帧激光数据时创建第一个活跃子图，除非在此初始化期间没有或只有一个子图存在，总是有两个子目录插入范围数据：用于匹配的旧子图，以及将用于下次匹配的新子图，他正在初始化。一旦一定数量的激光数据被插入后，新的子图被考虑初始化：旧的子图被改变，即新子图变为现在的旧子图，用于扫描匹配。而且一个新的子图被创建，忘记旧的子图。
两个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<ProbabilityGridRangeDataInserter2D>( options_.range_data_inserter_options() .probability_grid_range_data_inserter_options_2d()); 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."; }
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<std::shared_ptr<Submap2D>> submaps_; std::unique_ptr<RangeDataInserterInterface> range_data_inserter_; ValueConversionTables conversion_tables_;
成员函数：
1、InsertRangeData函数
std::vector<std::shared_ptr<const Submap2D>> 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<Submap2D>( origin, std::unique_ptr<Grid2D>( static_cast<Grid2D*>(CreateGrid(origin).release())), &conversion_tables_));
3、CreateGrid函数
std::unique_ptr<GridInterface> 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<ProbabilityGrid>( MapLimits(resolution, origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution * Eigen::Vector2d::Ones(), CellLimits(kInitialSubmapSize, kInitialSubmapSize)), &conversion_tables_); 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(), 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<ProbabilityGrid>(
MapLimits(resolution,
origin.cast<double>() + 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<Grid2D> grid, ValueConversionTables* conversion_tables); explicit Submap2D(const proto::Submap2D& proto, ValueConversionTables* conversion_tables);
1.由ActiveSubmaps2D类构造
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);}
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<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());}
成员变量：
std::unique_ptr<Grid2D> 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<Grid2D> 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<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)); }
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<uint16> correspondence_cost_cells_; //存的概率值 用0-32767表示 float min_correspondence_cost_; float max_correspondence_cost_; std::vector<int> update_indices_;
// Bounding box of known cells to efficiently compute cropping limits.已知单元格的边界框可有效计算裁剪限制。 Eigen::AlignedBox2i known_cells_box_; const std::vector<float>* 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<const std::tuple<float /* unknown_result */, float /* lower_bound */, float /* upper_bound */>,
std::unique_ptr<const std::vector<float>>> bounds_to_lookup_table_;
成员函数：
const std::vector<float>* 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<std::vector<uint16>*>& grids, const std::vector<uint16>& grids_unknown_cell_values)
//根据需要增加地图以包含“点”。 这改变了意义这些坐标向前发展。 必须立即调用此方法在'FinishUpdate'之后，在对 'ApplyLookupTable'的任何调用之前。
GrowLimits(point, {mutable_correspondence_cost_cells()}, {kUnknownCorrespondenceValue});
- 首先将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<uint16> new_cells(new_size, grids_unknown_cell_values[grid_index]);
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<uint16> hit_table_; const std::vector<uint16> 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<ProbabilityGrid*>(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<uint16>& hit_table, const std::vector<uint16>& 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<Eigen::Array2i> 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<Eigen::Array2i> 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); } }
评论（0）
您还未登录，请登录后发表或查看评论