cartographer（9） probability grid

cartographer 源码中实际上提供grid map两种形式，一种为probability，另一种为tsdf。

grid map

  MapLimits limits_;  // 地图大小边界，包括x和y最大值， 分辨率， x和yu方向栅格数
  std::vector<uint16> correspondence_cost_cells_;  //grid 地图存储值，采用uint16类型，
  float min_correspondence_cost_;  // 对应实际意义的最大和最小边界
  float max_correspondence_cost_; 
  std::vector<int> update_indices_; // 记录已经更新过的索引
  Eigen::AlignedBox2i known_cells_box_;  // 已知栅格状态的大小
  const std::vector<float>* value_to_correspondence_cost_table_;  // 此为查询表格，主要因为grid存储的uint16格式，可根据此表格将其对应成实际意义的如probability

GrowLimits用于扩展此gridmap的大小，即当加入一个已知栅格状态时，需要判断并扩展当前gridmap

// 更新地图大小，同时将原grid中数据按照位置放入新放大的grid中
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
                        const std::vector<std::vector<uint16>*>& grids,
                        const std::vector<uint16>& grids_unknown_cell_values) {
  CHECK(update_indices_.empty());
  //如果当前的存在point不在范围内，即需要更新，采用迭代方法放大地图边界，
  while (!limits_.Contains(limits_.GetCellIndex(point))) {
    //获取原来的地图大小的中心坐标，即栅格索引
    const int x_offset = limits_.cell_limits().num_x_cells / 2;
    const int y_offset = limits_.cell_limits().num_y_cells / 2;
    // grid最大值更新原来的一半， 地图总大小放大一倍。 即从地图中心位置上下左右均放大原大小一半
    const MapLimits new_limits(
        limits_.resolution(),
        limits_.max() +
            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
        CellLimits(2 * limits_.cell_limits().num_x_cells,
                   2 * limits_.cell_limits().num_y_cells));
    
    const int stride = new_limits.cell_limits().num_x_cells;// 行数，用于转换1维索引
    
    const int offset = x_offset + stride * y_offset;//新的offset
   
    const int new_size = new_limits.cell_limits().num_x_cells *
                         new_limits.cell_limits().num_y_cells; //新大小

    //更新grid概率，即将原来的概率赋值在新的grid中
    for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
      std::vector<uint16> new_cells(new_size,grids_unknown_cell_values[grid_index]);
      for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
        for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
          new_cells[offset + j + i * stride] =
              (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
        }
      }
      *grids[grid_index] = new_cells;
    }
    limits_ = new_limits;
    // 重新计算有效栅格空间边界，即由于起点发生改变，则矩形框的坐标需进行转换
    if (!known_cells_box_.isEmpty()) {
      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
    }
  }
}

下图可以表示这个函数的过程，地图的介绍就到这。匹配时候用的就是Grid2D这个类。
 

probability map

ProbabilityGrid是继承Grid2D，实际是具体描述了Grid2D单元存储内容的具体意义，即为概率。一个单元格最终想表示的障碍物是否存在，即三个状态：占有（hit），未占有（miss），未知。由于传感器噪声和栅格分辨率的问题，因此状态不会为确定值。因此我们将其以概率形式表示，包括占有概率和未占有概率。

Grid2D基类也提过每个单元存储的为uint16的的整数，因此probability_values.h文件中也将对应概率值转换为1~32767之间，而0表示未知状态。

具体转换公式：value =（float_value - lower_bound) *     (32766.f / (upper_bound -ower_bound)) +    1; 取整

//float_value：概率值  

upper_bound：最大概率

ower_bound：最小概率

probability map cell 概率栅格图初始化

即设置grid格的probability和获取对应的probability，其代码实现在cartographer/mapping/2d/probability_grid.c中

// 设置对应index的概率值， 在cell单元存储的是对应的free 占有率(uint16整形数)
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                     const float probability) {
  // cell_index栅格地图中对应索引
  // ToFlatIndex 二维坐标转换为1维索引
  // mutable_correspondence_cost_cells() 返回的grid中grid地图
  uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
  CHECK_EQ(cell, kUnknownProbabilityValue);
  // 将概率值转换对应的整型数，然后存入对应的索引的单元中
  cell = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));

  //返回的是有效概率值的矩形框，每更新一个cell，则更新矩形框box
  //采用eigen库extend，自动更新有效框
  mutable_known_cells_box()->extend(cell_index.matrix());
}

cartographer的grid cell概率实际存储的是free probability--空闲概率。

主要是匹配操作时采用类似最小二乘的优化思想，需要将最佳匹配值改为求取最小状态，因此采取free的概率进行匹配，则最小值则为最佳匹配。

probability grid地图更新

Free状态的概率:p(free) 

Occupied状态的概率:p(occ) 

p(free)+p(occ)=1

引入两者的比值来作为此cell的状态：

 引入观测值Z, Z的状态是hit或是miss.其观测的更新概率可以看做为p(s=occ|z)和p(s=free|z)

即在当前观测z（占hit或misss）的情况下，一个栅格实际为hit的概率和miss的概率，即为条件概率。因此可以用贝叶斯公式进行求取，如下:

现在假设原来cell为odd(s)， 而对应的更新odd则为

根据贝叶斯公式进行代入可获取如下公式：

如此可清晰看出更新后的cell应为

其中k-1为上一时刻的odds，而k为经过一次观测后的odds，显然仅是相差了C(z)这个系数

其中系数为 

其中p(z|s=occ)表示的是当cell为障碍时，即z=1，传感器认为是障碍的概率，显然是传感器本身属性决定的，因此为一常数。同样其他3种组合也均为常数，因此其系数C(z)是根据观测hit和miss决定的两个常数，即Cmiss 和Chit 两个常数。

简单总结:其更新过程可认为根据传感器观测的状态，对原cell的概率进行更新，假设原cell的值为value。当传感器测到此cell为hit时，则新的value_new=Chit * value; 如果传感器测的此cell为miss时，则新的value_new=Cmiss * value

函数ApplyLookupTable是用来通过查表来更新栅格单元的占用概率的

// 使用查找表对指定栅格进行栅格值的更新
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;
}