From 89f0c45de8a0a64e9d01c770213bdf802ce07b43 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 17 Jul 2018 17:40:49 +0200 Subject: [PATCH] [GenericPoseGraph] Add a NodeId struct. (#1290) --- .../pose_graph/node/imu_calibration_test.cc | 6 +-- cartographer/pose_graph/node/node.cc | 2 +- cartographer/pose_graph/node/node.h | 12 ++--- cartographer/pose_graph/node/node_id.cc | 36 +++++++++++++ cartographer/pose_graph/node/node_id.h | 54 +++++++++++++++++++ cartographer/pose_graph/node/node_id_test.cc | 52 ++++++++++++++++++ cartographer/pose_graph/node/nodes.h | 2 +- cartographer/pose_graph/node/pose_2d_test.cc | 5 +- cartographer/pose_graph/node/pose_3d_test.cc | 5 +- 9 files changed, 157 insertions(+), 17 deletions(-) create mode 100644 cartographer/pose_graph/node/node_id.cc create mode 100644 cartographer/pose_graph/node/node_id.h create mode 100644 cartographer/pose_graph/node/node_id_test.cc diff --git a/cartographer/pose_graph/node/imu_calibration_test.cc b/cartographer/pose_graph/node/imu_calibration_test.cc index 4e74e8c..17496ac 100644 --- a/cartographer/pose_graph/node/imu_calibration_test.cc +++ b/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)); } diff --git a/cartographer/pose_graph/node/node.cc b/cartographer/pose_graph/node/node.cc index cb1bc6a..b9293f1 100644 --- a/cartographer/pose_graph/node/node.cc +++ b/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; } diff --git a/cartographer/pose_graph/node/node.h b/cartographer/pose_graph/node/node.h index c5e63d1..46e5b64 100644 --- a/cartographer/pose_graph/node/node.h +++ b/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 -#include #include 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; } diff --git a/cartographer/pose_graph/node/node_id.cc b/cartographer/pose_graph/node/node_id.cc new file mode 100644 index 0000000..c340feb --- /dev/null +++ b/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 diff --git a/cartographer/pose_graph/node/node_id.h b/cartographer/pose_graph/node/node_id.h new file mode 100644 index 0000000..7130211 --- /dev/null +++ b/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 +#include + +#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_ diff --git a/cartographer/pose_graph/node/node_id_test.cc b/cartographer/pose_graph/node/node_id_test.cc new file mode 100644 index 0000000..cd64ba9 --- /dev/null +++ b/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 + +#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 diff --git a/cartographer/pose_graph/node/nodes.h b/cartographer/pose_graph/node/nodes.h index b42e934..f5036d0 100644 --- a/cartographer/pose_graph/node/nodes.h +++ b/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 pose_2d_nodes; std::map pose_3d_nodes; - std::map imu_calibration_nodes; + std::map imu_calibration_nodes; }; } // namespace pose_graph diff --git a/cartographer/pose_graph/node/pose_2d_test.cc b/cartographer/pose_graph/node/pose_2d_test.cc index 0eeaa39..eb6a179 100644 --- a/cartographer/pose_graph/node/pose_2d_test.cc +++ b/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)); } diff --git a/cartographer/pose_graph/node/pose_3d_test.cc b/cartographer/pose_graph/node/pose_3d_test.cc index 2377aba..ab05ada 100644 --- a/cartographer/pose_graph/node/pose_3d_test.cc +++ b/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)); }