cartographer_probability_grid

古路

已于 2022-11-29 16:48:30 修改

阅读量1.4k

点赞数 4

分类专栏： # cartographer # 2DLidar slam 文章标签：自动驾驶 cartographer 概率栅格地图概率栅格 probabilitygrid 1024程序员节

于 2022-06-15 10:09:38 首次发布

本文链接：https://blog.csdn.net/fb_941219/article/details/125264092

版权

slam 同时被 3 个专栏收录

70 篇文章 22 订阅

订阅专栏

2DLidar

21 篇文章 2 订阅

订阅专栏

cartographer

12 篇文章 2 订阅

订阅专栏

cartographer_probability_grid

0.引言

update:栅格地图部分参考最新的博客

首先从 ActiveSubmaps2D 上层进入，由于cartographer支持 2D 和 3D，所以在代码基本是面向接口编程的，不完整类图如下(画得不是很规范，将就看）：

请添加图片描述

ActiveSubmaps2D 类：

请添加图片描述

核心函数就是 InsertRangeData().

这部分代码辨别坐标时：

Eigen::Array2i ：代表像素坐标
Eigen::Vector2f：代表地图坐标
不错的参考文章

请添加图片描述

图中显示了一帧点云如何画到概率图。左侧是点云，右侧是画到概率图后的结果。绿色坐标系是概率图的坐标系，原点在左上角，水平方向，x坐标值从左到右变大；垂直方向，y坐标值从上到下变大。

概率图中，两相邻坐标间的单元称为栅格（Cell），是正方形，其坐标叫CellIndex。一个栅格表示resolutionresolution的真实世界面积。举个例子，resolution=0.05时，一个栅格面积就是5mm5mm。

栅格中存的值是概率，概率有两种：占用和空闲。为方便ScanMatch转换成最小二乘问题，概率图中栅格存储着以uint16_t表示的空闲概率：CorrespondenceCostValue。

一旦创建了概率图，那它就固定了一个有效范围，有效范围的面积是cell_limits_.num_x_cells(每行cell个数) * num_y_cells(每列cell个数)。在图1，只显示了概率图中有效范围。通过MapLimits::GetCellIndex(point)，可从点云坐标换算到CellIndex，此操作可能算出不在有效范围的CellIndex。而一个CellIndex是不是有效，可用MapLimits::Contains(cell_index)进行判断。

图1中，laser（激光雷达）有个初始平移：(0.05m, 0.1m)，即laser坐标系原点在map坐标系的坐标是(0.05, 0.1)。这个初始平移不会影响概率图尺寸，但会影响概率图在map坐标系的位置。如果平移是(0, 0)，此个概率图的位置是左上角(-0.25m, 0.25m)、尺寸5cmx5cm的矩形。平移变成(0.05m, 0.1m)后，概率图的位置变成了左上角(-0.2m, 0.35m)、尺寸5cmx5cm的矩形。当然，这个变动是针对map坐标系，对laser坐标系来说，不论初始平移是何值，它的左上角坐标总是(-0.25m, 0.25m)。这符合物理意义：雷达在map坐标系移动，它能扫描的范围总是以laser原点为中心的一个固定尺寸矩形，只是随着不断移动，这个矩形在map坐标系的位置会改变。

ProbabilityGrid用于表示概率图，它是Grid2D子类，由它构造的对象存放在ActiveSubmaps2D成员变量submaps_。

1.MapLimits

请添加图片描述

CellLimits

// 地图x方向与y方向的格子数
 int num_x_cells = 0;
 int num_y_cells = 0;

请添加图片描述

默认resolution:0.05–>5cm/pixel.分辨率还是挺高的，比较耗内存。

  // 计算物理坐标点的像素索引
  Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
    return Eigen::Array2i(
        common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
        common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
  }
  // 根据像素索引算物理坐标
  Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
    return {max_.x() - resolution() * (cell_index[1] + 0.5),
            max_.y() - resolution() * (cell_index[0] + 0.5)};
  }

2.栅格地图的更新

这一小节主要搬迁至这篇文章，备忘。

相关常量和数值的计算在 probability_values.h/.cc里面。
请添加图片描述

举例：假设 looccu = 0.9，lofree = -0.7。那么，显而易见，一个栅格状态的数值越大，就越表示该栅格为占据状态，相反数值越小，就表示改栅格为空闲状态。（这也就解决了此前文中提出的激光雷达观测值”不一定准”的问题）
请添加图片描述

每一个栅格的值，是历史上所有测量的累积和:

>0 则认为是占据
<0则认为是空闲

上图是用两次激光扫描数据更新地图的过程。在结果中，颜色越深越表示栅格是空闲的，颜色越浅越表示是占据的。这里要区分常用的激光SLAM算法中的地图，只是表述方式的不同，没有对错之分。

总结：更新模型为（记住有个取对数的过程就容易理解值的含义了）：

请添加图片描述

3.Grid2D

请添加图片描述

ValueConversionTables类位于 value_conversion_tables.h中，在Grid2D构造时，预先计算好转换表，将将[0, 1~32767] 映射到 [0.9, 0.1~0.9]的转换表，其他传输的是指针，很多地方都用到了这个表，但实际只计算了一次。
地图增长：判断点是否在地图内，如果不在，则地图一次扩大两倍直至该点包含在地图中：

请添加图片描述

上面栅格地图增长方式过于暴力，因为实际的栅格地图可能长宽差别很大，而使用的栅格图像确是正方形的（因为预设的栅格图像大小是100x100），所以需要根据known_cells_box_减少栅格地图所占的内存空间.

概率栅格图的值存储的是空闲的概率：

成员变量：

  // 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 
  // 将二维索引转换为一维索引
  std::vector<uint16> correspondence_cost_cells_;

构造函数中对correspondence_cost_cells_初始化，大小为地图大小（二维转一维表示）值为0 ，表示未知状态 $l o g (1)$ ，空闲或占据的概率相同。

 correspondence_cost_cells_(
     limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
     kUnknownCorrespondenceValue)

4.ProbabilityGrid

请添加图片描述

SetProbability
GetProbability

5.ProbabilityGridRangeDataInserter2D

请添加图片描述

将点云数据写入到栅格地图中。

数据流：

1.LocalTrajectoryBuilder2D：

    std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
  LocalTrajectoryBuilder2D::InsertIntoSubmap(
      const common::Time time, const sensor::RangeData& range_data_in_local,
      const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
      const transform::Rigid3d& pose_estimate,
      const Eigen::Quaterniond& gravity_alignment) {
  	// ...
    // 将点云数据写入到submap中
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
        active_submaps_.InsertRangeData(range_data_in_local);
  ``

2.ActiveSubmaps2D:

// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图
  // 因为这时第一个子图应该已经处于完成状态了
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(range_data, range_data_inserter_.get());
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  // 因为上一句for 循环是两个子图都在添加数据
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
    submaps_.front()->Finish();
  }
  return submaps();
}

3.Submap2D：

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

4.ProbabilityGridRangeDataInserter2D

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

5.probability_grid_range_data_inserter_2d匿名命名空间

/**odds
 * @brief 根据雷达点对栅格地图进行更新
 * odds
 * @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)

hit_table 更新占用栅格时的查找表
miss_table 更新空闲栅格时的查找表

在构造函数中预先计算好了这两个查找表：

// 写入器的构造, 新建了2个查找表
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
    : options_(options),
      // 生成更新占用栅格时的查找表 // param: hit_probability
      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()))),    // 0.55
      // 生成更新空闲栅格时的查找表 // param: miss_probability
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability()))) {} // 0.49

请添加图片描述

回顾概率栅格地图：概率栅格图的值存储的是空闲的概率：

成员变量：

  // 地图栅格值, 存储的是free的概率转成uint16后的[0, 32767]范围内的值, 0代表未知 
  // 将二维索引转换为一维索引
  std::vector<uint16> correspondence_cost_cells_;

构造函数中对correspondence_cost_cells_初始化，大小为地图大小（二维转一维表示）值为0 ，表示未知状态 $l o g (1)$ ，空闲或占据的概率相同。

 correspondence_cost_cells_(
     limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
     kUnknownCorrespondenceValue)

在更新的时候，转为浮点进行更新。哦哦哦，一帧数据只对地图更新一次，使用 +kUpdateMarker 标记，一帧结束后FinishUpdate时就把kUpdateMarker减掉，新的一帧数据进来时再继续对栅格进行更新。

为什么可以提前计算，hit_table 和 miss_table ？因为，击中值为0.55 miss的值0.49，一个栅格概率值的范围确定为 0.1-0.9，可以提前将所有情况遍历计算完。

参考文章

预计算hit_table和miss_table：这两个table存的都是Value（空闲概率的整数映射表达），只需计算一次，可视为常量数组。
- 1.hit_table：给定某个栅格的空闲概率整数值(uint16), 如果该栅格再次被激光击中了, 那么它新的空闲概率值, 其实是可以预先计算出来的，如下：
```
chit_table_[原有的空闲概率整数值(uint16)] = 再次击中之后新的空闲概率整数值(uint16)
```
- 2.miss_table：跟hit_table的作用相反
- 3.able中index 0的作用
  - (1) hit_table[0]：第一次被观测（在此之前该栅格是unknown状态，即，value=0）到的栅格（hit）对应的空闲value（p_hit=0.55）
  - (2) miss_table[0]：第一次被观测到的栅格（miss）对应的空闲value（p_miss=0.49）
栅格地图更新
- 1.保证栅格在一轮scan insert中只更新一次：对上面的hit_table和miss_table中每个value增加一个kUpdateMarker=32768来实现.
- 2.hit更新：hit指的是激光点打到物体上，对这个位置对应的栅格的概率进行更新，通过hit_table_实现.
- 3.miss更新：由两部分构造成：
  - （1）从激光点到hit之间的为miss，这中间的栅格的概率通过miss_table进行更新.（这里涉及画线算法）RayToPixelMask(begin, end, kSubpixelScale)
  - （2）激光打出去未返回的点，比如，在空旷区域，miss默认长度设置为5m，表示5m内这些空间都是free的.
超分辨率
提升分辨率后，hit点转到栅格图像的index的精度更高了，这样从origin->hit所经过的栅格更加精确，更有利于miss的更新，具体做法如下：
- 1.一条直线穿过很多cell，有的cell从对角穿过，有的只穿过一个小角落，虽然它们都是穿过，但是穿过的观测量不同，从对角穿过对这个cell的空闲概率提升应该作用更大，而只穿过一个小角落显然贡献很微弱.
- 2.为了定量得到直线穿过该cell时，对该cell的贡献，采用超分辨率来实现。对于从对角穿过的cell，那么超分辨率后，就有很多小的sub-cell落在原来的cell中，sub-cell数（记为n）越多，贡献就越大（其实就是对cell这个栅格的概率重复算了n次…），而对于从小角落穿过的cell，sub-cell数就很少了.
栅格地图增长
如果在栅格地图的更新过程中，发现图像的index超过了原来的栅格图像的范围，则长宽各需扩大1倍（左右各扩大0.5倍），并将原来的栅格图像整体挪到中心位置.
减少栅格地图内存空间
上面栅格地图增长方式太暴力了，因为实际的栅格地图可能长宽差别很大，而使用的栅格图像确是正方形的（因为预设的栅格图像大小是100x100），所以需要根据known_cells_box_减少栅格地图所占的内存空间.

6.贝汉明画线算法

7.Active map 匹配

请添加图片描述

古路

关注

4
点赞
踩
23

收藏

觉得还不错? 一键收藏
1
评论
cartographer_probability_grid

首先从上层进入，由于cartographer支持 2D 和 3D，所以在代码基本是面向接口编程的，不完整类图如下(画得不是很规范，将就看）：ActiveSubmaps2D 类：核心函数就是 .这部分代码辨别坐标时：默认resolution:0.05–>5cm/pixel.分辨率还是挺高的，比较耗内存。2.栅格地图的更新这一小节主要搬迁至这篇文章，备忘。相关常量和数值的计算在里面。举例：假设 looccu = 0.9，lofree = -0.7。那么，显而易见，一个栅格状态的
复制链接

扫一扫