Drop trajectory_id from TrajectoryNode. (#330)

PAIR=wohe

cartographer/mapping/sparse_pose_graph.cc

@@ -81,7 +81,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }
 
-  for (const auto& trajectory_nodes : GetTrajectoryNodes()) {
+  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();
@@ -91,8 +94,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     }
 
     if (!trajectory_nodes.empty()) {
-      for (const auto& transform : GetSubmapTransforms(
+      for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
-               trajectory_nodes[0].constant_data->trajectory_id)) {
         *trajectory_proto->add_submap()->mutable_pose() =
             transform::ToProto(transform);
       }

cartographer/mapping/trajectory_node.h

@@ -40,9 +40,6 @@ struct TrajectoryNode {
     // Range data in 'pose' frame. Only used in the 3D case.
     sensor::CompressedRangeData range_data_3d;
 
-    // Trajectory this node belongs to.
-    int trajectory_id;
-
     // Transform from the 3D 'tracking' frame to the 'pose' frame of the range
     // data, which contains roll, pitch and height for 2D. In 3D this is always
     // identity.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan(
               mapping::TrajectoryNode::Data{
                   time, range_data_in_pose,
                   Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
-                  trajectory_id, tracking_to_pose}),
+                  tracking_to_pose}),
           optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
@@ -228,7 +228,6 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                  .size())
           : 0};
   const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
-  CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
   for (const mapping::Submap* submap : insertion_submaps) {

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -101,7 +101,7 @@ void SparsePoseGraph::AddScan(
           std::make_shared<const mapping::TrajectoryNode::Data>(
               mapping::TrajectoryNode::Data{
                   time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
-                  sensor::Compress(range_data_in_tracking), trajectory_id,
+                  sensor::Compress(range_data_in_tracking),
                   transform::Rigid3d::Identity()}),
           optimized_pose});
   ++num_trajectory_nodes_;
@@ -245,7 +245,6 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                  .size())
           : 0};
   const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
-  CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
                                           scan_data->time, optimized_pose);
   for (const Submap* submap : insertion_submaps) {