雷达写成栅格地图-cartographer
Kong Liangqian Lv6
 2021-11-20 18:38:29  

雷达数据写入器生成

位于cartographer/mapping/2d/submap_2d.h中ActiveSubmaps2D类 的构造函数里

1
2
3
// ActiveSubmaps2D构造函数
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
    : options_(options), range_data_inserter_(CreateRangeDataInserter()) {}

CreateRangeDataInserter

创建两种地图写入器，PROBABILITY_GRID_INSERTER_2D 与 TSDF_INSERTER_2D

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 创建地图数据写入器
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::InsertRangeData 中，把数据写入每一个子图中

1
2
3
4
5
6
7
8
9
10
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  ...
  // 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  ...
}

此处的for循环的元素为每一个submap2D，调用submap2D的insertRangeData函数

1
2
3
4
5
6
7
8
9
10
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_inserter是PROBABILITY_GRID_INSERTER_2D，（默认）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
/**
 * @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();
}

CastRays

根据雷达点对栅格地图进行更新

1
2
3
4
5
6
7
8
9
10
11
12
13
/**
 * @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)

详见 https://blog.csdn.net/chaosir1991/article/details/109561010

这里一个是对hit点进行更新，另外一个是对原点到hit点图中的点进行更新，使用Bresenham算法

还有就是没有打到物体的点进行更新

如何对一个类进行for 循环，可以查看此sensor::RangeData 中的 pointcloud

 Comments