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]       \left ( x-x_{min} \right ) *32768/(x_{max}-x_{min}))

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的长、宽 

cartographer 生成子图_第1张图片

该为真是点与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.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 new_cells(new_size, grids_unknown_cell_values[grid_index]);

2>赋值  根据二者关系赋值} 

6.跟新边界的长宽   m_min += t; m_max += t;    }//while循环结束

cartographer 生成子图_第2张图片

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.   激光数据有 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);
    }
  }

 

 

 

 

 

 

 

你可能感兴趣的:(slam_2d)