盘锦做网站哪家好/网站在线客服系统源码

通常，初始化一个Grid Map时，地图不会很大，而是随着移动而实时的扩大地图。

cartographer中，地图增长函数是 gird_2d.cc文件中 GrowLimits。

我们还是从地图增长的源头(即插入rangedata to 地图)开始：

在文件 probability_grid_range_data_inserter_2d.cc 中，CastRays 函数内容如下：

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) {// 检查range_data是否在地图之外，如果是，则扩大地图     // 保证点肯定在地图之内  // GrowLimits函数在这个里面GrowAsNeeded(range_data, probability_grid);// 然后更新地图for (const sensor::RangefinderPoint& hit : range_data.returns) {ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);}}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];// 完成ODD更新DCHECK_GE(*cell, kUpdateMarker);mutable_known_cells_box()->extend(cell_index.matrix());return true;
}// Converts a 'cell_index' into an index into 'cells_'.int ToFlatIndex(const Eigen::Array2i& cell_index) const {
// 如果点超出子图范围，程序退出？ 不可能出现这种情况？
// 是的，不可能出现。因为 在这之前会调用 GrowAsNeeded 扩大地图CHECK(limits_.Contains(cell_index)) << cell_index; return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();}

可以发现，在进行插入动作之前，先进行了地图扩展操作。

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());while (!limits_.Contains(limits_.GetCellIndex(point))) {// xy边长分别扩大一倍const int x_offset = limits_.cell_limits().num_x_cells / 2;const int y_offset = limits_.cell_limits().num_y_cells / 2;//1. 设定new_limitconst 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;const int offset = x_offset + stride * y_offset;const int new_size = new_limits.cell_limits().num_x_cells *new_limits.cell_limits().num_y_cells;//2. 计算原图的概率值在新图中的位置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));}}
}