Introduces mapping::MapById for nodes in 2D. (#581)

PAIR=cschuet

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -102,7 +102,7 @@ void SparsePoseGraph::AddScan(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(trajectory_id);
-  trajectory_nodes_.Append(
+  const mapping::NodeId node_id = trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
   ++num_trajectory_nodes_;
 
@@ -124,7 +124,7 @@ void SparsePoseGraph::AddScan(
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
+    ComputeConstraintsForScan(node_id, insertion_submaps,
                               newly_finished_submap);
   });
 }
@@ -188,10 +188,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     // search window.
     const transform::Rigid2d initial_relative_pose =
         optimization_problem_.submap_data().at(submap_id).pose.inverse() *
-        optimization_problem_.node_data()
+        optimization_problem_.node_data().at(node_id).pose;
-            .at(node_id.trajectory_id)
-            .at(node_id.node_index)
-            .pose;
     constraint_builder_.MaybeAddConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get(),
@@ -206,40 +203,23 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
 void SparsePoseGraph::ComputeConstraintsForOldScans(
     const mapping::SubmapId& submap_id) {
   const auto& submap_data = submap_data_.at(submap_id);
-  const auto& node_data = optimization_problem_.node_data();
+  for (const auto& node_id_data : optimization_problem_.node_data()) {
-  for (size_t trajectory_id = 0; trajectory_id != node_data.size();
+    const mapping::NodeId& node_id = node_id_data.id;
-       ++trajectory_id) {
+    CHECK(!trajectory_nodes_.at(node_id).trimmed());
-    for (const auto& index_node_data : node_data[trajectory_id]) {
+    if (submap_data.node_ids.count(node_id) == 0) {
-      const mapping::NodeId node_id{static_cast<int>(trajectory_id),
+      ComputeConstraint(node_id, submap_id);
-                                    index_node_data.first};
-      CHECK(!trajectory_nodes_.at(node_id).trimmed());
-      if (submap_data.node_ids.count(node_id) == 0) {
-        ComputeConstraint(node_id, submap_id);
-      }
     }
   }
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    const int trajectory_id,
+    const mapping::NodeId& node_id,
     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
     const bool newly_finished_submap) {
   const std::vector<mapping::SubmapId> submap_ids =
-      GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
+      GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
-  const mapping::NodeId node_id{
-      matching_id.trajectory_id,
-      static_cast<size_t>(matching_id.trajectory_id) <
-                  optimization_problem_.node_data().size() &&
-              !optimization_problem_.node_data()[matching_id.trajectory_id]
-                   .empty()
-          ? static_cast<int>(optimization_problem_.node_data()
-                                 .at(matching_id.trajectory_id)
-                                 .rbegin()
-                                 ->first +
-                             1)
-          : 0};
   const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
   const transform::Rigid2d pose = transform::Project2D(
       constant_data->initial_pose *
@@ -489,14 +469,15 @@ void SparsePoseGraph::RunOptimization() {
 
   const auto& submap_data = optimization_problem_.submap_data();
   const auto& node_data = optimization_problem_.node_data();
-  for (int trajectory_id = 0;
+  for (auto node_it = node_data.begin(); node_it != node_data.end();) {
-       trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
+    const int trajectory_id = node_it->id.trajectory_id;
+    const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
     const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
-    for (const auto& node_data_index : node_data.at(trajectory_id)) {
+    for (; node_it != trajectory_end; ++node_it) {
-      const mapping::NodeId node_id{trajectory_id, node_data_index.first};
+      const mapping::NodeId node_id = node_it->id;
       auto& node = trajectory_nodes_.at(node_id);
       node.pose =
-          transform::Embed3D(node_data_index.second.pose) *
+          transform::Embed3D(node_it->data.pose) *
           transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
     }
     // Extrapolate all point cloud poses that were added later.
@@ -506,10 +487,7 @@ void SparsePoseGraph::RunOptimization() {
         optimized_submap_transforms_, trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
-    int last_optimized_node_index =
+    const int last_optimized_node_index = std::prev(node_it)->id.node_index;
-        node_data.at(trajectory_id).empty()
-            ? 0
-            : node_data.at(trajectory_id).rbegin()->first;
     for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
          ++node_index) {
       const mapping::NodeId node_id{trajectory_id, node_index};

cartographer/mapping_2d/sparse_pose_graph.h

@@ -134,7 +134,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
-      int trajectory_id,
+      const mapping::NodeId& node_id,
       std::vector<std::shared_ptr<const Submap>> insertion_submaps,
       bool newly_finished_submap) REQUIRES(mutex_);
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -82,25 +82,19 @@ void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
     const Eigen::Quaterniond& gravity_alignment) {
-  CHECK_GE(trajectory_id, 0);
+  node_data_.Append(trajectory_id,
-  node_data_.resize(
+                    NodeData{time, initial_pose, pose, gravity_alignment});
-      std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
-  auto& trajectory_data = trajectory_data_[trajectory_id];
-  node_data_[trajectory_id].emplace(
-      trajectory_data.next_node_index,
-      NodeData{time, initial_pose, pose, gravity_alignment});
-  ++trajectory_data.next_node_index;
 }
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
-  auto& node_data = node_data_.at(node_id.trajectory_id);
+  node_data_.Trim(node_id);
-  CHECK(node_data.erase(node_id.node_index));
 
-  if (!node_data.empty() &&
+  const int trajectory_id = node_id.trajectory_id;
-      node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
+  if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
-    const common::Time node_time = node_data.begin()->second.time;
+      trajectory_id < static_cast<int>(imu_data_.size())) {
-    auto& imu_data = imu_data_.at(node_id.trajectory_id);
+    const common::Time node_time =
+        node_data_.BeginOfTrajectory(trajectory_id)->data.time;
+    auto& imu_data = imu_data_.at(trajectory_id);
     while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
       imu_data.pop_front();
     }
@@ -134,7 +128,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
   mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
-  std::vector<std::map<int, std::array<double, 3>>> C_nodes(node_data_.size());
+  mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
   bool first_submap = true;
   for (const auto& submap_id_data : submap_data_) {
     const bool frozen =
@@ -148,19 +142,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
     }
   }
-  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
+  for (const auto& node_id_data : node_data_) {
-       ++trajectory_id) {
+    const bool frozen =
-    const bool frozen = frozen_trajectories.count(trajectory_id) != 0;
+        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
-    for (const auto& index_node_data : node_data_[trajectory_id]) {
+    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.pose));
-      const int node_index = index_node_data.first;
+    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
-      const NodeData& node_data = index_node_data.second;
+    if (frozen) {
-      C_nodes[trajectory_id].emplace(node_index, FromPose(node_data.pose));
+      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
-      problem.AddParameterBlock(C_nodes[trajectory_id].at(node_index).data(),
-                                3);
-      if (frozen) {
-        problem.SetParameterBlockConstant(
-            C_nodes[trajectory_id].at(node_index).data());
-      }
     }
   }
   // Add cost functions for intra- and inter-submap constraints.
@@ -173,58 +161,52 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
             ? new ceres::HuberLoss(options_.huber_scale())
             : nullptr,
         C_submaps.at(constraint.submap_id).data(),
-        C_nodes.at(constraint.node_id.trajectory_id)
+        C_nodes.at(constraint.node_id).data());
-            .at(constraint.node_id.node_index)
-            .data());
   }
 
   // Add penalties for violating odometry or changes between consecutive scans
   // if odometry is not available.
-  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
+  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
-       ++trajectory_id) {
+    const int trajectory_id = node_it->id.trajectory_id;
-    if (node_data_[trajectory_id].empty()) {
+    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
-      continue;
-    }
 
-    for (auto node_it = node_data_[trajectory_id].begin();;) {
+    auto prev_node_it = node_it;
-      const int node_index = node_it->first;
+    for (++node_it; node_it != trajectory_end; ++node_it) {
-      const NodeData& node_data = node_it->second;
+      const mapping::NodeId first_node_id = prev_node_it->id;
-      ++node_it;
+      const NodeData& first_node_data = prev_node_it->data;
-      if (node_it == node_data_[trajectory_id].end()) {
+      prev_node_it = node_it;
-        break;
+      const mapping::NodeId second_node_id = node_it->id;
-      }
+      const NodeData& second_node_data = node_it->data;
 
-      const int next_node_index = node_it->first;
+      if (second_node_id.node_index != first_node_id.node_index + 1) {
-      const NodeData& next_node_data = node_it->second;
-
-      if (next_node_index != node_index + 1) {
         continue;
       }
 
       const bool odometry_available =
-          trajectory_id < odometry_data_.size() &&
+          trajectory_id < static_cast<int>(odometry_data_.size()) &&
-          odometry_data_[trajectory_id].Has(next_node_data.time) &&
+          odometry_data_[trajectory_id].Has(second_node_data.time) &&
-          odometry_data_[trajectory_id].Has(node_data.time);
+          odometry_data_[trajectory_id].Has(first_node_data.time);
       const transform::Rigid3d relative_pose =
           odometry_available
-              ? transform::Rigid3d::Rotation(node_data.gravity_alignment) *
+              ? transform::Rigid3d::Rotation(
+                    first_node_data.gravity_alignment) *
                     odometry_data_[trajectory_id]
-                        .Lookup(node_data.time)
+                        .Lookup(first_node_data.time)
                         .inverse() *
-                    odometry_data_[trajectory_id].Lookup(next_node_data.time) *
+                    odometry_data_[trajectory_id].Lookup(
+                        second_node_data.time) *
                     transform::Rigid3d::Rotation(
-                        next_node_data.gravity_alignment.inverse())
+                        second_node_data.gravity_alignment.inverse())
-              : transform::Embed3D(node_data.initial_pose.inverse() *
+              : transform::Embed3D(first_node_data.initial_pose.inverse() *
-                                   next_node_data.initial_pose);
+                                   second_node_data.initial_pose);
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
               new SpaCostFunction(Constraint::Pose{
                   relative_pose,
                   options_.consecutive_scan_translation_penalty_factor(),
                   options_.consecutive_scan_rotation_penalty_factor()})),
-          nullptr /* loss function */,
+          nullptr /* loss function */, C_nodes.at(first_node_id).data(),
-          C_nodes[trajectory_id].at(node_index).data(),
+          C_nodes.at(second_node_id).data());
-          C_nodes[trajectory_id].at(next_node_index).data());
     }
   }
 
@@ -241,17 +223,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   for (const auto& C_submap_id_data : C_submaps) {
     submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data);
   }
-  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
+  for (const auto& C_node_id_data : C_nodes) {
-       ++trajectory_id) {
+    node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
-    for (auto& index_node_data : node_data_[trajectory_id]) {
-      index_node_data.second.pose =
-          ToPose(C_nodes[trajectory_id].at(index_node_data.first));
-    }
   }
 }
 
-const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
+const mapping::MapById<mapping::NodeId, NodeData>&
-    const {
+OptimizationProblem::node_data() const {
   return node_data_;
 }
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -79,20 +79,15 @@ class OptimizationProblem {
   void Solve(const std::vector<Constraint>& constraints,
              const std::set<int>& frozen_trajectories);
 
-  const std::vector<std::map<int, NodeData>>& node_data() const;
+  const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
 
  private:
-  struct TrajectoryData {
-    // TODO(hrapp): Remove, once we can relabel constraints.
-    int next_node_index = 0;
-  };
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
-  std::vector<std::map<int, NodeData>> node_data_;
+  mapping::MapById<mapping::NodeId, NodeData> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
-  std::vector<TrajectoryData> trajectory_data_;
 };
 
 }  // namespace sparse_pose_graph