概率地图-cartographer
Kong Liangqian Lv6
 2021-11-19 18:38:29  

ProbabilityGrid

ProbabilityGrid 是在cartographer可以被选择的其中一个2D地图，他继承与Grid2D。Grid2D保存了地图的栅格信息，以及控制的地图的扩张等功能。

他的成员变量只有conversiontable

下面看一下ProbabilityGrid的构造函数

构造函数

1
2
3
4
5
6
7
8
9
 /*
 * @param[in] limits 地图坐标信息
 * @param[in] conversion_tables 转换表
 */
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
                                 ValueConversionTables* conversion_tables)
    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
             conversion_tables),
      conversion_tables_(conversion_tables) {}

此构造函数首先调用的是Grid2D的构造函数，完成了转换表的初始化后，传入到conversiontables

关于概率的两个函数

  • SetProbability：将 索引 处单元格的概率设置为给定的概率, 仅当单元格之前处于未知状态时才允许

    probability 是被占用的概率，但是存的是free的栅格值，首先要转为free的概率，然后转为栅格值

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                         const float probability) {
      // 获取对应栅格的引用
      uint16& cell =
          (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
      CHECK_EQ(cell, kUnknownProbabilityValue);
      // 为栅格赋值 value
      cell =
          CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
      // 更新bounding_box
      mutable_known_cells_box()->extend(cell_index.matrix());
    }

  • GetProbability：获取 索引 处单元格的占用概率

    首先需要获取cell_index 对应的下表序号，然后转为对应的栅格值，再转为对应的栅格值，在转为free值，最后1-free

    1
    2
    3
    4
    5
    float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
      if (!limits().Contains(cell_index)) return kMinProbability;
      return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
          correspondence_cost_cells()[ToFlatIndex(cell_index)]));
    }

ApplyLookupTable

把cellindex位置的 table的值，存储到地图数据中，即correspondence_cost_cells 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// 使用查找表对指定栅格进行栅格值的更新
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  // 对处于更新状态的栅格, 不再进行更新了
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 标记这个索引的栅格已经被更新过
  mutable_update_indices()->push_back(flat_index);
  // 更新栅格值
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
  return true;
}

ComputeCroppedGrid

根据bounding box，对地图进行裁剪，裁剪到刚好可以包含整个点云

DrawToSubmapTexture

获取压缩后的地图栅格数据，这个获得的地图也是裁剪之后的

 Comments