diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index bed8e22..1a679d0 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -170,19 +170,21 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } // Next we serialize all node data. { - const auto node_data = sparse_pose_graph_->GetTrajectoryNodes(); + const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes(); for (int trajectory_id = 0; - trajectory_id != static_cast(node_data.size()); ++trajectory_id) { + trajectory_id != static_cast(trajectory_nodes.size()); + ++trajectory_id) { for (int node_index = 0; - node_index != static_cast(node_data[trajectory_id].size()); + node_index != + static_cast(trajectory_nodes[trajectory_id].size()); ++node_index) { proto::SerializedData proto; - auto* const node_data_proto = proto.mutable_node_data(); + auto* const node_proto = proto.mutable_node(); // TODO(whess): Handle trimmed data. - node_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id); - node_data_proto->mutable_node_id()->set_node_index(node_index); - *node_data_proto->mutable_trajectory_node() = - ToProto(*node_data[trajectory_id][node_index].constant_data); + node_proto->mutable_node_id()->set_trajectory_id(trajectory_id); + node_proto->mutable_node_id()->set_node_index(node_index); + *node_proto->mutable_node_data() = + ToProto(*trajectory_nodes[trajectory_id][node_index].constant_data); // TODO(whess): Only enable optionally? Resulting pbstream files will be // a lot larger now. writer->WriteProto(proto); diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index 60b9015..33bc11d 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/cartographer/mapping/proto/serialization.proto @@ -18,7 +18,7 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/sparse_pose_graph.proto"; import "cartographer/mapping/proto/submap.proto"; -import "cartographer/mapping/proto/trajectory_node.proto"; +import "cartographer/mapping/proto/trajectory_node_data.proto"; message Submap { optional SubmapId submap_id = 1; @@ -26,13 +26,13 @@ message Submap { optional Submap3D submap_3d = 3; } -message NodeData { +message Node { optional NodeId node_id = 1; - optional TrajectoryNode trajectory_node = 5; + optional TrajectoryNodeData node_data = 5; } message SerializedData { optional Submap submap = 1; - optional NodeData node_data = 2; + optional Node node = 2; // TODO(whess): Add IMU data, odometry. } diff --git a/cartographer/mapping/proto/trajectory_node.proto b/cartographer/mapping/proto/trajectory_node_data.proto similarity index 93% rename from cartographer/mapping/proto/trajectory_node.proto rename to cartographer/mapping/proto/trajectory_node_data.proto index 2016b76..8cbc528 100644 --- a/cartographer/mapping/proto/trajectory_node.proto +++ b/cartographer/mapping/proto/trajectory_node_data.proto @@ -19,8 +19,8 @@ package cartographer.mapping.proto; import "cartographer/sensor/proto/sensor.proto"; import "cartographer/transform/proto/transform.proto"; -// Serialized state of a mapping::TrajectoryNode. -message TrajectoryNode { +// Serialized state of a mapping::TrajectoryNode::Data. +message TrajectoryNodeData { optional int64 timestamp = 1; optional transform.proto.Quaterniond gravity_alignment = 2; optional sensor.proto.CompressedPointCloud diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc index 3d7947d..ad2325a 100644 --- a/cartographer/mapping/trajectory_node.cc +++ b/cartographer/mapping/trajectory_node.cc @@ -24,8 +24,8 @@ namespace cartographer { namespace mapping { -proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data) { - proto::TrajectoryNode proto; +proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) { + proto::TrajectoryNodeData proto; proto.set_timestamp(common::ToUniversal(constant_data.time)); *proto.mutable_gravity_alignment() = transform::ToProto(constant_data.gravity_alignment); @@ -47,7 +47,7 @@ proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data) { return proto; } -TrajectoryNode::Data FromProto(const proto::TrajectoryNode& proto) { +TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) { Eigen::VectorXf rotational_scan_matcher_histogram( proto.rotational_scan_matcher_histogram_size()); for (int i = 0; i != proto.rotational_scan_matcher_histogram_size(); ++i) { diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index c7a9279..95c659b 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -22,7 +22,7 @@ #include "Eigen/Core" #include "cartographer/common/time.h" -#include "cartographer/mapping/proto/trajectory_node.pb.h" +#include "cartographer/mapping/proto/trajectory_node_data.pb.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -57,8 +57,8 @@ struct TrajectoryNode { transform::Rigid3d pose; }; -proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data); -TrajectoryNode::Data FromProto(const proto::TrajectoryNode& proto); +proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data); +TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto); } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc index e89a772..141e288 100644 --- a/cartographer/mapping/trajectory_node_test.cc +++ b/cartographer/mapping/trajectory_node_test.cc @@ -20,7 +20,7 @@ #include "Eigen/Core" #include "cartographer/common/time.h" -#include "cartographer/mapping/proto/trajectory_node.pb.h" +#include "cartographer/mapping/proto/trajectory_node_data.pb.h" #include "gtest/gtest.h" namespace cartographer { @@ -37,7 +37,7 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(), Eigen::VectorXf::Unit(20, 4), }; - const proto::TrajectoryNode proto = ToProto(expected); + const proto::TrajectoryNodeData proto = ToProto(expected); const TrajectoryNode::Data actual = FromProto(proto); EXPECT_EQ(expected.time, actual.time); EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment));