Include all trajectory nodes when serializing without unfinished subm…

…aps. (#1410)
cartographer-project · Sep 5, 2018 · a351a8e · a351a8e
1 parent e318751
commit a351a8e
Show file tree

Hide file tree

Showing 3 changed files with 2 additions and 7 deletions.
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc
@@ -174,7 +174,7 @@ TEST_F(PoseGraph3DTest, SerializationWithUnfinishedSubmaps) {
       pose_graph_->ToProto(/*include_unfinished_submaps=*/false);
   EXPECT_EQ(actual_proto.constraint_size(), 2);
   EXPECT_EQ(actual_proto.trajectory_size(), 1);
-  EXPECT_EQ(actual_proto.trajectory(0).node_size(), 2);
+  EXPECT_EQ(actual_proto.trajectory(0).node_size(), 3);
   EXPECT_EQ(actual_proto.trajectory(0).submap_size(), 1);
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
       proto.constraint(0), actual_proto.constraint(0)));

diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc
@@ -190,11 +190,6 @@ proto::PoseGraph PoseGraph::ToProto(bool include_unfinished_submaps) const {
   for (const auto& node_id_data : GetTrajectoryNodes()) {
     proto::Trajectory* trajectory_proto =
         trajectory(node_id_data.id.trajectory_id);
-    if (!include_unfinished_submaps &&
-        orphaned_nodes.count(node_id_data.id) > 0) {
-      // Skip orphaned trajectory nodes.
-      continue;
-    }
     CHECK(node_id_data.data.constant_data != nullptr);
     auto* const node_proto = trajectory_proto->add_node();
     node_proto->set_node_index(node_id_data.id.node_index);

diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
@@ -141,7 +141,7 @@ class PoseGraphInterface {
   virtual std::vector<Constraint> constraints() const = 0;
 
   // Serializes the constraints and trajectories. If
-  // 'include_unfinished_submaps' is set to 'true', unfinished submps, i.e.
+  // 'include_unfinished_submaps' is set to 'true', unfinished submaps, i.e.
   // submaps that have not yet received all rangefinder data insertions, will
   // be included, otherwise not.
   virtual proto::PoseGraph ToProto(bool include_unfinished_submaps) const = 0;