Cartographer源码阅读2D-前端Submap生成及数据插入

Cartographer源码阅读-2D前端Submap生成及数据插入

前端CSM位姿计算后，将激光点云转换到Local SLAM的坐标系下，插入submaps中，该submaps指的是前端LocalTrajectoryBuilder2D中维护的ActiveSubmap2d。

ActiveSubmap2D

class ActiveSubmaps2D {
 public:
  explicit ActiveSubmaps2D(const proto::SubmapsOptions2D& options);

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

  // Returns the index of the newest initialized Submap which can be
  // used for scan-to-map matching.
  int matching_index() const;

  // Inserts 'range_data' into the Submap collection.
  // 入口函数，插入点云数据
  void InsertRangeData(const sensor::RangeData& range_data);
  // 获取submap数据，用于CSM匹配
  std::vector<std::shared_ptr<Submap2D>> submaps() const;

 private:
  // 生成插入器
  std::unique_ptr<RangeDataInserterInterface> CreateRangeDataInserter();
  // 生成网格，构造submap2d
  std::unique_ptr<GridInterface> CreateGrid(const Eigen::Vector2f& origin);
  // 如果submap满了，则Finish submap
  void FinishSubmap();
  // 添加新的submap
  void AddSubmap(const Eigen::Vector2f& origin);
  
  const proto::SubmapsOptions2D options_;
  int matching_submap_index_ = 0;
  // 类内维护的2个submap2d
  std::vector<std::shared_ptr<Submap2D>> submaps_;
  // 插入器
  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
};

接口函数：将数据插入submap，并管理是否新建submap或者finish submap

void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
  for (auto& submap : submaps_) {
	// 2个submap分别插入数据      
    submap->InsertRangeData(range_data, range_data_inserter_.get());    
  }
  // 如果最新的submap的数据量达到一半，则将第一个submap Finish并创建新的submap
  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
}

创建新的submap：

void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  if (submaps_.size() > 1) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    // 结束最老的submap：即第一个submap
    FinishSubmap();
  }
   // 创建新的submap，submap的localpose为第一帧的中心位置（在local slam坐标系下），不带角度
    submaps_.push_back(common::make_unique<Submap2D>(
      origin, std::unique_ptr<Grid2D>(
                  // 构造初始网格数据
                  static_cast<Grid2D*>(CreateGrid(origin).release()))));
  LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
}

构造网格数据：

// 构造网格数据
std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
    const Eigen::Vector2f& origin) {
  constexpr int kInitialSubmapSize = 100;
  float resolution = options_.grid_options_2d().resolution();
  // 后面再看MapLimits和CellLimits
  return common::make_unique<ProbabilityGrid>(
      MapLimits(resolution,
                origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
                                            Eigen::Vector2d::Ones(),
                CellLimits(kInitialSubmapSize, kInitialSubmapSize)));
}

结束submap的创建：

void ActiveSubmaps2D::FinishSubmap() {
  // 结束最老的submap
  Submap2D* submap = submaps_.front().get();
  submap->Finish();
  ++matching_submap_index_;
  // 删除最老的submap
  submaps_.erase(submaps_.begin());
}

Submap2D

class Submap {
 public:
  Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}
  virtual ~Submap() {}

  virtual void ToProto(proto::Submap* proto,
                       bool include_probability_grid_data) const = 0;
  virtual void UpdateFromProto(const proto::Submap& proto) = 0;

  // Fills data into the 'response'.
  virtual void ToResponseProto(
      const transform::Rigid3d& global_submap_pose,
      proto::SubmapQuery::Response* response) const = 0;

  // Pose of this submap in the local map frame.
  transform::Rigid3d local_pose() const { return local_pose_; }

  // Number of RangeData inserted.
  int num_range_data() const { return num_range_data_; }
  void set_num_range_data(const int num_range_data) {
    num_range_data_ = num_range_data;
  }
  
  // Whether the submap is finished or not.
  bool finished() const { return finished_; }
  void set_finished(bool finished) { finished_ = finished; }

 private:
  const transform::Rigid3d local_pose_;
  int num_range_data_ = 0;
  bool finished_ = false;

};

class Submap2D : public Submap {
 public:
  // 构造函数
  Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid);
  // 将 proto转换为Submap2D
  explicit Submap2D(const proto::Submap2D& proto);
  // 写入proto
  void ToProto(proto::Submap* proto,
               bool include_probability_grid_data) const override;
  // 从Proto中解析submap
  void UpdateFromProto(const proto::Submap& proto) override;
  // 其他的程序(显示或保存)通过服务调用submap的数据
  void ToResponseProto(const transform::Rigid3d& global_submap_pose,
                       proto::SubmapQuery::Response* response) const override;
  // 获取网格数据
  const Grid2D* grid() const { return grid_.get(); }

  // Insert 'range_data' into this submap using 'range_data_inserter'. The
  // submap must not be finished yet.
  // 插入点云数据
  void InsertRangeData(const sensor::RangeData& range_data,
                       const RangeDataInserterInterface* range_data_inserter);
  // 结束submap
  void Finish();

 private:
  // 网格数据
  std::unique_ptr<Grid2D> grid_;
};

void Submap2D::InsertRangeData(
    const sensor::RangeData& range_data,
    const RangeDataInserterInterface* range_data_inserter) {
  CHECK(grid_);
  CHECK(!finished());
  // 将点云插入网格
  range_data_inserter->Insert(range_data, grid_.get());
  // 计数
  set_num_range_data(num_range_data() + 1);
}

void Submap2D::Finish() {
  CHECK(grid_);
  CHECK(!finished());
  // 网格裁剪
  grid_ = grid_->ComputeCroppedGrid();
  set_finished(true);
}

Grid2D

在submap里维护了grid，看一下Grid2D类：

在网格里存放着：网格的大小信息、每个网格的概率值（Odd），并可以调用相应函数更新网格的概率值，进行网格的裁剪操作。

class Grid2D : public GridInterface {
 public:
  explicit Grid2D(const MapLimits& limits, float min_correspondence_cost,
                  float max_correspondence_cost);
  explicit Grid2D(const proto::Grid2D& proto);

  // Returns the limits of this Grid2D.
  const MapLimits& limits() const { return limits_; }

  // Finishes the update sequence.
  void FinishUpdate();
  // Returns the correspondence cost of the cell with 'cell_index'.
  float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;

  // Returns the minimum possible correspondence cost.
  float GetMinCorrespondenceCost() const { return min_correspondence_cost_; }

  // Returns the maximum possible correspondence cost.
  float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; }

  // Returns true if the probability at the specified index is known.
  bool IsKnown(const Eigen::Array2i& cell_index) const;

  // Fills in 'offset' and 'limits' to define a subregion of that contains all
  // known cells.
  void ComputeCroppedLimits(Eigen::Array2i* const offset,
                            CellLimits* const limits) const;

  // Grows the map as necessary to include 'point'. This changes the meaning of
  // these coordinates going forward. This method must be called immediately
  // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
  virtual void GrowLimits(const Eigen::Vector2f& point);

  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;

  virtual proto::Grid2D ToProto() const;

  virtual bool DrawToSubmapTexture(
      proto::SubmapQuery::Response::SubmapTexture* const texture,
      transform::Rigid3d local_pose) const = 0;

 protected:
  const std::vector<uint16>& correspondence_cost_cells() const {
    return correspondence_cost_cells_;
  }
  const std::vector<int>& update_indices() const { return update_indices_; }
  const Eigen::AlignedBox2i& known_cells_box() const {
    return known_cells_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;

 private:
  // 地图的信息
  MapLimits limits_;
  // 地图中每个网格的概率值
  std::vector<uint16> correspondence_cost_cells_;
  // 地图概率最小值
  float min_correspondence_cost_;
  // 地图概率最大值
  float max_correspondence_cost_;
  // 记录被更新的栅格索引
  std::vector<int> update_indices_;

  // Bounding box of known cells to efficiently compute cropping limits.
  // 已知概率的cells的盒子，方便做裁剪
  Eigen::AlignedBox2i known_cells_box_;
};

class ProbabilityGrid : public Grid2D {
 public:
  explicit ProbabilityGrid(const MapLimits& limits);
  explicit ProbabilityGrid(const proto::Grid2D& proto);

  // Sets the probability of the cell at 'cell_index' to the given
  // 'probability'. Only allowed if the cell was unknown before.
  void SetProbability(const Eigen::Array2i& cell_index,
                      const float probability);

  // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
  // to the probability of the cell at 'cell_index' if the cell has not already
  // been updated. Multiple updates of the same cell will be ignored until
  // FinishUpdate() 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::Array2i& cell_index,
                        const std::vector<uint16>& table);

  // Returns the probability of the cell with 'cell_index'.
  float GetProbability(const Eigen::Array2i& cell_index) const;

  virtual proto::Grid2D ToProto() const override;
  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
  virtual bool DrawToSubmapTexture(
      proto::SubmapQuery::Response::SubmapTexture* const texture,
      transform::Rigid3d local_pose) const override;
};

为保证一帧数据，光线经过的网格只被更新一次，所以设置了一个更新的标记，如果该网格被更新过，则加上标记，后面在查表的时候，该位置处的概率不会被更新，因此，需要设置更新标记，同时也要保证被查的表格是根据Int值生成了两份。

// 对于原本是未知的，只需要减去kUpdateMarker即可
void Grid2D::FinishUpdate() {
  while (!update_indices_.empty()) {
    DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
              kUpdateMarker);
    // 因为被更新的会加上kUpdateMarker，所以
    correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
    update_indices_.pop_back();
  }
}

扩展边界，更新坐标系：

void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
  CHECK(update_indices_.empty());
  // 如果在原来的地图网格内，则不扩展，否则扩展2倍大小
  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;
    // 生成新的maplimits
    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;
    std::vector<uint16> new_cells(new_size, kUnknownCorrespondenceValue);
    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] =
            correspondence_cost_cells_[j +
                                       i * limits_.cell_limits().num_x_cells];
      }
    }
    correspondence_cost_cells_ = new_cells;
    limits_ = new_limits;
    if (!known_cells_box_.isEmpty()) {
      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
    }
  }
}

查表更新概率：

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  // xy坐标转换为vector内的坐标
  const int flat_index = ToFlatIndex(cell_index);
  // 取出该位置的free概率
  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);
  mutable_known_cells_box()->extend(cell_index.matrix());
  return true;
}

网格裁剪：

std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 裁剪cell limits更新offset
  ComputeCroppedLimits(&offset, &cell_limits);
  const double resolution = limits().resolution();
  // 更新Max所在的位置
  const Eigen::Vector2d max =
      limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
  // 创建新的裁剪网格
  std::unique_ptr<ProbabilityGrid> cropped_grid =
      common::make_unique<ProbabilityGrid>(
          // 更新maplimits
          MapLimits(resolution, max, cell_limits));
  // 裁剪网格赋值
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    if (!IsKnown(xy_index + offset)) continue;
    cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
  }

  return std::unique_ptr<Grid2D>(cropped_grid.release());
}
void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
                                  CellLimits* const limits) const {
  if (known_cells_box_.isEmpty()) {
    *offset = Eigen::Array2i::Zero();
    *limits = CellLimits(1, 1);
    return;
  }
  *offset = known_cells_box_.min().array();
  *limits = CellLimits(known_cells_box_.sizes().x() + 1,
                       known_cells_box_.sizes().y() + 1);
}

设置网格概率：

void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                     const float probability) {
  // 取出free概率值
  uint16& cell =
      (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
  CHECK_EQ(cell, kUnknownProbabilityValue);
  // 更新该位置的概率
  cell =
      CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
  mutable_known_cells_box()->extend(cell_index.matrix());
}

获取网格概率：

// 获取cell_index处的hit概率值
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
  if (!limits().Contains(cell_index)) return kMinProbability;
  return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
      correspondence_cost_cells()[ToFlatIndex(cell_index)]));
}

关于为什么会有offset的变动：

因为submap坐标系的原点一直都是左上角（真实世界里最远的点），所以submap坐标系的原点一直在变动。所以根据cell的Index来计算真实点，及根据真实点来计算cell的Index也是显而易见的。其具体的计算都在MapLimits类里。

MapLimits类

class MapLimits {
 public:
  MapLimits(const double resolution, const Eigen::Vector2d& max,
            const CellLimits& cell_limits)
      : resolution_(resolution), max_(max), cell_limits_(cell_limits) {
    CHECK_GT(resolution_, 0.);
    CHECK_GT(cell_limits.num_x_cells, 0.);
    CHECK_GT(cell_limits.num_y_cells, 0.);
  }

  explicit MapLimits(const proto::MapLimits& map_limits)
      : resolution_(map_limits.resolution()),
        max_(transform::ToEigen(map_limits.max())),
        cell_limits_(map_limits.cell_limits()) {}

  // Returns the cell size in meters. All cells are square and the resolution is
  // the length of one side.
  double resolution() const { return resolution_; }

  // Returns the corner of the limits, i.e., all pixels have positions with
  // smaller coordinates.
  const Eigen::Vector2d& max() const { return max_; }

  // Returns the limits of the grid in number of cells.
  const CellLimits& cell_limits() const { return cell_limits_; }

  // Returns the index of the cell containing the 'point' which may be outside
  // the map, i.e., negative or too large indices that will return false for
  // Contains().
  // 根据真实值计算cell index，显然，根据cell index计算真实值的公式，也是知道的。
  Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
    // Index values are row major and the top left has Eigen::Array2i::Zero()
    // and contains (centered_max_x, centered_max_y). We need to flip and
    // rotate.
    return Eigen::Array2i(
        common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
        common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
  }

  // Returns true if the ProbabilityGrid contains 'cell_index'.
  bool Contains(const Eigen::Array2i& cell_index) const {
    return (Eigen::Array2i(0, 0) <= cell_index).all() &&
           (cell_index <
            Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
               .all();
  }

 private:
  // 地图分辨率
  double resolution_;
  // 地图中xy方向的最大值（左上角），地图最左上角的xy索引为（0,0）
  Eigen::Vector2d max_;
  // 栅格化后，xy方向上的栅格数量
  CellLimits cell_limits_;
};

概率&odd相关计算：

constexpr float kMinProbability = 0.1f;  // 最小概率0.1
constexpr float kMaxProbability = 1.f - kMinProbability; // 最大概率0.9
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 最小free概率
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 最大free概率
constexpr uint16 kUnknownProbabilityValue = 0;  // 未知概率为0
constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue; // 未知free概率为0
constexpr uint16 kUpdateMarker = 1u << 15; // 32768 加上整数会越界，如果越界，则可以判断为被更新过
//由Probability计算odds
inline float Odds(float probability) {
  return probability / (1.f - probability);
}
//由odds计算probability
inline float ProbabilityFromOdds(const float odds) {
  return odds / (odds + 1.f);
}
//Probability转CorrespondenceCost
inline float ProbabilityToCorrespondenceCost(const float probability) {
  return 1.f - probability;
}
// CorrespondenceCost转Probability
inline float CorrespondenceCostToProbability(const float correspondence_cost) {
  return 1.f - correspondence_cost;
}
//限制概率区间
// Clamps probability to be in the range [kMinProbability, kMaxProbability].
inline float ClampProbability(const float probability) {
  return common::Clamp(probability, kMinProbability, kMaxProbability);
}
// Clamps correspondece cost to be in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ClampCorrespondenceCost(const float correspondence_cost) {
  return common::Clamp(correspondence_cost, kMinCorrespondenceCost,
                       kMaxCorrespondenceCost);
}
// float映射到int
inline uint16 BoundedFloatToValue(const float float_value,
                                  const float lower_bound,
                                  const float upper_bound) {
  const int value =
      common::RoundToInt(
          (common::Clamp(float_value, lower_bound, upper_bound) - lower_bound) *
          (32766.f / (upper_bound - lower_bound))) +
      1;
  // DCHECK for performance.
  DCHECK_GE(value, 1);
  DCHECK_LE(value, 32767);
  return value;
}

// Converts a correspondence_cost to a uint16 in the [1, 32767] range.
inline uint16 CorrespondenceCostToValue(const float correspondence_cost) {
  return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost,
                             kMaxCorrespondenceCost);
}

// Converts a probability to a uint16 in the [1, 32767] range.
inline uint16 ProbabilityToValue(const float probability) {
  return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
}

// 整数转换为Probability和CorrespondenceCost的表
extern const std::vector<float>* const kValueToProbability;
extern const std::vector<float>* const kValueToCorrespondenceCost;

// Converts a uint16 (which may or may not have the update marker set) to a
// probability in the range [kMinProbability, kMaxProbability].
inline float ValueToProbability(const uint16 value) {
  return (*kValueToProbability)[value];
}

// Converts a uint16 (which may or may not have the update marker set) to a
// correspondence cost in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ValueToCorrespondenceCost(const uint16 value) {
  return (*kValueToCorrespondenceCost)[value];
}

// Probability的Value转成CorrespondenceCost的Value:
inline uint16 ProbabilityValueToCorrespondenceCostValue(
    uint16 probability_value) {
   //如果是Unknown值还返回unknown值。Probability和CorrespondenceCost的Unknown值都是0
  if (probability_value == kUnknownProbabilityValue) {
    return kUnknownCorrespondenceValue;
  }
  //如果该值超过最大范围
  bool update_carry = false;
  if (probability_value > kUpdateMarker) {
    probability_value -= kUpdateMarker;//防止溢出范围
    update_carry = true;
  }
  // ProbabilityValue-->Probability-->CorrespondenceCost-->CorrespondenceCostValue
  uint16 result = CorrespondenceCostToValue(
      ProbabilityToCorrespondenceCost(ValueToProbability(probability_value)));
  if (update_carry) result += kUpdateMarker;//原先减去过一个最大范围，现在再加回来
  return result;
}
// 类似以上
inline uint16 CorrespondenceCostValueToProbabilityValue(
    uint16 correspondence_cost_value) {
  if (correspondence_cost_value == kUnknownCorrespondenceValue)
    return kUnknownProbabilityValue;
  bool update_carry = false;
  if (correspondence_cost_value > kUpdateMarker) {
    correspondence_cost_value -= kUpdateMarker;
    update_carry = true;
  }
  uint16 result = ProbabilityToValue(CorrespondenceCostToProbability(
      ValueToCorrespondenceCost(correspondence_cost_value)));
  if (update_carry) result += kUpdateMarker;
  return result;
}

//在计算时并不是用浮点数进行的计算，二是将0~1的概率值映射到1_32767的整数值
// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value,
                              const float unknown_result,
                              const float lower_bound,
                              const float upper_bound) {
  CHECK_GE(value, 0);
  CHECK_LE(value, 32767);
  if (value == unknown_value) return unknown_result;
  const float kScale = (upper_bound - lower_bound) / 32766.f;
  return value * kScale + (lower_bound - kScale);
}

// 计算将整数转换为概率的表，重复了2遍，保证了加不加updateMarker，仍然能够取得其概率值
std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(
    const uint16 unknown_value, const float unknown_result,
    const float lower_bound, const float upper_bound) {
  auto result = common::make_unique<std::vector<float>>();
  // Repeat two times, so that both values with and without the update marker
  // can be converted to a probability.
  for (int repeat = 0; repeat != 2; ++repeat) {
    for (int value = 0; value != 32768; ++value) {
      result->push_back(SlowValueToBoundedFloat(
          value, unknown_value, unknown_result, lower_bound, upper_bound));
    }
  }
  return result;
}

// 整数转Probability的表生成
std::unique_ptr<std::vector<float>> PrecomputeValueToProbability() {
  return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue,
                                       kMinProbability, kMinProbability,
                                       kMaxProbability);
}
// 整数转CorrespondenceCost的表生成
std::unique_ptr<std::vector<float>> PrecomputeValueToCorrespondenceCost() {
  return PrecomputeValueToBoundedFloat(
      kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
      kMinCorrespondenceCost, kMaxCorrespondenceCost);
}

// 构造用来更新odd的表，其中odd是Probability的odd
std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
  std::vector<uint16> result;
  result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) +
                   kUpdateMarker);
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(ProbabilityToValue(ProbabilityFromOdds(
                         odds * Odds((*kValueToProbability)[cell]))) +
                     kUpdateMarker);
  }
  return result;
}

// 构造用来更新odd的表，其中odd是CorrespondenceCostOdds，在submap中的grid存放的都是CorrespondenceCostOdds，因此，在查表时，主要查的表为该表
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
    float odds) {
  std::vector<uint16> result;
   //这个表示这个表中的第一个元素对应了如果之前该点是unknown状态，更新的value应该是什么,所以加了一个kUpdateMarker
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds))) +
                   kUpdateMarker);
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]))))) +
        kUpdateMarker);
  }
  return result;
}

点云数据的插入器：

在构造ActiveSubmaps2D时，构造了插入器ProbabilityGridRangeDataInserter2D，主要算法是RayCasting（光线跟踪算法），连接激光雷达中心点和扫描得到的障碍物点，该条光线经过的位置都是可通行区域。

class RangeDataInserterInterface {
 public:
  // Inserts 'range_data' into 'grid'.
  virtual void Insert(const sensor::RangeData& range_data,
                      GridInterface* grid) const = 0;
};

class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface {
 public:
  explicit ProbabilityGridRangeDataInserter2D(
      const proto::ProbabilityGridRangeDataInserterOptions2D& options);

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

  // Inserts 'range_data' into 'probability_grid'.
  virtual void Insert(const sensor::RangeData& range_data,
                      GridInterface* grid) const override;

 private:
  const proto::ProbabilityGridRangeDataInserterOptions2D options_;
  const std::vector<uint16> hit_table_; // 预先创建的hit表，用于更新hit概率(1-p的概率)
  const std::vector<uint16> miss_table_; // 预先创建的miss表，用于更新miss概率（1-c的概率）
};

// 构造函数创建两个表，两个表都是创建的CorrespondenceCostOdds
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
    : options_(options),
      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()))),
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability()))) {}

// 向网格中插入数据
void ProbabilityGridRangeDataInserter2D::Insert(
    const sensor::RangeData& range_data, GridInterface* const grid) const {
  ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
  // By not finishing the update after hits are inserted, we give hits priority
  // (i.e. no hits will be ignored because of a miss in the same cell).
  CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
           CHECK_NOTNULL(probability_grid));
  probability_grid->FinishUpdate();
}
    
    
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* const probability_grid) {
  // 扩展网格的maplimits
  GrowAsNeeded(range_data, probability_grid);

  const MapLimits& limits = probability_grid->limits();
  // 将maplimits扩大，更精细
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  // 将激光帧的原点当做中心，投影到网格中，计算其ID
  const Eigen::Array2i begin =
      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
  // Compute and add the end points.
  // 计算Hit点的终点在网格中的id
  std::vector<Eigen::Array2i> ends;
  ends.reserve(range_data.returns.size());
  for (const Eigen::Vector3f& hit : range_data.returns) {
    ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
    // 更新hit点的概率
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
  }

  if (!insert_free_space) {
    return;
  }

  // Now add the misses.
  // 更新hit点经过的光线的miss点的概率
  for (const Eigen::Array2i& end : ends) {
    CastRay(begin, end, miss_table, probability_grid);
  }

  // Finally, compute and add empty rays based on misses in the range data.
  // 计算miss点经过的光线的miss点的概率
  for (const Eigen::Vector3f& missing_echo : range_data.misses) {
    CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
            miss_table, probability_grid);
  }
}

// 更新光线经过的所有栅格的miss概率
void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
             const std::vector<uint16>& miss_table,
             ProbabilityGrid* const probability_grid) {
  // For simplicity, we order 'begin' and 'end' by their x coordinate.
  if (begin.x() > end.x()) {
    // 保证CastRay传进来的end的x大于begin的x
    CastRay(end, begin, miss_table, probability_grid);
    return;
  }

  CHECK_GE(begin.x(), 0);
  CHECK_GE(begin.y(), 0);
  CHECK_GE(end.y(), 0);

  // Special case: We have to draw a vertical line in full pixels, as 'begin'
  // and 'end' have the same full pixel x coordinate.
  // 光线的斜率为无穷大
  if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
    Eigen::Array2i current(begin.x() / kSubpixelScale,
                           std::min(begin.y(), end.y()) / kSubpixelScale);
    const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
    for (; current.y() <= end_y; ++current.y()) {
      probability_grid->ApplyLookupTable(current, miss_table);
    }
    return;
  }

  const int64 dx = end.x() - begin.x();
  const int64 dy = end.y() - begin.y();
  const int64 denominator = 2 * kSubpixelScale * dx;

  // The current full pixel coordinates. We begin at 'begin'.
  Eigen::Array2i current = begin / kSubpixelScale;

  // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
  // the denominator.
  // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
  // | | | |
  // +-+-+-+
  // | | | |
  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
  // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
  // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)

  // The center of the subpixel part of 'begin.y()' assuming the
  // 'denominator', i.e., sub_y / denominator is in (0, 1).
  int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;

  // The distance from the from 'begin' to the right pixel border, to be divided
  // by 2 * 'kSubpixelScale'.
  const int first_pixel =
      2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
  // The same from the left pixel border to 'end'.
  const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;

  // The full pixel x coordinate of 'end'.
  const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;

  // Move from 'begin' to the next pixel border to the right.
  sub_y += dy * first_pixel;
  if (dy > 0) {
    while (true) {
      probability_grid->ApplyLookupTable(current, miss_table);
      while (sub_y > denominator) {
        sub_y -= denominator;
        ++current.y();
        probability_grid->ApplyLookupTable(current, miss_table);
      }
      ++current.x();
      if (sub_y == denominator) {
        sub_y -= denominator;
        ++current.y();
      }
      if (current.x() == end_x) {
        break;
      }
      // Move from one pixel border to the next.
      sub_y += dy * 2 * kSubpixelScale;
    }
    // Move from the pixel border on the right to 'end'.
    sub_y += dy * last_pixel;
    probability_grid->ApplyLookupTable(current, miss_table);
    while (sub_y > denominator) {
      sub_y -= denominator;
      ++current.y();
      probability_grid->ApplyLookupTable(current, miss_table);
    }
    CHECK_NE(sub_y, denominator);
    CHECK_EQ(current.y(), end.y() / kSubpixelScale);
    return;
  }

  // Same for lines non-ascending in y coordinates.
  while (true) {
    probability_grid->ApplyLookupTable(current, miss_table);
    while (sub_y < 0) {
      sub_y += denominator;
      --current.y();
      probability_grid->ApplyLookupTable(current, miss_table);
    }
    ++current.x();
    if (sub_y == 0) {
      sub_y += denominator;
      --current.y();
    }
    if (current.x() == end_x) {
      break;
    }
    sub_y += dy * 2 * kSubpixelScale;
  }
  sub_y += dy * last_pixel;
  probability_grid->ApplyLookupTable(current, miss_table);
  while (sub_y < 0) {
    sub_y += denominator;
    --current.y();
    probability_grid->ApplyLookupTable(current, miss_table);
  }
  CHECK_NE(sub_y, 0);
  CHECK_EQ(current.y(), end.y() / kSubpixelScale);
}

void GrowAsNeeded(const sensor::RangeData& range_data,
                  ProbabilityGrid* const probability_grid) {
  // 计算该帧点云的最小矩形
  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
  constexpr float kPadding = 1e-6f;
  for (const Eigen::Vector3f& hit : range_data.returns) {
    bounding_box.extend(hit.head<2>());
  }
  for (const Eigen::Vector3f& miss : range_data.misses) {
    bounding_box.extend(miss.head<2>());
  }
  // 扩展grid的maplimits
  probability_grid->GrowLimits(bounding_box.min() -
                               kPadding * Eigen::Vector2f::Ones());
  probability_grid->GrowLimits(bounding_box.max() +
                               kPadding * Eigen::Vector2f::Ones());
}