cartographer之GridInterfaceGrid_2dProbabilityGrid

Posted 2021-08-30 COCO_PEAK_NOODLE

GridInterface基本废弃

namespace cartographer {
namespace mapping {

class GridInterface {
  // todo(kdaun) move mutual functions of Grid2D/3D here
 public:
  virtual ~GridInterface() {}
};

}  // namespace mapping
}  // namespace cartographer

Grid2d类

class Grid2D : public GridInterface {
   public:
   Grid2D(const MapLimits& limits, float min_correspondence_cost,
          float max_correspondence_cost,
          ValueConversionTables* conversion_tables);
   explict Grid2D(const proto::Grid2D& proto,
                   ValueConversionTables* conversion_tables);
   const mapLimits& limits() const { return limits_;}
 
   void FinsihUpdate();
   float GetCorrespondenceCost(const Eigen::Aray2i& cell_index) const{}
 
   virtual GridType GetGridType() const = 0;
   float GetMinCorrespondenceCost() const {}
   float GetMaxCorrespondenceCost() const { return max_correspondence_cost_;};
   bool IsKnown(const Eigen::Array2i& cell_index) const {}
   void ComputeCroppedLimits(Eigen::Array2i* const offset,
                             CellLimits* const limits) const;
   virtual void GrowLimits(const Eigen::vector2f& point);    
   virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;
   virtual proto::Grid2D ToProto() const;
   virtual bool DraeToSubmapTexture(
      proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const = 0;
    
protectef:
  void GrowLimits(const Eigen::Vector2f& point,
                  const std::vector<std::vector<uint16>*>& grids,
                  const std::vector<uint16>& grids_unknown_cell_values);

 const std::vector<uint16>& correspondence_cost_cells(){
  return corresopondence_cost_cells;
}

  const std::vector<int>& update_indices() const { return update_indices_;}

   const Eigen::AlignedBox2i& known_cells_box() const{
    return known_cell_box_;
}

 std::vector<uint16>* mutable_correspondence_cost_cells() {
    return &correspondence_cost_cells_;
  }

  std::vector<int>* mutable_update_indices() { return &update_indices_; }
  Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }

  // Converts a 'cell_index' into an index into 'cells_'.
  int ToFlatIndex(const Eigen::Array2i& cell_index) const {
    CHECK(limits_.Contains(cell_index)) << cell_index;
    return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
  }

private:
  MapLimits limits_;
  std::vector<unit16> correspondence_cost_cells_;
  float min_correspondence_cost;
  float max_correspondence_cost;
  std::vector<int> update_indices_;
  
  Eigen::AlignedBox2i known_cell_box_;
  const std::vector<float>* value_to_correspondence_cost_table_;

}

这里面最重要的就是GrowLimits
它是如何扩展更新地图的呢？
细节见代码，效果如下图
在坐标系不变的情况下，向四周扩展了地图，扩展距离为原来地图的一半
对应的实现代码如下：

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))) {
    const int x_offset = limits_.cell_limits().num_x_cells / 2;
    const int y_offset = limits_.cell_limits().num_y_cells / 2;

    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;
    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;

    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));
    }
  }
}