Trim node data. (#331)

Also, remove trimmed nodes from proto serialization.

PAIR=wohe

cartographer/mapping/sparse_pose_graph.cc

@@ -61,6 +61,36 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
 
+  std::map<NodeId, NodeId> node_id_remapping;  // Due to trimming.
+
+  const auto all_trajectory_nodes = GetTrajectoryNodes();
+  for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
+       ++trajectory_id) {
+    const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
+    auto* trajectory_proto = proto.add_trajectory();
+
+    for (size_t old_node_index = 0;
+         old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
+      const auto& node = single_trajectory_nodes[old_node_index];
+      if (!node.trimmed()) {
+        node_id_remapping[NodeId{trajectory_id, old_node_index}] =
+            NodeId{trajectory_id, trajectory_proto->node_size()};
+        auto* node_proto = trajectory_proto->add_node();
+        node_proto->set_timestamp(
+            common::ToUniversal(node.constant_data->time));
+        *node_proto->mutable_pose() = transform::ToProto(
+            node.pose * node.constant_data->tracking_to_pose);
+      }
+    }
+
+    if (!single_trajectory_nodes.empty()) {
+      for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
+        *trajectory_proto->add_submap()->mutable_pose() =
+            transform::ToProto(transform);
+      }
+    }
+  }
+
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
     *constraint_proto->mutable_relative_pose() =
@@ -73,34 +103,14 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     constraint_proto->mutable_submap_id()->set_submap_index(
         constraint.submap_id.submap_index);
 
+    const NodeId node_id = node_id_remapping.at(constraint.node_id);
     constraint_proto->mutable_scan_id()->set_trajectory_id(
-        constraint.node_id.trajectory_id);
-    constraint_proto->mutable_scan_id()->set_scan_index(
-        constraint.node_id.node_index);
+        node_id.trajectory_id);
+    constraint_proto->mutable_scan_id()->set_scan_index(node_id.node_index);
 
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }
 
-  const auto all_trajectory_nodes = GetTrajectoryNodes();
-  for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
-       ++trajectory_id) {
-    const auto& trajectory_nodes = all_trajectory_nodes[trajectory_id];
-    auto* trajectory_proto = proto.add_trajectory();
-    for (const auto& node : trajectory_nodes) {
-      auto* node_proto = trajectory_proto->add_node();
-      node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
-      *node_proto->mutable_pose() =
-          transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
-    }
-
-    if (!trajectory_nodes.empty()) {
-      for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
-        *trajectory_proto->add_submap()->mutable_pose() =
-            transform::ToProto(transform);
-      }
-    }
-  }
-
   return proto;
 }
 

cartographer/mapping/trajectory_node.h

@@ -47,9 +47,10 @@ struct TrajectoryNode {
   };
 
   common::Time time() const { return constant_data->time; }
+  bool trimmed() const { return constant_data == nullptr; }
 
   // This must be a shared_ptr. If the data is used for visualization while the
-  // node is being deleted, it must survive until all use finishes.
+  // node is being trimmed, it must survive until all use finishes.
   std::shared_ptr<const Data> constant_data;
 
   transform::Rigid3d pose;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -200,7 +200,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
          ++node_index) {
       const mapping::NodeId node_id{static_cast<int>(trajectory_id),
                                     static_cast<int>(node_index)};
-      if (submap_data.node_ids.count(node_id) == 0) {
+      if (!trajectory_nodes_.at(node_id).trimmed() &&
+          submap_data.node_ids.count(node_id) == 0) {
         ComputeConstraint(node_id, submap_id);
       }
     }
@@ -542,14 +543,19 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   // TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
   // there.
 
-  // 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.
+  // 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();
+  }
 
   // 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.
+
+  // TODO(hrapp): Delete related IMU data.
 }
 
 }  // namespace mapping_2d