Avoid uninitialized NodeId, SubmapId (#1223)

Previously, NodeId and SubmapId could be (partially) uninitialized,
for instance like this:

```
NodeId node_id;
SubmapId submap{0}; // uninitialized submap_index
```

This introduces constructors to prevent this.

cartographer/mapping/id.h

@@ -55,6 +55,9 @@ common::Time GetTime(const T& t) {
 // Uniquely identifies a trajectory node using a combination of a unique
 // trajectory ID and a zero-based index of the node inside that trajectory.
 struct NodeId {
+  NodeId(int trajectory_id, int node_index)
+      : trajectory_id(trajectory_id), node_index(node_index) {}
+
   int trajectory_id;
   int node_index;
 
@@ -83,6 +86,9 @@ inline std::ostream& operator<<(std::ostream& os, const NodeId& v) {
 // Uniquely identifies a submap using a combination of a unique trajectory ID
 // and a zero-based index of the submap inside that trajectory.
 struct SubmapId {
+  SubmapId(int trajectory_id, int submap_index)
+      : trajectory_id(trajectory_id), submap_index(submap_index) {}
+
   int trajectory_id;
   int submap_index;
 

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

@@ -385,10 +385,16 @@ void PoseGraph2D::HandleWorkQueue(
       const auto& submap_data = optimization_problem_->submap_data();
       const auto& node_data = optimization_problem_->node_data();
       for (const int trajectory_id : node_data.trajectory_ids()) {
-        trajectory_id_to_last_optimized_node_id[trajectory_id] =
+        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
-            std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
+            submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
-        trajectory_id_to_last_optimized_submap_id[trajectory_id] =
+          continue;
-            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
+        }
+        trajectory_id_to_last_optimized_node_id.emplace(
+            trajectory_id,
+            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
+        trajectory_id_to_last_optimized_submap_id.emplace(
+            trajectory_id,
+            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
       }
     }
     global_slam_optimization_callback_(

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

@@ -402,10 +402,16 @@ void PoseGraph3D::HandleWorkQueue(
       const auto& submap_data = optimization_problem_->submap_data();
       const auto& node_data = optimization_problem_->node_data();
       for (const int trajectory_id : node_data.trajectory_ids()) {
-        trajectory_id_to_last_optimized_node_id[trajectory_id] =
+        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
-            std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
+            submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
-        trajectory_id_to_last_optimized_submap_id[trajectory_id] =
+          continue;
-            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
+        }
+        trajectory_id_to_last_optimized_node_id.emplace(
+            trajectory_id,
+            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
+        trajectory_id_to_last_optimized_submap_id.emplace(
+            trajectory_id,
+            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
       }
     }
     global_slam_optimization_callback_(

cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc

@@ -79,12 +79,12 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
   for (int i = 0; i < 2; ++i) {
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
     for (int j = 0; j < 2; ++j) {
-      constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{},
+      constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{0, 0},
                                               &node_data,
                                               transform::Rigid2d::Identity());
     }
-    constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap, NodeId{},
+    constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap,
-                                                  &node_data);
+                                                  NodeId{0, 0}, &node_data);
     constraint_builder_->NotifyEndOfNode();
     thread_pool_.WaitUntilIdle();
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);

cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc

@@ -87,12 +87,14 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
     for (int j = 0; j < 2; ++j) {
       constraint_builder_->MaybeAddConstraint(
-          submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes,
+          submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
-          transform::Rigid3d::Identity(), transform::Rigid3d::Identity());
+          submap_nodes, transform::Rigid3d::Identity(),
+          transform::Rigid3d::Identity());
     }
     constraint_builder_->MaybeAddGlobalConstraint(
-        submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes,
+        submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
-        Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity());
+        submap_nodes, Eigen::Quaterniond::Identity(),
+        Eigen::Quaterniond::Identity());
     constraint_builder_->NotifyEndOfNode();
     thread_pool_.WaitUntilIdle();
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);