[GenericPoseGraph] Add a NodeId struct. (#1290)

cartographer/pose_graph/node/imu_calibration_test.cc

@@ -23,7 +23,7 @@ namespace pose_graph {
 namespace {
 
 constexpr char kExpectedNode[] = R"PROTO(
-  id { object_id: "accelerometer" }
+  id { object_id: "accelerometer" timestamp: 1 }
   constant: true
   parameters {
     imu_calibration {
@@ -34,8 +34,8 @@ constexpr char kExpectedNode[] = R"PROTO(
 )PROTO";
 
 TEST(Pose3DTest, ToProto) {
-  ImuCalibration imu_calibration("accelerometer", true, 10,
-                                 Eigen::Quaterniond(0., 1., 2., 3.));
+  ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)},
+                                 true, 10, Eigen::Quaterniond(0., 1., 2., 3.));
   EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
 }
 

cartographer/pose_graph/node/node.cc

@@ -22,7 +22,7 @@ namespace pose_graph {
 proto::Node Node::ToProto() const {
   proto::Node node;
   node.set_constant(constant_);
-  node.mutable_id()->set_object_id(node_id_);
+  *node.mutable_id() = node_id_.ToProto();
   *node.mutable_parameters() = ToParametersProto();
   return node;
 }

cartographer/pose_graph/node/node.h

@@ -17,22 +17,19 @@
 #ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_
 #define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_
 
+#include "cartographer/pose_graph/node/node_id.h"
 #include "cartographer/pose_graph/proto/node.pb.h"
 
-#include <array>
-#include <string>
 #include <vector>
 
 namespace cartographer {
 namespace pose_graph {
 
-// TODO(pifon): Change it to struct { string, timestamp} and introduce the
-// necessary operators.
-using NodeId = std::string;
-
 class Node {
  public:
-  explicit Node(NodeId id, bool constant) : node_id_(id), constant_(constant) {}
+  explicit Node(const NodeId& id, bool constant)
+      : node_id_(id), constant_(constant) {}
+
   ~Node() = default;
 
   Node(const Node&) = delete;
@@ -41,7 +38,6 @@ class Node {
   proto::Node ToProto() const;
 
   const NodeId node_id() const { return node_id_; }
-  void set_node_id(const NodeId& id) { node_id_ = id; }
 
   bool constant() const { return constant_; }
   void set_constant(bool constant) { constant_ = constant; }

cartographer/pose_graph/node/node_id.cc

@@ -0,0 +1,36 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/pose_graph/node/node_id.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+NodeId::NodeId(const std::string& object_id, common::Time time)
+    : object_id(object_id), time(time) {}
+
+NodeId::NodeId(const proto::NodeId& node_id)
+    : NodeId(node_id.object_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_timestamp(common::ToUniversal(time));
+  return node_id;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node/node_id.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_
+
+#include <ostream>
+#include <string>
+
+#include "cartographer/common/time.h"
+#include "cartographer/pose_graph/proto/node.pb.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+struct NodeId {
+  NodeId(const std::string& object_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;
+  // 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);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const NodeId& id) {
+  return os << "(object_id: " << id.object_id << ", time: " << id.time << ")";
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_

cartographer/pose_graph/node/node_id_test.cc

@@ -0,0 +1,52 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/pose_graph/node/node_id.h"
+
+#include <sstream>
+
+#include "cartographer/pose_graph/internal/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+TEST(NodeIdTest, FromProto) {
+  proto::NodeId proto;
+  proto.set_object_id("some_object");
+  proto.set_timestamp(1);
+
+  NodeId node_id(proto);
+  EXPECT_EQ(node_id.object_id, "some_object");
+  EXPECT_EQ(common::ToUniversal(node_id.time), 1);
+}
+
+TEST(NodeIdTest, ToProto) {
+  NodeId node_id{"some_object", common::FromUniversal(1)};
+  EXPECT_THAT(node_id.ToProto(),
+              testing::EqualsProto("object_id: 'some_object' timestamp: 1"));
+}
+
+TEST(NodeIdTest, ToString) {
+  std::stringstream ss;
+  ss << NodeId{"some_object", common::FromUniversal(1)};
+
+  EXPECT_EQ("(object_id: some_object, time: 1)", ss.str());
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node/nodes.h

@@ -30,7 +30,7 @@ struct Nodes {
   // TODO(pifon): Should it really be an std::map or smth else?
   std::map<NodeId, Pose2D> pose_2d_nodes;
   std::map<NodeId, Pose3D> pose_3d_nodes;
-  std::map<NodeId, Pose3D> imu_calibration_nodes;
+  std::map<NodeId, ImuCalibration> imu_calibration_nodes;
 };
 
 }  // namespace pose_graph

cartographer/pose_graph/node/pose_2d_test.cc

@@ -23,7 +23,7 @@ namespace pose_graph {
 namespace {
 
 constexpr char kExpectedNode[] = R"PROTO(
-  id { object_id: "flat_world" }
+  id { object_id: "flat_world" timestamp: 1 }
   constant: true
   parameters {
     pose_2d {
@@ -34,7 +34,8 @@ constexpr char kExpectedNode[] = R"PROTO(
 )PROTO";
 
 TEST(Pose2DTest, ToProto) {
-  Pose2D pose_2d("flat_world", true, Eigen::Vector2d(1., 2.), 5.);
+  Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true,
+                 Eigen::Vector2d(1., 2.), 5.);
   EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode));
 }
 

cartographer/pose_graph/node/pose_3d_test.cc

@@ -23,7 +23,7 @@ namespace pose_graph {
 namespace {
 
 constexpr char kExpectedNode[] = R"PROTO(
-  id { object_id: "bumpy_world" }
+  id { object_id: "bumpy_world" timestamp: 1 }
   constant: true
   parameters {
     pose_3d {
@@ -34,7 +34,8 @@ constexpr char kExpectedNode[] = R"PROTO(
 )PROTO";
 
 TEST(Pose3DTest, ToProto) {
-  Pose3D pose_3d("bumpy_world", true, Eigen::Vector3d(1., 2., 3.),
+  Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true,
+                 Eigen::Vector3d(1., 2., 3.),
                  Eigen::Quaterniond(0., 1., 2., 3.));
   EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
 }