cartographer tsdf插入子图

函数

  virtual void Insert(const sensor::RangeData& range_data,
                      GridInterface* grid) const override;

步骤

1.首先获得截断距离

从lua中读取 runcation_distance

2.设定TSDF类

静态转换，基类转换为子类

  TSDF2D* tsdf = static_cast<TSDF2D*>(grid);

3.延伸点云

• 将某一点云到激光原点方向归一化，然后将该hit点增长truncation_distance倍，最后判断增长是否超出范围，确保在范围之内。

void GrowAsNeeded(const sensor::RangeData& range_data,
                  const float truncation_distance, TSDF2D* const tsdf) {
  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    const Eigen::Vector3f direction =
        (hit.position - range_data.origin).normalized();
    const Eigen::Vector3f end_position =
        hit.position + truncation_distance * direction;
    bounding_box.extend(end_position.head<2>());
  }
  // Padding around bounding box to avoid numerical issues at cell boundaries.
  constexpr float kPadding = 1e-6f;
  tsdf->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones());
  tsdf->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones());
}

4.点云排序

    std::sort(sorted_range_data.returns.begin(),
              sorted_range_data.returns.end(),
              RangeDataSorter(sorted_range_data.origin));

5.循环遍历点云

确定hit，normal，进行正常插入

    const Eigen::Vector2f hit =
        sorted_range_data.returns[hit_index].position.head<2>();
    const float normal = normals.empty()
                             ? std::numeric_limits<float>::quiet_NaN()
                             : normals[hit_index];
    InsertHit(options_, hit, origin, normal, tsdf);

5.1核心函数 InsertHit

• 1.计算 hit -> origin 的距离
• 2.当距离小于truncation_distance（截止距离）时，直接return
• 3.截止距离占总长度的百分比，用于确定tsdf中点云的使用范围
• 4.返回为超标的激光std::pair<Eigen::Array2i, Eigen::Array2i> superscaled_ray
  SuperscaleRay(ray_begin, ray_end, tsdf)
  确定超标的分辨率，进而确定地图的范围，最后将起始放入

std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
   const Eigen::Vector2f& begin, const Eigen::Vector2f& end,
   TSDF2D* const tsdf) {
 const MapLimits& limits = tsdf->limits();
 const double superscaled_resolution = limits.resolution() / kSubpixelScale;
 const MapLimits superscaled_limits(
     superscaled_resolution, limits.max(),
     CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                limits.cell_limits().num_y_cells * kSubpixelScale));

 const Eigen::Array2i superscaled_begin =
     superscaled_limits.GetCellIndex(begin);
 const Eigen::Array2i superscaled_end = superscaled_limits.GetCellIndex(end);
 return std::make_pair(superscaled_begin, superscaled_end);
}

• 5.计算包含线段连接的某些部分的所有像素,‘scaled_begin’和’scaled_end’。 缩放’scaled_begin’和’scaled_end’,‘subpixel_scale’。 预计’scaled_begin’和’scaled_end’将是大于零。 返回值以像素为单位而不是缩放。
• 6.跟新栅格值

6.完成跟新