Delete SubmapData from trimmed submaps in the OptimizationProblem. (#366)

Related to #283.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -65,20 +65,25 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
           trajectory_id,
           sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
     }
-    const mapping::SubmapId submap_id{
+    CHECK_EQ(optimization_problem_.num_trimmed_submaps(trajectory_id), 0);
-        trajectory_id, static_cast<int>(submap_data[trajectory_id].size()) - 1};
+    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,
+      trajectory_id, static_cast<int>(submap_data.at(trajectory_id).size() +
-      static_cast<int>(submap_data.at(trajectory_id).size() - 1)};
+                                      num_trimmed_submaps - 1)};
   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).at(last_submap_id.submap_index).pose;
+        submap_data.at(trajectory_id)
+            .at(last_submap_id.submap_index - num_trimmed_submaps)
+            .pose;
     optimization_problem_.AddSubmap(
         trajectory_id,
         first_submap_pose *
@@ -186,7 +191,9 @@ 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)
@@ -231,10 +238,11 @@ 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)
+          .at(matching_id.submap_index - num_trimmed_submaps)
           .pose *
       sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
           .inverse() *
@@ -389,6 +397,14 @@ void SparsePoseGraph::RunOptimization() {
   optimization_problem_.Solve(constraints_);
   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) {
@@ -403,9 +419,10 @@ void SparsePoseGraph::RunOptimization() {
     }
     // Extrapolate all point cloud poses that were added later.
     const auto local_to_new_global = ComputeLocalToGlobalTransform(
-        optimization_problem_.submap_data(), trajectory_id);
+        submap_data, num_trimmed_submaps, trajectory_id);
     const auto local_to_old_global = ComputeLocalToGlobalTransform(
-        optimized_submap_transforms_, trajectory_id);
+        optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
+        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) {
@@ -414,7 +431,8 @@ void SparsePoseGraph::RunOptimization() {
           old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
     }
   }
-  optimized_submap_transforms_ = optimization_problem_.submap_data();
+  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) {
@@ -444,6 +462,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
+                                       num_trimmed_submaps_at_last_optimization_,
                                        trajectory_id);
 }
 
@@ -486,8 +505,9 @@ SparsePoseGraph::GetAllSubmapData() {
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
-    const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
+    const std::vector<std::deque<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()) {
@@ -495,7 +515,8 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
   }
   const mapping::SubmapId last_optimized_submap_id{
       trajectory_id,
-      static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)};
+      static_cast<int>(submap_transforms.at(trajectory_id).size() +
+                       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) *
          submap_data_.at(last_optimized_submap_id)
@@ -509,20 +530,25 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
     return {};
   }
   auto submap = submap_data_.at(submap_id).submap;
-  // We already have an optimized pose.
   if (submap_id.trajectory_id <
-          static_cast<int>(optimized_submap_transforms_.size()) &&
+      static_cast<int>(optimized_submap_transforms_.size())) {
-      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
+    const size_t submap_data_index =
-                                                    .at(submap_id.trajectory_id)
+        submap_id.submap_index -
-                                                    .size())) {
+        num_trimmed_submaps_at_last_optimization_.at(submap_id.trajectory_id);
-    return {submap, transform::Embed3D(
+    if (submap_data_index <
-                        optimized_submap_transforms_.at(submap_id.trajectory_id)
+        optimized_submap_transforms_.at(submap_id.trajectory_id).size()) {
-                            .at(submap_id.submap_index)
+      // We already have an optimized pose.
-                            .pose)};
+      return {submap, transform::Embed3D(optimized_submap_transforms_
+                                             .at(submap_id.trajectory_id)
+                                             .at(submap_data_index)
+                                             .pose)};
+    }
   }
   // We have to extrapolate.
-  return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
+  return {submap, ComputeLocalToGlobalTransform(
-                                                submap_id.trajectory_id) *
+                      optimized_submap_transforms_,
+                      num_trimmed_submaps_at_last_optimization_,
+                      submap_id.trajectory_id) *
                       submap->local_pose()};
 }
 
@@ -531,7 +557,8 @@ 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(
@@ -585,6 +612,7 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   CHECK(submap_data.submap != nullptr);
   submap_data.submap.reset();
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
+  parent_->optimization_problem_.TrimSubmap(submap_id);
 
   // Mark the 'nodes_to_remove' as trimmed and remove their data.
   for (const mapping::NodeId& node_id : nodes_to_remove) {
@@ -592,11 +620,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     parent_->trajectory_nodes_.at(node_id).constant_data.reset();
     parent_->optimization_problem_.TrimTrajectoryNode(node_id);
   }
-
-  // TODO(whess): The optimization problem should no longer include the submap.
-
-  // TODO(whess): If the first submap is gone, we want to tie the first not
-  // yet trimmed submap to be set fixed to its current pose.
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/sparse_pose_graph.h

@@ -150,9 +150,10 @@ 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::vector<sparse_pose_graph::SubmapData>>&
+      const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
           submap_transforms,
-      int trajectory_id) const REQUIRES(mutex_);
+      const std::vector<int>& num_trimmed_submaps, int trajectory_id) const
+      REQUIRES(mutex_);
 
   mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
       const mapping::SubmapId& submap_id) REQUIRES(mutex_);
@@ -199,7 +200,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Current submap transforms used for displaying data.
-  std::vector<std::vector<sparse_pose_graph::SubmapData>>
+  std::vector<int> num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_);
+  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

@@ -104,6 +104,18 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
   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()));
+}
+
+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());
+  submap_data.pop_front();
+  ++trajectory_data.num_trimmed_submaps;
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -122,23 +134,22 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
-  std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
+  std::vector<std::vector<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) {
-    for (size_t submap_index = 0;
+    // Reserve guarantees that data does not move, so the pointers for Ceres
-         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
+    // stay valid.
-      if (trajectory_id == 0 && submap_index == 0) {
+    C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size());
-        // Fix the pose of the first submap of the first trajectory.
+    for (const SubmapData& submap_data : submap_data_[trajectory_id]) {
-        C_submaps[trajectory_id].push_back(
+      C_submaps[trajectory_id].push_back(FromPose(submap_data.pose));
-            FromPose(transform::Rigid2d::Identity()));
+      problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
-        problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
+      if (first_submap) {
+        first_submap = false;
+        // Fix the pose of the first submap.
         problem.SetParameterBlockConstant(
             C_submaps[trajectory_id].back().data());
-      } else {
-        C_submaps[trajectory_id].push_back(
-            FromPose(submap_data_[trajectory_id][submap_index].pose));
-        problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
       }
     }
   }
@@ -163,7 +174,9 @@ 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 -
@@ -207,10 +220,10 @@ 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_index = 0;
+    for (size_t submap_data_index = 0;
-         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
+         submap_data_index != submap_data_[trajectory_id].size(); ++submap_data_index) {
-      submap_data_[trajectory_id][submap_index].pose =
+      submap_data_[trajectory_id][submap_data_index].pose =
-          ToPose(C_submaps[trajectory_id][submap_index]);
+          ToPose(C_submaps[trajectory_id][submap_data_index]);
     }
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@@ -229,7 +242,7 @@ const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data()
   return node_data_;
 }
 
-const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
+const std::vector<std::deque<SubmapData>>& OptimizationProblem::submap_data()
     const {
   return submap_data_;
 }
@@ -238,6 +251,10 @@ 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

@@ -65,6 +65,7 @@ class OptimizationProblem {
                          const transform::Rigid2d& point_cloud_pose);
   void TrimTrajectoryNode(const mapping::NodeId& node_id);
   void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
+  void TrimSubmap(const mapping::SubmapId& submap_id);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 
@@ -72,19 +73,21 @@ class OptimizationProblem {
   void Solve(const std::vector<Constraint>& constraints);
 
   const std::vector<std::deque<NodeData>>& node_data() const;
-  const std::vector<std::vector<SubmapData>>& submap_data() const;
+  const std::vector<std::deque<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 num_trimmed_nodes = 0;
+    int num_trimmed_submaps = 0;
   };
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
   std::vector<std::deque<NodeData>> node_data_;
-  std::vector<std::vector<SubmapData>> submap_data_;
+  std::vector<std::deque<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
 };