Use vector<map<>> instead of vector<deque> for submap_data. (#422)

cartographer/mapping/pose_graph_trimmer.cc

@@ -28,8 +28,7 @@ PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
 }
 
 void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
-  const int total_num_submaps = pose_graph->num_submaps(trajectory_id_);
+  while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) {
-  while (total_num_submaps > num_submaps_trimmed_ + num_submaps_to_keep_) {
     const int submap_index_to_trim_next = num_submaps_trimmed_;
     pose_graph->MarkSubmapAsTrimmed(
         SubmapId{trajectory_id_, submap_index_to_trim_next});

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -64,25 +64,20 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
           trajectory_id,
           sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
     }
-    CHECK_EQ(optimization_problem_.num_trimmed_submaps(trajectory_id), 0);
     CHECK_EQ(submap_data[trajectory_id].size(), 1);
     const mapping::SubmapId submap_id{trajectory_id, 0};
     CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
     return {submap_id};
   }
   CHECK_EQ(2, insertion_submaps.size());
-  const int num_trimmed_submaps =
-      optimization_problem_.num_trimmed_submaps(trajectory_id);
   const mapping::SubmapId last_submap_id{
-      trajectory_id, static_cast<int>(submap_data.at(trajectory_id).size() +
+      trajectory_id,
-                                      num_trimmed_submaps - 1)};
+      submap_data.at(trajectory_id).rbegin()->first};
   if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
     // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
     // and 'insertions_submaps.back()' is new.
     const auto& first_submap_pose =
-        submap_data.at(trajectory_id)
+        submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose;
-            .at(last_submap_id.submap_index - num_trimmed_submaps)
-            .pose;
     optimization_problem_.AddSubmap(
         trajectory_id,
         first_submap_pose *
@@ -194,14 +189,13 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
       const transform::Rigid2d initial_relative_pose =
           optimization_problem_.submap_data()
               .at(submap_id.trajectory_id)
-              .at(submap_id.submap_index -
+              .at(submap_id.submap_index)
-                  optimization_problem_.num_trimmed_submaps(
-                      submap_id.trajectory_id))
               .pose.inverse() *
           optimization_problem_.node_data()
               .at(node_id.trajectory_id)
-              .at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
+              .at(node_id.node_index -
-                                           node_id.trajectory_id))
+                  optimization_problem_.num_trimmed_nodes(
+                      node_id.trajectory_id))
               .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@@ -241,12 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
-  const int num_trimmed_submaps =
-      optimization_problem_.num_trimmed_submaps(trajectory_id);
   const transform::Rigid2d optimized_pose =
       optimization_problem_.submap_data()
           .at(matching_id.trajectory_id)
-          .at(matching_id.submap_index - num_trimmed_submaps)
+          .at(matching_id.submap_index)
           .pose *
       sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
           .inverse() *
@@ -356,21 +348,19 @@ void SparsePoseGraph::WaitForAllComputations() {
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. *
+                  << 100. * (constraint_builder_.GetNumFinishedScans() -
-                         (constraint_builder_.GetNumFinishedScans() -
+                             num_finished_scans_at_start) /
-                          num_finished_scans_at_start) /
                          (num_trajectory_nodes_ - num_finished_scans_at_start)
                   << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone(
+  constraint_builder_.WhenDone([this, &notification](
-      [this, &notification](
+      const sparse_pose_graph::ConstraintBuilder::Result& result) {
-          const sparse_pose_graph::ConstraintBuilder::Result& result) {
+    common::MutexLocker locker(&mutex_);
-        common::MutexLocker locker(&mutex_);
+    constraints_.insert(constraints_.end(), result.begin(), result.end());
-        constraints_.insert(constraints_.end(), result.begin(), result.end());
+    notification = true;
-        notification = true;
+  });
-      });
   locker.Await([&notification]() { return notification; });
 }
 
@@ -401,14 +391,11 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
   CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
            optimized_submap_transforms_.size());
   optimized_submap_transforms_.resize(submap_data_.num_trajectories());
-  CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
-           num_trimmed_submaps_at_last_optimization_.size());
-  num_trimmed_submaps_at_last_optimization_.resize(
-      submap_data_.num_trajectories());
   CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
            submap_id.submap_index);
   optimized_submap_transforms_.at(trajectory_id)
-      .push_back(sparse_pose_graph::SubmapData{initial_pose_2d});
+      .emplace(submap_id.submap_index,
+               sparse_pose_graph::SubmapData{initial_pose_2d});
   AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
     submap_data_.at(submap_id).state = SubmapState::kFinished;
@@ -447,14 +434,7 @@ void SparsePoseGraph::RunOptimization() {
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 
-  std::vector<int> num_trimmed_submaps;
   const auto& submap_data = optimization_problem_.submap_data();
-  for (int trajectory_id = 0;
-       trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) {
-    num_trimmed_submaps.push_back(
-        optimization_problem_.num_trimmed_submaps(trajectory_id));
-  }
-
   const auto& node_data = optimization_problem_.node_data();
   for (int trajectory_id = 0;
        trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@@ -468,11 +448,10 @@ void SparsePoseGraph::RunOptimization() {
           node_data[trajectory_id][node_data_index].point_cloud_pose);
     }
     // Extrapolate all point cloud poses that were added later.
-    const auto local_to_new_global = ComputeLocalToGlobalTransform(
+    const auto local_to_new_global =
-        submap_data, num_trimmed_submaps, trajectory_id);
+        ComputeLocalToGlobalTransform(submap_data, trajectory_id);
     const auto local_to_old_global = ComputeLocalToGlobalTransform(
-        optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
+        optimized_submap_transforms_, trajectory_id);
-        trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
     for (; node_index < num_nodes; ++node_index) {
@@ -482,7 +461,6 @@ void SparsePoseGraph::RunOptimization() {
     }
   }
   optimized_submap_transforms_ = submap_data;
-  num_trimmed_submaps_at_last_optimization_ = num_trimmed_submaps;
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
   reverse_connected_components_.clear();
   for (size_t i = 0; i != connected_components_.size(); ++i) {
@@ -511,9 +489,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
-  return ComputeLocalToGlobalTransform(
+  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-      optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
+                                       trajectory_id);
-      trajectory_id);
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@@ -555,20 +532,19 @@ SparsePoseGraph::GetAllSubmapData() {
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
-    const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
+    const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
         submap_transforms,
-    const std::vector<int>& num_trimmed_submaps,
     const int trajectory_id) const {
   if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
       submap_transforms.at(trajectory_id).empty()) {
     return transform::Rigid3d::Identity();
   }
-  const mapping::SubmapId last_optimized_submap_id{
+
-      trajectory_id,
+  const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
-      static_cast<int>(submap_transforms.at(trajectory_id).size() +
+  const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
-                       num_trimmed_submaps.at(trajectory_id) - 1)};
   // Accessing 'local_pose' in Submap is okay, since the member is const.
-  return transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) *
+  return transform::Embed3D(
+             submap_transforms.at(trajectory_id).at(submap_index).pose) *
          submap_data_.at(last_optimized_submap_id)
              .submap->local_pose()
              .inverse();
@@ -582,9 +558,7 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
   auto submap = submap_data_.at(submap_id).submap;
   if (submap_id.trajectory_id <
       static_cast<int>(optimized_submap_transforms_.size())) {
-    const size_t submap_data_index =
+    const size_t submap_data_index = submap_id.submap_index;
-        submap_id.submap_index -
-        num_trimmed_submaps_at_last_optimization_.at(submap_id.trajectory_id);
     if (submap_data_index <
         optimized_submap_transforms_.at(submap_id.trajectory_id).size()) {
       // We already have an optimized pose.
@@ -595,10 +569,8 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
     }
   }
   // We have to extrapolate.
-  return {submap, ComputeLocalToGlobalTransform(
+  return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-                      optimized_submap_transforms_,
+                                                submap_id.trajectory_id) *
-                      num_trimmed_submaps_at_last_optimization_,
-                      submap_id.trajectory_id) *
                       submap->local_pose()};
 }
 
@@ -607,8 +579,7 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
 
 int SparsePoseGraph::TrimmingHandle::num_submaps(
     const int trajectory_id) const {
-  return parent_->optimization_problem_.submap_data().at(trajectory_id).size() +
+  return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
-         parent_->optimization_problem_.num_trimmed_submaps(trajectory_id);
 }
 
 void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(

cartographer/mapping_2d/sparse_pose_graph.h

@@ -155,10 +155,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Computes the local to global frame transform based on the given optimized
   // 'submap_transforms'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
+      const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
           submap_transforms,
-      const std::vector<int>& num_trimmed_submaps, int trajectory_id) const
+      int trajectory_id) const REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
       const mapping::SubmapId& submap_id) REQUIRES(mutex_);
@@ -205,8 +204,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Current submap transforms used for displaying data.
-  std::vector<int> num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_);
+  std::vector<std::map<int, sparse_pose_graph::SubmapData>>
-  std::vector<std::deque<sparse_pose_graph::SubmapData>>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
 
   // List of all trimmers to consult when optimizations finish.

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -112,19 +112,18 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
   CHECK_GE(trajectory_id, 0);
   submap_data_.resize(
       std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
   trajectory_data_.resize(
       std::max(trajectory_data_.size(), submap_data_.size()));
+
+  auto& trajectory_data = trajectory_data_.at(trajectory_id);
+  submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
+                                      SubmapData{submap_pose});
+  ++trajectory_data.next_submap_index;
 }
 
 void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
-  auto& trajectory_data = trajectory_data_.at(submap_id.trajectory_id);
-  // We only allow trimming from the start.
-  CHECK_EQ(trajectory_data.num_trimmed_submaps, submap_id.submap_index);
   auto& submap_data = submap_data_.at(submap_id.trajectory_id);
-  CHECK(!submap_data.empty());
+  CHECK(submap_data.erase(submap_id.submap_index));
-  submap_data.pop_front();
-  ++trajectory_data.num_trimmed_submaps;
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -144,25 +143,27 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
-  std::vector<std::vector<std::array<double, 3>>> C_submaps(
+  std::vector<std::map<int, std::array<double, 3>>> C_submaps(
       submap_data_.size());
   std::vector<std::vector<std::array<double, 3>>> C_nodes(node_data_.size());
   bool first_submap = true;
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     const bool frozen = frozen_trajectories.count(trajectory_id);
-    // Reserve guarantees that data does not move, so the pointers for Ceres
+    for (const auto& index_submap_data : submap_data_[trajectory_id]) {
-    // stay valid.
+      const int submap_index = index_submap_data.first;
-    C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size());
+      const SubmapData& submap_data = index_submap_data.second;
-    for (const SubmapData& submap_data : submap_data_[trajectory_id]) {
+
-      C_submaps[trajectory_id].push_back(FromPose(submap_data.pose));
+      C_submaps[trajectory_id].emplace(
-      problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
+              submap_index, FromPose(submap_data.pose));
+      problem.AddParameterBlock(
+          C_submaps[trajectory_id].at(submap_index).data(), 3);
       if (first_submap || frozen) {
         first_submap = false;
         // Fix the pose of the first submap or all submaps of a frozen
         // trajectory.
         problem.SetParameterBlockConstant(
-            C_submaps[trajectory_id].back().data());
+            C_submaps[trajectory_id].at(submap_index).data());
       }
     }
   }
@@ -190,9 +191,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
             ? new ceres::HuberLoss(options_.huber_scale())
             : nullptr,
         C_submaps.at(constraint.submap_id.trajectory_id)
-            .at(constraint.submap_id.submap_index -
+            .at(constraint.submap_id.submap_index)
-                trajectory_data_.at(constraint.submap_id.trajectory_id)
-                    .num_trimmed_submaps)
             .data(),
         C_nodes.at(constraint.node_id.trajectory_id)
             .at(constraint.node_id.node_index -
@@ -244,7 +243,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   ceres::Solve(
       common::CreateCeresSolverOptions(options_.ceres_solver_options()),
       &problem, &summary);
-
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();
   }
@@ -252,11 +250,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // Store the result.
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
-    for (size_t submap_data_index = 0;
+    for (auto& index_submap_data : submap_data_[trajectory_id]) {
-         submap_data_index != submap_data_[trajectory_id].size();
+      index_submap_data.second.pose =
-         ++submap_data_index) {
+          ToPose(C_submaps[trajectory_id].at(index_submap_data.first));
-      submap_data_[trajectory_id][submap_data_index].pose =
-          ToPose(C_submaps[trajectory_id][submap_data_index]);
     }
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@@ -275,7 +271,7 @@ const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data()
   return node_data_;
 }
 
-const std::vector<std::deque<SubmapData>>& OptimizationProblem::submap_data()
+const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
     const {
   return submap_data_;
 }
@@ -284,10 +280,6 @@ int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const {
   return trajectory_data_.at(trajectory_id).num_trimmed_nodes;
 }
 
-int OptimizationProblem::num_trimmed_submaps(int trajectory_id) const {
-  return trajectory_data_.at(trajectory_id).num_trimmed_submaps;
-}
-
 }  // namespace sparse_pose_graph
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -77,22 +77,21 @@ class OptimizationProblem {
              const std::set<int>& frozen_trajectories);
 
   const std::vector<std::deque<NodeData>>& node_data() const;
-  const std::vector<std::deque<SubmapData>>& submap_data() const;
+  const std::vector<std::map<int, SubmapData>>& submap_data() const;
 
   int num_trimmed_nodes(int trajectory_id) const;
-  int num_trimmed_submaps(int trajectory_id) const;
 
  private:
   struct TrajectoryData {
     // TODO(hrapp): Remove, once we can relabel constraints.
+    int next_submap_index = 0;
     int num_trimmed_nodes = 0;
-    int num_trimmed_submaps = 0;
   };
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::deque<NodeData>> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
-  std::vector<std::deque<SubmapData>> submap_data_;
+  std::vector<std::map<int, SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
 };