cartographer grid_2d submap2d probality_grid 关系

路径 cartographer/mapping/2d/***

一、grid_2d

class Grid2D : public GridInterface // GridInterface空的 todo(kdaun) move mutual functions of Grid2D/3D here

1.成员变量：

MapLimits limits_;
std::vector<uint16> correspondence_cost_cells_; //存的概率值用0-32767表示
float min_correspondence_cost_;
float max_correspondence_cost_;
std::vector<int> update_indices_;

// Bounding box of known cells to efficiently compute cropping limits.已知单元格的边界框可有效计算裁剪限制。
Eigen::AlignedBox2i known_cells_box_;
const std::vector<float>* value_to_correspondence_cost_table_;

2.Grid2D构建

Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
               float max_correspondence_cost,
               ValueConversionTables* conversion_tables)
    : limits_(limits),
      correspondence_cost_cells_(
          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
          kUnknownCorrespondenceValue),
      min_correspondence_cost_(min_correspondence_cost),
      max_correspondence_cost_(max_correspondence_cost),
      value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
          max_correspondence_cost, min_correspondence_cost,
          max_correspondence_cost)) {
  CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
}

MapLimits 定义网格图的限制

成员变量 double resolution_; //分辨率 0.05

Eigen::Vector2d max_; 栅格左上角的真实位姿
CellLimits cell_limits_; //Grid的长、宽

该为真是点与Grid点转化的关系

ValueConversionTables

成员变量 // 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].

std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */, float /* upper_bound */>,
std::unique_ptr<const std::vector<float>>> bounds_to_lookup_table_;

成员函数：

const std::vector<float>* GetConversionTable(float unknown_result, float lower_bound, float upper_bound);

栅格图Grid2D构建的时，首先根据limits确定大小，并且值未知赋值给0。

3.ComputeCroppedLimits 计算裁剪限制

void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
                                  CellLimits* const limits) const

已知单元格的边界框可有效计算裁剪限制。

如果栅格是空的，则offset=(0,0) limits=(1,1)

*offset = known_cells_box_.min().array();
*limits = CellLimits(known_cells_box_.sizes().x() + 1, known_cells_box_.sizes().y() + 1);

4.GrowLimits

void Grid2D::GrowLimits(const Eigen::Vector2f& point)

//根据需要增加地图以包含“点”。这改变了意义这些坐标向前发展。必须立即调用此方法在'FinishUpdate'之后，在对 'ApplyLookupTable'的任何调用之前。

GrowLimits(point, {mutable_correspondence_cost_cells()}, {kUnknownCorrespondenceValue});

void Grid2D::GrowLimits(const Eigen::Vector2f& point,
                        const std::vector<std::vector<uint16>*>& grids,
                        const std::vector<uint16>& grids_unknown_cell_values)

1. while (!limits_.Contains(limits_.GetCellIndex(point))) {

首先将point转化为Grid坐标系，然后判断该点是否在该坐标系下，不是则进入下面

2. 计算x_offset,y_offset. 当前Grid的长宽的一半

const int x_offset = limits_.cell_limits().num_x_cells / 2;
const int y_offset = limits_.cell_limits().num_y_cells / 2;

3.创建 MapLimits new_limits (limits_.resolution(),

limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset)，CellLimits(2倍的长宽))

4.stride Grid坐标系中 new_limits 的列数

offset =x_offset + stride * y_offset; 在新的Grid坐标系下原数据的起点

new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells;

5.将原来的Grid数据投到新的Grid上。

for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {

1>新建 std::vector<uint16> new_cells(new_size, grids_unknown_cell_values[grid_index]);

2>赋值根据二者关系赋值}

6.跟新边界的长宽 m_min += t; m_max += t; }//while循环结束

5.其他

1.FinishUpdate 完成更新顺序。

2.GetCorrespondenceCost 使用'cell_index'返回单元格的对应代价。

3.IsKnown 如果已知指定索引处的对应成本，则返回true。

4.ToFlatIndex 根据 (x,y)得出一维数组的下标

二、ProbabilityGrid 表示概率的2D网格。

class ProbabilityGrid : public Grid2D

1.数据成员

ValueConversionTables* conversion_tables_;

ValueConversionTables //与Grid类似

成员变量 // 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].

std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */, float /* upper_bound */>,
std::unique_ptr<const std::vector<float>>> bounds_to_lookup_table_;

成员函数：

const std::vector<float>* GetConversionTable(float unknown_result, float lower_bound, float upper_bound);

栅格图Grid2D构建的时，首先根据limits确定大小，并且值未知赋值给0。

2.构造

ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
                                 ValueConversionTables* conversion_tables)
    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
             conversion_tables),
      conversion_tables_(conversion_tables) {}

构造主要是进行了Grid的构造

3.SetProbability

void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                     const float probability)

将'cell_index'处的单元格的概率设置为给定的'概率'。只有在细胞未知之前才允许。

首先求出 cell_index 所对应存储的cell，然后检测cell是为赋值的，最后将probability赋值给保存值

同时跟新已知单元的box mutable_known_cells_box()->extend(cell_index.matrix());

4.ApplyLookupTable

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table)

应用调用ComputeLookupTableToApplyOdds（）时指定的'赔率'如果单元格尚未存在，则为“cell_index”处单元格的概率
已更新直到将忽略同一单元的多次更新调用FinishUpdate（）。如果单元格已更新，则返回true。
如果这是对指定单元格的第一次ApplyOdds（）调用，则为其值将被设置为与“赔率”相对应的概率。