cartographer的一帧激光雷达数据插入到submap地图中

1.首先将 原点 位于local坐标系原点处的 点云 变换成 原点位于匹配后的位姿处的点云

2.将变化后的激光点云数据 传入到InsertIntoSubmap

 std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, range_data_in_local, filtered_gravity_aligned_point_cloud,
      pose_estimate, gravity_alignment.rotation());

3.进入到InsertIntoSubmap函数中，调用InsertRangeData

// 将点云数据写入到submap中
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

4.进入到InsertRangeData函数中

// 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }

5.进入到InsertRangeData函数中，这个是上面InsertRangeData嵌套的

// 将雷达数据写到栅格地图中
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);
}

6.调用insert（）函数 实际上是调用ProbabilityGridRangeDataInserter2D的insert（）函数

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();
}

7.查找表制作 ProbabilityGridRangeDataInserter2D构造函数会根据传入的占用栅格概率/miss概率 生成两个查找表

具体是占用概率转成free概率，转成value值 [0, 1- 32767]

//将映射表中value值

更新空闲

8.kUpdateMarker = 32769

finish_update 会减去kUpdateMarker

9.进入函数CastRays（）

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

10.进入到GrowAsNeeded（）

// 根据雷达数据调整地图范围

// 根据点云的bounding box, 看是否需要对地图进行扩张

GrowAsNeeded(range_data, probability_grid);

11. 进入到GrowLimits （）函数

// 根据坐标决定是否对地图进行扩大

12.回到 CastRays（）函数

// 计算hit点在地图中的像素坐标, 作为画线的终止点坐标
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
// 更新hit点的栅格值
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);

13.更新miss点

 // Now add the misses.
  for (const Eigen::Array2i& end : ends) {
    std::vector<Eigen::Array2i> ray =
        RayToPixelMask(begin, end, kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
      // 从起点到end点之前, 更新miss点的栅格值
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }

14.更新超过的点

后面会详细介绍

欢迎大家一起学习交流~

每天进步一点点，时光不负有心人❤