[GenericPoseGraph] Add 'group_id' field to NodeId. (#1438)

cartographer/pose_graph/node/node_id.cc

@@ -22,12 +22,18 @@ namespace pose_graph {
 NodeId::NodeId(const std::string& object_id, common::Time time)
     : object_id(object_id), time(time) {}
 
+NodeId::NodeId(const std::string& object_id, const std::string& group_id,
+               common::Time time)
+    : object_id(object_id), group_id(group_id), time(time) {}
+
 NodeId::NodeId(const proto::NodeId& node_id)
-    : NodeId(node_id.object_id(), common::FromUniversal(node_id.timestamp())) {}
+    : NodeId(node_id.object_id(), node_id.group_id(),
+             common::FromUniversal(node_id.timestamp())) {}
 
 proto::NodeId NodeId::ToProto() const {
   proto::NodeId node_id;
   node_id.set_object_id(object_id);
+  node_id.set_group_id(group_id);
   node_id.set_timestamp(common::ToUniversal(time));
   return node_id;
 }

cartographer/pose_graph/node/node_id.h

@@ -28,24 +28,33 @@ namespace pose_graph {
 
 struct NodeId {
   NodeId(const std::string& object_id, common::Time time);
+  NodeId(const std::string& object_id, const std::string& group_id,
+         common::Time time);
   explicit NodeId(const proto::NodeId& node_id);
 
   // Object refers to dynamic/static objects, e.g. robot/landmark/submap poses,
   // IMU calibration, etc.
   std::string object_id;
+  // Id of the group to which the node belongs, e.g. "submap".
+  std::string group_id;
   // Time associated with the object's pose.
   common::Time time;
 
   proto::NodeId ToProto() const;
 };
 
 inline bool operator<(const NodeId& lhs, const NodeId& rhs) {
-  return std::forward_as_tuple(lhs.object_id, lhs.time) <
-         std::forward_as_tuple(rhs.object_id, rhs.time);
+  return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) <
+         std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time);
 }
 
 inline std::ostream& operator<<(std::ostream& os, const NodeId& id) {
-  return os << "(object_id: " << id.object_id << ", time: " << id.time << ")";
+  std::string group_message;
+  if (!id.group_id.empty()) {
+    group_message = ", group_id: " + id.group_id;
+  }
+  return os << "(object_id: " << id.object_id << group_message
+            << ", time: " << id.time << ")";
 }
 
 }  // namespace pose_graph

cartographer/pose_graph/node/node_id_test.cc

@@ -27,26 +27,36 @@ namespace {
 TEST(NodeIdTest, FromProto) {
   proto::NodeId proto;
   proto.set_object_id("some_object");
+  proto.set_group_id("submap");
   proto.set_timestamp(1);
 
   NodeId node_id(proto);
   EXPECT_EQ(node_id.object_id, "some_object");
+  EXPECT_EQ(node_id.group_id, "submap");
   EXPECT_EQ(common::ToUniversal(node_id.time), 1);
 }
 
 TEST(NodeIdTest, ToProto) {
-  NodeId node_id{"some_object", common::FromUniversal(1)};
+  NodeId node_id{"some_object", "submap", common::FromUniversal(1)};
   EXPECT_THAT(node_id.ToProto(),
-              testing::EqualsProto("object_id: 'some_object' timestamp: 1"));
+              testing::EqualsProto(
+                  "object_id: 'some_object' group_id: 'submap' timestamp: 1"));
 }
 
-TEST(NodeIdTest, ToString) {
+TEST(NodeIdTest, ToStringWithoutGroupId) {
   std::stringstream ss;
   ss << NodeId{"some_object", common::FromUniversal(1)};
 
   EXPECT_EQ("(object_id: some_object, time: 1)", ss.str());
 }
 
+TEST(NodeIdTest, ToStringWithGroupId) {
+  std::stringstream ss;
+  ss << NodeId{"some_object", "submap", common::FromUniversal(1)};
+
+  EXPECT_EQ("(object_id: some_object, group_id: submap, time: 1)", ss.str());
+}
+
 }  // namespace
 }  // namespace pose_graph
 }  // namespace cartographer

cartographer/pose_graph/proto/node.proto

@@ -45,7 +45,8 @@ message ImuCalibration {
 
 message NodeId {
   string object_id = 1;
-  int64 timestamp = 2;
+  string group_id = 2;
+  int64 timestamp = 3;
 }
 
 message Parameters {