cartographer-mapping-id

id

cartographer主要处理数据包括：node和submap；这些数据，主要通过MapById 类进行封装，并处理，因此，有必要研究下MapById类。理解了id类，也就理解类carto中数据结构以及trajectroy nodeid submapid等重要概念。

namespace cartographer {
namespace mapping {

/*
每一个轨迹trajectory上有多个节点node。
如何唯一的表达这些节点node？节点标号:轨迹id+{0,1,2,...}
*/

struct NodeId {
  NodeId(int trajectory_id, int node_index)
      : trajectory_id(trajectory_id), node_index(node_index) {}

  int trajectory_id;
  int node_index;
};
/*
一般来说，重建全局地图global map时，是由多个submap组成。
如何给这些submap标号? 轨迹id+ {0,1,2,3...}
*/
struct SubmapId {
  SubmapId(int trajectory_id, int submap_index)
      : trajectory_id(trajectory_id), submap_index(submap_index) {}
  int trajectory_id;
  int submap_index;
};

template <typename IteratorType>
class Range {
private:
  IteratorType begin_;
  IteratorType end_;
public: 
  Range(const IteratorType& begin, const IteratorType& end)
      : begin_(begin), end_(end) {}
};

// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or
// 'SubmapId'.
/*
MapById:通过id（IdType，包括 trajectory_id, submap_index;），和 data（DataType）形成 一个表（map），结构如下：
1   trajectory1  ： can_append_(true),{(0,submap),(1,submap),(2,submap),(3,submap),(4,submap)...}
2   trajectory2  ： can_append_(true),{(0,submap),(1,submap),(2,submap),(3,submap),(4,submap)...}
3   trajectory3  ： can_append_(true),{(0,submap),(1,submap),(2,submap),(3,submap),(4,submap)...}

自定义该表（map中data）的索引，包括三个元素。
ConstIterator{
    typename std::map<int, MapByIndex>::const_iterator current_trajectory_; //当前轨迹iter
    typename std::map<int, MapByIndex>::const_iterator end_trajectory_;// 最后轨迹iter
    typename std::map<int, DataType>::const_iterator current_data_;//当前值，指向当前轨迹的某个data 
}
ConstTrajectoryIterator{
    typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
}

*/
template <typename IdType, typename DataType>
class MapById {
 private:
  struct MapByIndex {
    bool can_append_ = true;
    std::map<int, DataType> data_;
  };
  std::map<int, MapByIndex> trajectories_;

 public:
  struct IdDataReference {
    IdType id;
    const DataType& data;
  };

  class ConstIterator {
   public:
    using iterator_category = std::bidirectional_iterator_tag;
    using value_type = IdDataReference;
    using difference_type = int64;
    using pointer = std::unique_ptr<const IdDataReference>;
    using reference = const IdDataReference&;
    

    // 通过表和轨迹id（trajectory_id）获得索引，current_data_为该轨迹的第一个validData值
    explicit ConstIterator(const MapById& map_by_id, const int trajectory_id)
        : current_trajectory_(
              map_by_id.trajectories_.lower_bound(trajectory_id)),
          end_trajectory_(map_by_id.trajectories_.end()) {
      if (current_trajectory_ != end_trajectory_) {
        current_data_ = current_trajectory_->second.data_.begin();
        AdvanceToValidDataIterator();
      }
    }
    // 通过表和data的Id获取索引。
    explicit ConstIterator(const MapById& map_by_id, const IdType& id)
        : current_trajectory_(map_by_id.trajectories_.find(id.trajectory_id)),
          end_trajectory_(map_by_id.trajectories_.end()) {
      if (current_trajectory_ != end_trajectory_) {
        current_data_ =
            current_trajectory_->second.data_.find(MapById::GetIndex(id));
        if (current_data_ == current_trajectory_->second.data_.end()) {
          current_trajectory_ = end_trajectory_;
        }
      }
    }
    
    IdDataReference operator*() const {  // 对* 重载
      CHECK(current_trajectory_ != end_trajectory_);
      return IdDataReference{
          IdType{current_trajectory_->first, current_data_->first},
          current_data_->second};
    }
    
    std::unique_ptr<const IdDataReference> operator->() const {
      return absl::make_unique<const IdDataReference>(this->operator*());
    }

   private:
    // 如果数据到了current_trajectory_末尾，那么移动到下一个trajectory_的begin()
    void AdvanceToValidDataIterator() {
      CHECK(current_trajectory_ != end_trajectory_);
      while (current_data_ == current_trajectory_->second.data_.end()) {
        ++current_trajectory_;
        if (current_trajectory_ == end_trajectory_) {
          return;
        }
        current_data_ = current_trajectory_->second.data_.begin();
      }
    }

    typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
    typename std::map<int, MapByIndex>::const_iterator end_trajectory_;
    typename std::map<int, DataType>::const_iterator current_data_;

  };

  class ConstTrajectoryIterator {
    typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
  };

  // Appends data to a trajectories_[trajectory_id], creating trajectories as needed.
  // 把某个数据加入到trajectory_id对应的trajectory下
  IdType Append(const int trajectory_id, const DataType& data) {
    CHECK_GE(trajectory_id, 0);
    auto& trajectory = trajectories_[trajectory_id];
    CHECK(trajectory.can_append_);
    const int index =
        trajectory.data_.empty() ? 0 : trajectory.data_.rbegin()->first + 1; //rbegin,Return reverse iterator to reverse beginning
    trajectory.data_.emplace(index, data);
    return IdType{trajectory_id, index};
  }

  // Returns an iterator to the element at 'id' or the end iterator if it does
  // not exist. 
  // 返回 本表（map）中，id对应索引（ConstIterator形式）
  ConstIterator find(const IdType& id) const {
    return ConstIterator(*this, id);
  }

  // Inserts data (which must not exist already) into a trajectory.
  // 把id对应的data插入到表中
  void Insert(const IdType& id, const DataType& data) {
    CHECK_GE(id.trajectory_id, 0);
    CHECK_GE(GetIndex(id), 0);
    auto& trajectory = trajectories_[id.trajectory_id];
    trajectory.can_append_ = false;
    CHECK(trajectory.data_.emplace(GetIndex(id), data).second);
  }

  // Removes the data for 'id' which must exist.
  // 删除表中对应id的数据
  void Trim(const IdType& id) {
    auto& trajectory = trajectories_.at(id.trajectory_id);
    const auto it = trajectory.data_.find(GetIndex(id));
    CHECK(it != trajectory.data_.end()) << id;
    if (std::next(it) == trajectory.data_.end()) {
      // We are removing the data with the highest index from this trajectory.
      // We assume that we will never append to it anymore. If we did, we would
      // have to make sure that gaps in indices are properly chosen to maintain
      // correct connectivity.
      trajectory.can_append_ = false;
    }
    trajectory.data_.erase(it);
    if (trajectory.data_.empty()) {
      trajectories_.erase(id.trajectory_id);
    }
  }
  // 查看是否有对应id的数据
  bool Contains(const IdType& id) const {
    return trajectories_.count(id.trajectory_id) != 0 &&
           trajectories_.at(id.trajectory_id).d ata_.count(GetIndex(id)) != 0;
  }
  // 返回表中固定id对应的数据
  const DataType& at(const IdType& id) const {
    return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
  }

  DataType& at(const IdType& id) {
    return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
  }

  // 某个trajectory_id 对应的首地址ConstIterator表示形式
  ConstIterator BeginOfTrajectory(const int trajectory_id) const {
    return ConstIterator(*this, trajectory_id);
  }
  // 某个trajectory_id 对应的尾地址ConstIterator表示形式
  ConstIterator EndOfTrajectory(const int trajectory_id) const {
    return BeginOfTrajectory(trajectory_id + 1);
  }

  // Returns 0 if 'trajectory_id' does not exist.
  //整个表轨迹个数
  size_t SizeOfTrajectoryOrZero(const int trajectory_id) const {
    return trajectories_.count(trajectory_id)
               ? trajectories_.at(trajectory_id).data_.size()
               : 0;
  }

  // Returns count of all elements.
  //整个表数据个数
  size_t size() const {
    size_t size = 0;
    for (const auto& item : trajectories_) {
      size += item.second.data_.size();
    }
    return size;
  }

  // Returns Range object for range-based loops over the nodes of a trajectory.
  Range<ConstIterator> trajectory(const int trajectory_id) const {
    return Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
                                EndOfTrajectory(trajectory_id));
  }

  // Returns Range object for range-based loops over the trajectory IDs.
  Range<ConstTrajectoryIterator> trajectory_ids() const {
    return Range<ConstTrajectoryIterator>(
        ConstTrajectoryIterator(trajectories_.begin()),
        ConstTrajectoryIterator(trajectories_.end()));
  }

  ConstIterator begin() const { return BeginOfTrajectory(0); }
  ConstIterator end() const {
    return BeginOfTrajectory(std::numeric_limits<int>::max());
  }

  bool empty() const { return begin() == end(); }

  // Returns an iterator to the first element in the container belonging to
  // trajectory 'trajectory_id' whose time is not considered to go before
  // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
  // before 'time'. 

  ConstIterator lower_bound(const int trajectory_id,
                            const common::Time time) const {
    if (SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      return EndOfTrajectory(trajectory_id);
    }

    const std::map<int, DataType>& trajectory =
        trajectories_.at(trajectory_id).data_;
    if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
      return EndOfTrajectory(trajectory_id);
    }
    auto left = trajectory.begin();
    auto right = std::prev(trajectory.end());
    while (left != right) {
      const int middle = left->first + (right->first - left->first) / 2;
      const auto lower_bound_middle = trajectory.lower_bound(middle);
      if (internal::GetTime(lower_bound_middle->second) < time) {
        left = std::next(lower_bound_middle);
      } else {
        right = lower_bound_middle;
      }
    }
    return ConstIterator(*this, IdType{trajectory_id, left->first});
  }

 private:
  static int GetIndex(const NodeId& id) { return id.node_index; }
  static int GetIndex(const SubmapId& id) { return id.submap_index; }
};

}  // namespace mapping
}  // namespace cartographer