catographer Submap

0. 重力對齊

• ‘gravity_alignment’ is used for the orientation of new submaps so that the z axis approximately aligns with gravity.

1. SubmapList

1.1 定義

• 只負責傳輸Submap列表，每個Submap項包括：
  • 版本號（即Scans數量）
  • Submap的位姿
• 通過發佈方式實現

message SubmapList {
  message SubmapEntry {
    int32 submap_version = 1;
    transform.proto.Rigid3d pose = 3;
  }
  message TrajectorySubmapList {
    repeated SubmapEntry submap = 1;
  }
  repeated TrajectorySubmapList trajectory = 2;
}

1.2 topic傳輸內容

$ rostopic type /submap_list | rosmsg show
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
cartographer_ros_msgs/TrajectorySubmapList[] trajectory
  cartographer_ros_msgs/SubmapEntry[] submap
    int32 submap_version
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

2. SubmapQuery

• 傳輸可顯示的地圖，地圖數據cells通過gzipped壓縮
• 通過查詢方式實現
• 定義

message SubmapQuery {
  message Request {
    // Index into 'SubmapList.trajectory(trajectory_id).submap'.
    optional int32 submap_index = 1;
    // Index into 'TrajectoryList.trajectory'.
    optional int32 trajectory_id = 2;
  }

  message Response {
    // Version of the given submap, higher means newer.
    // 插入到此Submap中的RangeData個數
    optional int32 submap_version = 2;　

    // GZipped map data, in row-major order, starting with (0,0). Each cell
    // consists of two bytes: value (premultiplied by alpha) and alpha.
    optional bytes cells = 3;

    // Dimensions of the grid in cells.
    // 概率值大於0.501的所有Cells圍成的最大box
    optional int32 width = 4;
    optional int32 height = 5;

    // Size of one cell in meters(高分辨率3D網格的分辨率).
    optional double resolution = 6;

    // Pose of the resolution*width x resolution*height rectangle in the submap
    // frame.
    optional transform.proto.Rigid3d slice_pose = 9;

    // Error message in response to malformed requests.
    optional string error_message = 8;
  }

  optional Request request = 1;
  optional Response response = 2;
}

4. 3D Submap相關數據定義

4.1 SubmapData

  struct SubmapData {
    const Submap* submap;
    transform::Rigid3d pose;
  };

4.2 Submap

• 個Submap中包含一個高分辨率和一個低分辨率的3D網格

class Submap : public mapping::Submap {
 public:
  Submap(float high_resolution, float low_resolution,
         const transform::Rigid3d& local_pose);

  const HybridGrid& high_resolution_hybrid_grid() const {
    return high_resolution_hybrid_grid_;
  }
  const HybridGrid& low_resolution_hybrid_grid() const {
    return low_resolution_hybrid_grid_;
  }
  const bool finished() const { return finished_; }

  void ToResponseProto(
      const transform::Rigid3d& global_submap_pose,
      mapping::proto::SubmapQuery::Response* response) const override;

 private:
  // TODO(hrapp): Remove friend declaration.
  friend class Submaps;

  HybridGrid high_resolution_hybrid_grid_;
  HybridGrid low_resolution_hybrid_grid_;
  bool finished_ = false;
};

4.3 Submaps

• 管理Submap的容器
• 把RangeData插入Submap的方法

class Submaps : public mapping::Submaps {
public:
 explicit Submaps(const proto::SubmapsOptions& options);

 Submaps(const Submaps&) = delete;
 Submaps& operator=(const Submaps&) = delete;

 const Submap* Get(int index) const override;
 int size() const override;

 // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
 // used for the orientation of new submaps so that the z axis approximately
 // aligns with gravity.
 void InsertRangeData(const sensor::RangeData& range_data,
                      const Eigen::Quaterniond& gravity_alignment);

private:
 void AddSubmap(const transform::Rigid3d& local_pose);

 const proto::SubmapsOptions options_;

 // 'submaps_' contains pointers, so that resizing the vector does not
 // invalidate handed out Submap* pointers.
 std::vector<std::unique_ptr<Submap>> submaps_;
 RangeDataInserter range_data_inserter_;
};

4.4 HybridGrid

• 包含使用15位存儲概率值的網格，還有1位用於存儲對應體素的更新標記

// A grid containing probability values stored using 15 bits, and an update
// marker per voxel.
class HybridGrid : public HybridGridBase<uint16> {
 public:
  explicit HybridGrid(const float resolution)
      : HybridGridBase<uint16>(resolution) {}

  explicit HybridGrid(const proto::HybridGrid& proto)
      : HybridGrid(proto.resolution()) {
    CHECK_EQ(proto.values_size(), proto.x_indices_size());
    CHECK_EQ(proto.values_size(), proto.y_indices_size());
    CHECK_EQ(proto.values_size(), proto.z_indices_size());

    for (int i = 0; i < proto.values_size(); ++i) {
      // SetProbability does some error checking for us.
      SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i),
                                     proto.z_indices(i)),
                     mapping::ValueToProbability(proto.values(i)));
    }
  }

  // Sets the probability of the cell at 'index' to the given 'probability'.
  void SetProbability(const Eigen::Array3i& index, const float probability) {
    *mutable_value(index) = mapping::ProbabilityToValue(probability);
  }

  // Starts the next update sequence.
  void StartUpdate() {
    while (!update_indices_.empty()) {
      DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker);
      *update_indices_.back() -= mapping::kUpdateMarker;
      update_indices_.pop_back();
    }
  }

  // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
  // to the probability of the cell at 'index' if the cell has not already been
  // updated. Multiple updates of the same cell will be ignored until
  // StartUpdate() is called. Returns true if the cell was updated.
  //
  // If this is the first call to ApplyOdds() for the specified cell, its value
  // will be set to probability corresponding to 'odds'.
  bool ApplyLookupTable(const Eigen::Array3i& index,
                        const std::vector<uint16>& table) {
    DCHECK_EQ(table.size(), mapping::kUpdateMarker);
    uint16* const cell = mutable_value(index);
    if (*cell >= mapping::kUpdateMarker) {
      return false;
    }
    update_indices_.push_back(cell);
    *cell = table[*cell];
    DCHECK_GE(*cell, mapping::kUpdateMarker);
    return true;
  }

  // Returns the probability of the cell with 'index'.
  float GetProbability(const Eigen::Array3i& index) const {
    return mapping::ValueToProbability(value(index));
  }

  // Returns true if the probability at the specified 'index' is known.
  bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; }

 private:
  // Markers at changed cells.
  std::vector<ValueType*> update_indices_;
};

4.5 HybridGridBase

• 將3D網格表示爲寬而淺的樹

// Represents a 3D grid as a wide, shallow tree.
template <typename ValueType>
class HybridGridBase : public Grid<ValueType> {
 public:
  using Iterator = typename Grid<ValueType>::Iterator;

  // Creates a new tree-based probability grid with voxels having edge length
  // 'resolution' around the origin which becomes the center of the cell at
  // index (0, 0, 0).
  explicit HybridGridBase(const float resolution) : resolution_(resolution) {}

  float resolution() const { return resolution_; }

  // Returns the index of the cell containing the 'point'. Indices are integer
  // vectors identifying cells, for this the coordinates are rounded to the next
  // multiple of the resolution.
  Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const {
    Eigen::Array3f index = point.array() / resolution_;
    return Eigen::Array3i(common::RoundToInt(index.x()),
                          common::RoundToInt(index.y()),
                          common::RoundToInt(index.z()));
  }

  // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
  static Eigen::Array3i GetOctant(const int i) {
    DCHECK_GE(i, 0);
    DCHECK_LT(i, 8);
    return Eigen::Array3i(static_cast<bool>(i & 1), static_cast<bool>(i & 2),
                          static_cast<bool>(i & 4));
  }

  // Returns the center of the cell at 'index'.
  Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const {
    return index.matrix().cast<float>() * resolution_;
  }

  // Iterator functions for range-based for loops.
  Iterator begin() const { return Iterator(*this); }

  Iterator end() const {
    Iterator it(*this);
    it.AdvanceToEnd();
    return it;
  }

 private:
  // Edge length of each voxel. (每個體素的邊長)
  const float resolution_;
};

4.6 Grid

• 在3D Submap中，ValueType爲uint16

template <typename ValueType>
using Grid = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;

4.7 DynamicGrid

• 此動態網格最初由“WrappedGrid”類型的2x2x2格子組成
• 通過’mutable_value（）'在第一次訪問時構造Wrapped grids
• 如有必要，網格增長到每個維度的兩倍大小
• 索引值範圍（幾乎）圍繞原點對稱，即允許負索引。

// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped
// grids are constructed on first access via 'mutable_value()'. If necessary,
// the grid grows to twice the size in each dimension. The range of indices is
// (almost) symmetric around the origin, i.e. negative indices are allowed.
template <typename WrappedGrid>
class DynamicGrid {
 public:
  using ValueType = typename WrappedGrid::ValueType;

  DynamicGrid() : bits_(1), meta_cells_(8) {}
  DynamicGrid(DynamicGrid&&) = default;
  DynamicGrid& operator=(DynamicGrid&&) = default;

  // Returns the current number of voxels per dimension.
  // 返回每個維度的當前體素數
  int grid_size() const { return WrappedGrid::grid_size() << bits_; }

  // Returns the value stored at 'index'.
  ValueType value(const Eigen::Array3i& index) const {
    const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
    // The cast to unsigned is for performance to check with 3 comparisons
    // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
    if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
      return ValueType();
    }
    const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
    const WrappedGrid* const meta_cell =
        meta_cells_[ToFlatIndex(meta_index, bits_)].get();
    if (meta_cell == nullptr) {
      return ValueType();
    }
    const Eigen::Array3i inner_index =
        shifted_index - meta_index * WrappedGrid::grid_size();
    return meta_cell->value(inner_index);
  }

  // Returns a pointer to the value at 'index' to allow changing it, dynamically
  // growing the DynamicGrid and constructing new WrappedGrids as needed.
  ValueType* mutable_value(const Eigen::Array3i& index) {
    const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
    // The cast to unsigned is for performance to check with 3 comparisons
    // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
    if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
      Grow();
      return mutable_value(index);
    }
    const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
    std::unique_ptr<WrappedGrid>& meta_cell =
        meta_cells_[ToFlatIndex(meta_index, bits_)];
    if (meta_cell == nullptr) {
      meta_cell = common::make_unique<WrappedGrid>();
    }
    const Eigen::Array3i inner_index =
        shifted_index - meta_index * WrappedGrid::grid_size();
    return meta_cell->mutable_value(inner_index);
  }

  // An iterator for iterating over all values not comparing equal to the
  // default constructed value.
  class Iterator {
   public:
    explicit Iterator(const DynamicGrid& dynamic_grid)
        : bits_(dynamic_grid.bits_),
          current_(dynamic_grid.meta_cells_.data()),
          end_(dynamic_grid.meta_cells_.data() +
               dynamic_grid.meta_cells_.size()),
          nested_iterator_() {
      AdvanceToValidNestedIterator();
    }

    void Next() {
      DCHECK(!Done());
      nested_iterator_.Next();
      if (!nested_iterator_.Done()) {
        return;
      }
      ++current_;
      AdvanceToValidNestedIterator();
    }

    bool Done() const { return current_ == end_; }

    Eigen::Array3i GetCellIndex() const {
      DCHECK(!Done());
      const int outer_index = (1 << (3 * bits_)) - (end_ - current_);
      const Eigen::Array3i shifted_index =
          To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() +
          nested_iterator_.GetCellIndex();
      return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size());
    }

    const ValueType& GetValue() const {
      DCHECK(!Done());
      return nested_iterator_.GetValue();
    }

    void AdvanceToEnd() { current_ = end_; }

    const std::pair<Eigen::Array3i, ValueType> operator*() const {
      return std::pair<Eigen::Array3i, ValueType>(GetCellIndex(), GetValue());
    }

    Iterator& operator++() {
      Next();
      return *this;
    }

    bool operator!=(const Iterator& it) const {
      return it.current_ != current_;
    }

   private:
    void AdvanceToValidNestedIterator() {
      for (; !Done(); ++current_) {
        if (*current_ != nullptr) {
          nested_iterator_ = typename WrappedGrid::Iterator(**current_);
          if (!nested_iterator_.Done()) {
            break;
          }
        }
      }
    }

    int bits_;
    const std::unique_ptr<WrappedGrid>* current_;
    const std::unique_ptr<WrappedGrid>* const end_;
    typename WrappedGrid::Iterator nested_iterator_;
  };

 private:
  // Returns the Eigen::Array3i (meta) index of the meta cell containing
  // 'index'. (返回包含'index'的元單元格的Eigen :: Array3i（meta）索引)
  Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
    DCHECK((index >= 0).all()) << index;
    const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
    DCHECK((meta_index < (1 << bits_)).all()) << index;
    return meta_index;
  }

  // Grows this grid by a factor of 2 in each of the 3 dimensions.
  // 在3個維度的每個維度中將此網格增長2倍
  void Grow() {
    const int new_bits = bits_ + 1;
    CHECK_LE(new_bits, 8);
    std::vector<std::unique_ptr<WrappedGrid>> new_meta_cells_(
        8 * meta_cells_.size());
    for (int z = 0; z != (1 << bits_); ++z) {
      for (int y = 0; y != (1 << bits_); ++y) {
        for (int x = 0; x != (1 << bits_); ++x) {
          const Eigen::Array3i original_meta_index(x, y, z);
          const Eigen::Array3i new_meta_index =
              original_meta_index + (1 << (bits_ - 1));
          new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] =
              std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]);
        }
      }
    }
    meta_cells_ = std::move(new_meta_cells_);
    bits_ = new_bits;
  }

  int bits_;
  std::vector<std::unique_ptr<WrappedGrid>> meta_cells_;
};

4.8 NestedGrid

• NestedGrid有$2^{kBits}*2^{kBits}*2^{kBits}$個WrappedGrid類型的格子
• 每個WrappedGrid類型的格子在通過函數mutable_value()訪問時創建

// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type
// 'WrappedGrid'. Wrapped grids are constructed on first access via
// 'mutable_value()'.
template <typename WrappedGrid, int kBits>
class NestedGrid {
 public:
  using ValueType = typename WrappedGrid::ValueType;

  // Returns the number of voxels per dimension.
  static int grid_size() { return WrappedGrid::grid_size() << kBits; }

  // Returns the value stored at 'index', each dimension of 'index' being
  // between 0 and grid_size() - 1.
  ValueType value(const Eigen::Array3i& index) const {
    const Eigen::Array3i meta_index = GetMetaIndex(index);
    const WrappedGrid* const meta_cell =
        meta_cells_[ToFlatIndex(meta_index, kBits)].get();
    if (meta_cell == nullptr) {
      return ValueType();
    }
    const Eigen::Array3i inner_index =
        index - meta_index * WrappedGrid::grid_size();
    return meta_cell->value(inner_index);
  }

  // Returns a pointer to the value at 'index' to allow changing it. If
  // necessary a new wrapped grid is constructed to contain that value.
  ValueType* mutable_value(const Eigen::Array3i& index) {
    const Eigen::Array3i meta_index = GetMetaIndex(index);
    std::unique_ptr<WrappedGrid>& meta_cell =
        meta_cells_[ToFlatIndex(meta_index, kBits)];
    if (meta_cell == nullptr) {
      meta_cell = common::make_unique<WrappedGrid>();
    }
    const Eigen::Array3i inner_index =
        index - meta_index * WrappedGrid::grid_size();
    return meta_cell->mutable_value(inner_index);
  }

  // An iterator for iterating over all values not comparing equal to the
  // default constructed value.
  class Iterator {
   public:
    Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {}

    explicit Iterator(const NestedGrid& nested_grid)
        : current_(nested_grid.meta_cells_.data()),
          end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()),
          nested_iterator_() {
      AdvanceToValidNestedIterator();
    }

    void Next() {
      DCHECK(!Done());
      nested_iterator_.Next();
      if (!nested_iterator_.Done()) {
        return;
      }
      ++current_;
      AdvanceToValidNestedIterator();
    }

    bool Done() const { return current_ == end_; }

    Eigen::Array3i GetCellIndex() const {
      DCHECK(!Done());
      const int index = (1 << (3 * kBits)) - (end_ - current_);
      return To3DIndex(index, kBits) * WrappedGrid::grid_size() +
             nested_iterator_.GetCellIndex();
    }

    const ValueType& GetValue() const {
      DCHECK(!Done());
      return nested_iterator_.GetValue();
    }

   private:
    void AdvanceToValidNestedIterator() {
      for (; !Done(); ++current_) {
        if (*current_ != nullptr) {
          nested_iterator_ = typename WrappedGrid::Iterator(**current_);
          if (!nested_iterator_.Done()) {
            break;
          }
        }
      }
    }

    const std::unique_ptr<WrappedGrid>* current_;
    const std::unique_ptr<WrappedGrid>* end_;
    typename WrappedGrid::Iterator nested_iterator_;
  };

 private:
  // Returns the Eigen::Array3i (meta) index of the meta cell containing
  // 'index'.
  Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
    DCHECK((index >= 0).all()) << index;
    const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
    DCHECK((meta_index < (1 << kBits)).all()) << index;
    return meta_index;
  }

  std::array<std::unique_ptr<WrappedGrid>, 1 << (3 * kBits)> meta_cells_;
};

4.9 FlatGrid

• FlatGrid是一個包含$2^{kBits}*2^{kBits}*2^{kBits}$個存儲ValueType值的連續內存塊
• 每一維的索引從0開始

// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based.
template <typename TValueType, int kBits>
class FlatGrid {
 public:
  using ValueType = TValueType;

  // Creates a new flat grid with all values being default constructed.
  FlatGrid() {
    for (ValueType& value : cells_) {
      value = ValueType();
    }
  }

  FlatGrid(const FlatGrid&) = delete;
  FlatGrid& operator=(const FlatGrid&) = delete;

  // Returns the number of voxels per dimension.
  static int grid_size() { return 1 << kBits; }

  // Returns the value stored at 'index', each dimension of 'index' being
  // between 0 and grid_size() - 1.
  ValueType value(const Eigen::Array3i& index) const {
    return cells_[ToFlatIndex(index, kBits)];
  }

  // Returns a pointer to a value to allow changing it.
  ValueType* mutable_value(const Eigen::Array3i& index) {
    return &cells_[ToFlatIndex(index, kBits)];
  }

  // An iterator for iterating over all values not comparing equal to the
  // default constructed value.
  class Iterator {
   public:
    Iterator() : current_(nullptr), end_(nullptr) {}

    explicit Iterator(const FlatGrid& flat_grid)
        : current_(flat_grid.cells_.data()),
          end_(flat_grid.cells_.data() + flat_grid.cells_.size()) {
      while (!Done() && IsDefaultValue(*current_)) {
        ++current_;
      }
    }

    void Next() {
      DCHECK(!Done());
      do {
        ++current_;
      } while (!Done() && IsDefaultValue(*current_));
    }

    bool Done() const { return current_ == end_; }

    Eigen::Array3i GetCellIndex() const {
      DCHECK(!Done());
      const int index = (1 << (3 * kBits)) - (end_ - current_);
      return To3DIndex(index, kBits);
    }

    const ValueType& GetValue() const {
      DCHECK(!Done());
      return *current_;
    }

   private:
    const ValueType* current_;
    const ValueType* end_;
  };

 private:
  std::array<ValueType, 1 << (3 * kBits)> cells_;
};

5 三維與一維互轉

5.1 三維轉一維

• z:在高位
• y:在中間
• x:在低位
• 若bits爲3,　則一維數組索引格式爲：z(3bits) y(3bits) x(3bits)，共9bits

// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat
// z-major index.
inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) {
  DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index;
  return (((index.z() << bits) + index.y()) << bits) + index.x();
}

5.2 一維轉三維

// Converts a flat z-major 'index' to a 3-dimensional index with each dimension
// from 0 to 2^'bits' - 1.
inline Eigen::Array3i To3DIndex(const int index, const int bits) {
  DCHECK_LT(index, 1 << (3 * bits));
  const int mask = (1 << bits) - 1;
  return Eigen::Array3i(index & mask, (index >> bits) & mask,
                        (index >> bits) >> bits);
}

6. 把3D網格轉換爲rviz顯示的地圖

• 採用高分辨率3D網絡地圖
• 實現代碼

void Submap::ToResponseProto(
   const transform::Rigid3d& global_submap_pose,
   mapping::proto::SubmapQuery::Response* const response) const {
 // 版本號爲插入Submap中的RangeData數量
 response->set_submap_version(num_range_data());
 
 // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
 // in the global map frame.
 const float resolution = high_resolution_hybrid_grid_.resolution();
 response->set_resolution(resolution);

 // Compute a bounding box for the texture.
 Eigen::Array2i min_index(INT_MAX, INT_MAX);
 Eigen::Array2i max_index(INT_MIN, INT_MIN);
 // 把HybridGrid中所有概率值大於0.501f的Voxel取出來並放於列表中
 // 同時計算在x-y平面上的最大矩形空間
 const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
     ExtractVoxelData(high_resolution_hybrid_grid_,
                      global_submap_pose.cast<float>(), &min_index,
                      &max_index);
 // 計算在x-y平面上的矩形寬度和高度 　
 const int width = max_index.y() - min_index.y() + 1;
 const int height = max_index.x() - min_index.x() + 1;
 response->set_width(width);
 response->set_height(height);
 // 計算X-Y平面上每個像素的統計信息
 const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
     width, height, min_index, max_index, voxel_indices_and_probabilities);
 //  計算每一個像素的value和alpha，各以8bit保存     
 const string cell_data = ComputePixelValues(accumulated_pixel_data);
 // 壓縮生成的圖像數據
 common::FastGzipString(cell_data, response->mutable_cells());
 // 計算此矩形框在Submap中的局部位姿
 *response->mutable_slice_pose() = transform::ToProto(
     global_submap_pose.inverse() *
     transform::Rigid3d::Translation(Eigen::Vector3d(
         max_index.x() * resolution, max_index.y() * resolution,
         global_submap_pose.translation().z())));
}

• PiexlData數據
  • 定義X-Y平面上的一個像素，在HybridGrid中，與此像素對應的Z軸上有多個Voxels，每個Voxel有一個對應的概率值（且此概率值大於0.501）

struct PixelData {
int min_z = INT_MAX; // 所有Z軸上Voxels的z值最小的 (voxel索引)
int max_z = INT_MIN; // 所有Z軸上Voxels的z值最大的 (voxel索引)
int count = 0;  // 所有Z軸上Voxels的個數
float probability_sum = 0.f;　// 所有Z軸上Voxels的概率值之和
float max_probability = 0.5f; // 所有Z軸上Voxels的概率最大值
};

• 版本號爲插入Submap中的RangeData數量
• 把HybridGrid中所有概率值大於0.501的Voxel取出來，把這些Voxel的索引及對應的概率放於列表中，同時計算在x-y平面上的最大矩形空間 (std::vectorEigen::Array4i)
• 計算X-Y平面上每個像素的統計信息std::vector, 元素個數 $width * height$
• 計算每一個像素的value和alpha，各以8bit保存, 元素個數$width * height$，結果保存在string中
• 採用gzip壓縮生成的圖像數據
• 計算此矩形框在Submap中的局部位姿