Add trajectory trimming support to 3d (#559)

* Adds DeleteScanMatcher() to 3D constraint builder
* Add possibility to remove submaps and scans from the 3D optimization
  problem.
* Implements TrimmingHandle in SPA.

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -648,12 +648,68 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
 
 int SparsePoseGraph::TrimmingHandle::num_submaps(
     const int trajectory_id) const {
-  LOG(FATAL) << "Not yet implemented for 3D.";
+  return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
 }
 
 void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
-  LOG(FATAL) << "Not yet implemented for 3D.";
+  // TODO(hrapp): We have to make sure that the trajectory has been finished
+  // if we want to delete the last submaps.
+  CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
+
+  // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
+  // with 'submap_id' is gone.
+  std::set<mapping::NodeId> nodes_to_retain;
+  for (const Constraint& constraint : parent_->constraints_) {
+    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
+        constraint.submap_id != submap_id) {
+      nodes_to_retain.insert(constraint.node_id);
+    }
+  }
+  // Remove all 'constraints_' related to 'submap_id'.
+  std::set<mapping::NodeId> nodes_to_remove;
+  {
+    std::vector<Constraint> constraints;
+    for (const Constraint& constraint : parent_->constraints_) {
+      if (constraint.submap_id == submap_id) {
+        if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
+            nodes_to_retain.count(constraint.node_id) == 0) {
+          // This node will no longer be INTRA_SUBMAP contrained and has to be
+          // removed.
+          nodes_to_remove.insert(constraint.node_id);
+        }
+      } else {
+        constraints.push_back(constraint);
+      }
+    }
+    parent_->constraints_ = std::move(constraints);
+  }
+  // Remove all 'constraints_' related to 'nodes_to_remove'.
+  {
+    std::vector<Constraint> constraints;
+    for (const Constraint& constraint : parent_->constraints_) {
+      if (nodes_to_remove.count(constraint.node_id) == 0) {
+        constraints.push_back(constraint);
+      }
+    }
+    parent_->constraints_ = std::move(constraints);
+  }
+
+  // Mark the submap with 'submap_id' as trimmed and remove its data.
+  auto& submap_data = parent_->submap_data_.at(submap_id);
+  CHECK(submap_data.state == SubmapState::kFinished);
+  submap_data.state = SubmapState::kTrimmed;
+  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) {
+    CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed());
+    parent_->trajectory_nodes_.at(node_id).constant_data.reset();
+    parent_->optimization_problem_.TrimTrajectoryNode(node_id);
+  }
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -294,6 +294,12 @@ int ConstraintBuilder::GetNumFinishedScans() {
   return pending_computations_.begin()->first;
 }
 
+void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
+  common::MutexLocker locker(&mutex_);
+  CHECK(pending_computations_.empty());
+  submap_scan_matchers_.erase(submap_id);
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -105,6 +105,9 @@ class ConstraintBuilder {
   // Returns the number of consecutive finished scans.
   int GetNumFinishedScans();
 
+  // Delete data related to 'submap_id'.
+  void DeleteScanMatcher(const mapping::SubmapId& submap_id);
+
  private:
   struct SubmapScanMatcher {
     const HybridGrid* high_resolution_hybrid_grid;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -93,6 +93,20 @@ void OptimizationProblem::AddTrajectoryNode(
   ++trajectory_data.next_node_index;
 }
 
+void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
+  auto& node_data = node_data_.at(node_id.trajectory_id);
+  CHECK(node_data.erase(node_id.node_index));
+
+  if (!node_data.empty() &&
+      node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
+    const common::Time node_time = node_data.begin()->second.time;
+    auto& imu_data = imu_data_.at(node_id.trajectory_id);
+    while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
+      imu_data.pop_front();
+    }
+  }
+}
+
 void OptimizationProblem::AddSubmap(const int trajectory_id,
                                     const transform::Rigid3d& submap_pose) {
   CHECK_GE(trajectory_id, 0);
@@ -106,6 +120,11 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
   ++trajectory_data.next_submap_index;
 }
 
+void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
+  auto& submap_data = submap_data_.at(submap_id.trajectory_id);
+  CHECK(submap_data.erase(submap_id.submap_index));
+}
+
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
   options_.mutable_ceres_solver_options()->set_max_num_iterations(
       max_num_iterations);

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -73,7 +73,9 @@ class OptimizationProblem {
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid3d& initial_pose,
                          const transform::Rigid3d& pose);
+  void TrimTrajectoryNode(const mapping::NodeId& node_id);
   void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
+  void TrimSubmap(const mapping::SubmapId& submap_id);
 
   void SetMaxNumIterations(int32 max_num_iterations);