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

master
Alexander Belyaev 2018-10-02 17:07:04 +02:00 committed by Wally B. Feed
parent fa35a21493
commit 8285673abb
4 changed files with 34 additions and 8 deletions

View File

@ -22,12 +22,18 @@ namespace pose_graph {
NodeId::NodeId(const std::string& object_id, common::Time time) NodeId::NodeId(const std::string& object_id, common::Time time)
: object_id(object_id), 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::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 NodeId::ToProto() const {
proto::NodeId node_id; proto::NodeId node_id;
node_id.set_object_id(object_id); node_id.set_object_id(object_id);
node_id.set_group_id(group_id);
node_id.set_timestamp(common::ToUniversal(time)); node_id.set_timestamp(common::ToUniversal(time));
return node_id; return node_id;
} }

View File

@ -28,11 +28,15 @@ namespace pose_graph {
struct NodeId { struct NodeId {
NodeId(const std::string& object_id, common::Time time); 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); explicit NodeId(const proto::NodeId& node_id);
// Object refers to dynamic/static objects, e.g. robot/landmark/submap poses, // Object refers to dynamic/static objects, e.g. robot/landmark/submap poses,
// IMU calibration, etc. // IMU calibration, etc.
std::string object_id; 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. // Time associated with the object's pose.
common::Time time; common::Time time;
@ -40,12 +44,17 @@ struct NodeId {
}; };
inline bool operator<(const NodeId& lhs, const NodeId& rhs) { inline bool operator<(const NodeId& lhs, const NodeId& rhs) {
return std::forward_as_tuple(lhs.object_id, lhs.time) < return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) <
std::forward_as_tuple(rhs.object_id, rhs.time); std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time);
} }
inline std::ostream& operator<<(std::ostream& os, const NodeId& id) { 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 } // namespace pose_graph

View File

@ -27,26 +27,36 @@ namespace {
TEST(NodeIdTest, FromProto) { TEST(NodeIdTest, FromProto) {
proto::NodeId proto; proto::NodeId proto;
proto.set_object_id("some_object"); proto.set_object_id("some_object");
proto.set_group_id("submap");
proto.set_timestamp(1); proto.set_timestamp(1);
NodeId node_id(proto); NodeId node_id(proto);
EXPECT_EQ(node_id.object_id, "some_object"); EXPECT_EQ(node_id.object_id, "some_object");
EXPECT_EQ(node_id.group_id, "submap");
EXPECT_EQ(common::ToUniversal(node_id.time), 1); EXPECT_EQ(common::ToUniversal(node_id.time), 1);
} }
TEST(NodeIdTest, ToProto) { 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(), 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; std::stringstream ss;
ss << NodeId{"some_object", common::FromUniversal(1)}; ss << NodeId{"some_object", common::FromUniversal(1)};
EXPECT_EQ("(object_id: some_object, time: 1)", ss.str()); 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
} // namespace pose_graph } // namespace pose_graph
} // namespace cartographer } // namespace cartographer

View File

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