From b3b9735a4b177401314b1e5d66292c4537ae0606 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Wed, 7 Jun 2017 14:23:58 +0200
Subject: [PATCH] Remove trimmed constraints. (#317)

Still missing marking submaps and nodes as trimmed.

PAIR=SirVer
---
 cartographer/mapping/id.h                    |  2 +
 cartographer/mapping_2d/sparse_pose_graph.cc | 50 +++++++++++++++++++-
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h
index f33fe50..980420c 100644
--- a/cartographer/mapping/id.h
+++ b/cartographer/mapping/id.h
@@ -52,6 +52,8 @@ struct SubmapId {
            std::forward_as_tuple(other.trajectory_id, other.submap_index);
   }
 
+  bool operator!=(const SubmapId& other) const { return !operator==(other); }
+
   bool operator<(const SubmapId& other) const {
     return std::forward_as_tuple(trajectory_id, submap_index) <
            std::forward_as_tuple(other.trajectory_id, other.submap_index);
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 805b304..5e91fa8 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -24,6 +24,7 @@
 #include <iostream>
 #include <limits>
 #include <memory>
+#include <set>
 #include <sstream>
 #include <string>
 
@@ -492,7 +493,54 @@ int SparsePoseGraph::TrimmingHandle::num_submaps(
 
 void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
-  LOG(FATAL) << "Not yet implemented.";
+  // 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);
+  }
+  // TODO(whess): Mark the submap with 'submap_id' as pruned and remove its
+  // data. Also make sure we no longer try to scan match against it.
+
+  // TODO(whess): Mark the 'nodes_to_remove' as pruned and remove their data.
+  // Also make sure we no longer try to scan match against it.
+
+  // TODO(whess): The optimization problem should no longer include the submap
+  // and the removed nodes.
+
+  // 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