Clean trimming of frozen trajectory data. (#1767)

The deletion logic needs to take care of deleting all data that is
"exclusively" connected to the submaps that are to be removed. This is
achieved by looking at the data that is connected via constraints in
the graph.

Deleting a frozen trajectory (one without optimization constraints)
doesn't work that way and would leave dangling nodes in the graph.

This adds an additional logic that uses the `node_ids` field of the
submap instead of the constraints if the trajectory is frozen.

Signed-off-by: Michael Grupp <grupp@magazino.eu>

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -1210,33 +1210,36 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   CHECK(parent_->data_.submap_data.at(submap_id).state ==
         SubmapState::kFinished);
 
-  // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
+  // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
-  // with 'submap_id' is gone.
+  // once the submap with 'submap_id' is gone.
+  // We need to use node_ids instead of constraints here to be also compatible
+  // with frozen trajectories that don't have intra-constraints.
   std::set<NodeId> nodes_to_retain;
-  for (const Constraint& constraint : parent_->data_.constraints) {
+  for (const auto& submap_data : parent_->data_.submap_data) {
-    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
+    if (submap_data.id != submap_id) {
-        constraint.submap_id != submap_id) {
+      nodes_to_retain.insert(submap_data.data.node_ids.begin(),
-      nodes_to_retain.insert(constraint.node_id);
+                             submap_data.data.node_ids.end());
     }
   }
-  // Remove all 'data_.constraints' related to 'submap_id'.
+
+  // Remove all nodes that are exlusively associated to 'submap_id'.
   std::set<NodeId> nodes_to_remove;
+  std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
+                      parent_->data_.submap_data.at(submap_id).node_ids.end(),
+                      nodes_to_retain.begin(), nodes_to_retain.end(),
+                      std::inserter(nodes_to_remove, nodes_to_remove.begin()));
+
+  // Remove all 'data_.constraints' related to 'submap_id'.
   {
     std::vector<Constraint> constraints;
     for (const Constraint& constraint : parent_->data_.constraints) {
-      if (constraint.submap_id == submap_id) {
+      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_->data_.constraints = std::move(constraints);
   }
+
   // Remove all 'data_.constraints' related to 'nodes_to_remove'.
   // If the removal lets other submaps lose all their inter-submap constraints,
   // delete their corresponding constraint submap matchers to save memory.

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -1190,33 +1190,36 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   CHECK(parent_->data_.submap_data.at(submap_id).state ==
         SubmapState::kFinished);
 
-  // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
+  // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
-  // with 'submap_id' is gone.
+  // once the submap with 'submap_id' is gone.
+  // We need to use node_ids instead of constraints here to be also compatible
+  // with frozen trajectories that don't have intra-constraints.
   std::set<NodeId> nodes_to_retain;
-  for (const Constraint& constraint : parent_->data_.constraints) {
+  for (const auto& submap_data : parent_->data_.submap_data) {
-    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
+    if (submap_data.id != submap_id) {
-        constraint.submap_id != submap_id) {
+      nodes_to_retain.insert(submap_data.data.node_ids.begin(),
-      nodes_to_retain.insert(constraint.node_id);
+                             submap_data.data.node_ids.end());
     }
   }
-  // Remove all 'data_.constraints' related to 'submap_id'.
+
+  // Remove all nodes that are exlusively associated to 'submap_id'.
   std::set<NodeId> nodes_to_remove;
+  std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
+                      parent_->data_.submap_data.at(submap_id).node_ids.end(),
+                      nodes_to_retain.begin(), nodes_to_retain.end(),
+                      std::inserter(nodes_to_remove, nodes_to_remove.begin()));
+
+  // Remove all 'data_.constraints' related to 'submap_id'.
   {
     std::vector<Constraint> constraints;
     for (const Constraint& constraint : parent_->data_.constraints) {
-      if (constraint.submap_id == submap_id) {
+      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_->data_.constraints = std::move(constraints);
   }
+
   // Remove all 'data_.constraints' related to 'nodes_to_remove'.
   // If the removal lets other submaps lose all their inter-submap constraints,
   // delete their corresponding constraint submap matchers to save memory.

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -323,6 +323,59 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) {
   }
 }
 
+TEST_F(PoseGraph3DTest, EvenSubmapTrimmerOnFrozenTrajectory) {
+  BuildPoseGraphWithFakeOptimization();
+  const int trajectory_id = 2;
+  const int num_submaps_to_keep = 10;
+  const int num_submaps_to_create = 2 * num_submaps_to_keep;
+  const int num_nodes_per_submap = 3;
+  for (int i = 0; i < num_submaps_to_create; ++i) {
+    int submap_index = 42 + i;
+    auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index);
+    pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
+    for (int j = 0; j < num_nodes_per_submap; ++j) {
+      int node_index = 7 + num_nodes_per_submap * i + j;
+      auto node = testing::CreateFakeNode(trajectory_id, node_index);
+      pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
+      // This step is normally done by a MapBuilder when loading frozen state.
+      pose_graph_->AddNodeToSubmap(NodeId{trajectory_id, node_index},
+                                   SubmapId{trajectory_id, submap_index});
+    }
+  }
+  pose_graph_->FreezeTrajectory(trajectory_id);
+  pose_graph_->AddTrimmer(absl::make_unique<EvenSubmapTrimmer>(trajectory_id));
+  pose_graph_->WaitForAllComputations();
+  EXPECT_EQ(
+      pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
+      num_submaps_to_create);
+  EXPECT_EQ(
+      pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
+      num_submaps_to_create);
+  EXPECT_EQ(
+      pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
+      num_nodes_per_submap * num_submaps_to_create);
+  EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
+                trajectory_id),
+            num_nodes_per_submap * num_submaps_to_create);
+  EXPECT_EQ(pose_graph_->constraints().size(), 0);
+  for (int i = 0; i < 2; ++i) {
+    pose_graph_->RunFinalOptimization();
+    EXPECT_EQ(
+        pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
+        num_submaps_to_keep);
+    EXPECT_EQ(
+        pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
+        num_submaps_to_keep);
+    EXPECT_EQ(
+        pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
+        num_nodes_per_submap * num_submaps_to_keep);
+    EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
+                  trajectory_id),
+              num_nodes_per_submap * num_submaps_to_keep);
+    EXPECT_EQ(pose_graph_->constraints().size(), 0);
+  }
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/map_builder.cc

@@ -359,7 +359,7 @@ std::map<int, int> MapBuilder::LoadState(
 
   if (load_frozen_state) {
     // Add information about which nodes belong to which submap.
-    // Required for 3D pure localization.
+    // This is required, even without constraints.
     for (const proto::PoseGraph::Constraint& constraint_proto :
          pose_graph_proto.constraint()) {
       if (constraint_proto.tag() !=