diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index c5ade1b..bed8e22 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -168,7 +168,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } } } - // Next we serialize all range data. + // Next we serialize all node data. { const auto node_data = sparse_pose_graph_->GetTrajectoryNodes(); for (int trajectory_id = 0; @@ -177,11 +177,12 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { node_index != static_cast(node_data[trajectory_id].size()); ++node_index) { proto::SerializedData proto; - auto* const range_data_proto = proto.mutable_range_data(); + auto* const node_data_proto = proto.mutable_node_data(); // TODO(whess): Handle trimmed data. - range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id); - range_data_proto->mutable_node_id()->set_node_index(node_index); - range_data_proto->mutable_range_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); // 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 5b5b9bf..60b9015 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/sensor/proto/sensor.proto"; +import "cartographer/mapping/proto/trajectory_node.proto"; message Submap { optional SubmapId submap_id = 1; @@ -26,13 +26,13 @@ message Submap { optional Submap3D submap_3d = 3; } -message RangeData { +message NodeData { optional NodeId node_id = 1; - optional sensor.proto.CompressedRangeData range_data = 4; + optional TrajectoryNode trajectory_node = 5; } message SerializedData { optional Submap submap = 1; - optional RangeData range_data = 2; + optional NodeData node_data = 2; // TODO(whess): Add IMU data, odometry. } diff --git a/cartographer/mapping/proto/trajectory_node.proto b/cartographer/mapping/proto/trajectory_node.proto new file mode 100644 index 0000000..2016b76 --- /dev/null +++ b/cartographer/mapping/proto/trajectory_node.proto @@ -0,0 +1,31 @@ +// Copyright 2017 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. + +syntax = "proto2"; + +package cartographer.mapping.proto; + +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/transform.proto"; + +// Serialized state of a mapping::TrajectoryNode. +message TrajectoryNode { + optional int64 timestamp = 1; + optional transform.proto.Quaterniond gravity_alignment = 2; + optional sensor.proto.CompressedPointCloud + filtered_gravity_aligned_point_cloud = 3; + optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4; + optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5; + repeated float rotational_scan_matcher_histogram = 6; +} diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc new file mode 100644 index 0000000..3d7947d --- /dev/null +++ b/cartographer/mapping/trajectory_node.cc @@ -0,0 +1,70 @@ +/* + * Copyright 2017 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/mapping/trajectory_node.h" + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data) { + proto::TrajectoryNode proto; + proto.set_timestamp(common::ToUniversal(constant_data.time)); + *proto.mutable_gravity_alignment() = + transform::ToProto(constant_data.gravity_alignment); + *proto.mutable_filtered_gravity_aligned_point_cloud() = + sensor::CompressedPointCloud( + constant_data.filtered_gravity_aligned_point_cloud) + .ToProto(); + *proto.mutable_high_resolution_point_cloud() = + sensor::CompressedPointCloud(constant_data.high_resolution_point_cloud) + .ToProto(); + *proto.mutable_low_resolution_point_cloud() = + sensor::CompressedPointCloud(constant_data.low_resolution_point_cloud) + .ToProto(); + for (Eigen::VectorXf::Index i = 0; + i != constant_data.rotational_scan_matcher_histogram.size(); ++i) { + proto.add_rotational_scan_matcher_histogram( + constant_data.rotational_scan_matcher_histogram(i)); + } + return proto; +} + +TrajectoryNode::Data FromProto(const proto::TrajectoryNode& 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) { + rotational_scan_matcher_histogram(i) = + proto.rotational_scan_matcher_histogram(i); + } + return TrajectoryNode::Data{ + common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.gravity_alignment()), + sensor::CompressedPointCloud(proto.filtered_gravity_aligned_point_cloud()) + .Decompress(), + sensor::CompressedPointCloud(proto.high_resolution_point_cloud()) + .Decompress(), + sensor::CompressedPointCloud(proto.low_resolution_point_cloud()) + .Decompress(), + rotational_scan_matcher_histogram}; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 3d7ad98..c7a9279 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -22,6 +22,7 @@ #include "Eigen/Core" #include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_node.pb.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -56,6 +57,9 @@ struct TrajectoryNode { transform::Rigid3d pose; }; +proto::TrajectoryNode ToProto(const TrajectoryNode::Data& constant_data); +TrajectoryNode::Data FromProto(const proto::TrajectoryNode& proto); + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc new file mode 100644 index 0000000..e89a772 --- /dev/null +++ b/cartographer/mapping/trajectory_node_test.cc @@ -0,0 +1,56 @@ +/* + * Copyright 2017 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/mapping/trajectory_node.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory_node.pb.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(TrajectoryNodeTest, ToAndFromProto) { + const TrajectoryNode::Data expected{ + common::FromUniversal(42), + Eigen::Quaterniond(1., 2., -3., -4.), + sensor::CompressedPointCloud({{1.f, 2.f, 0.f}, {0.f, 0.f, 1.f}}) + .Decompress(), + sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(), + sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(), + Eigen::VectorXf::Unit(20, 4), + }; + const proto::TrajectoryNode proto = ToProto(expected); + const TrajectoryNode::Data actual = FromProto(proto); + EXPECT_EQ(expected.time, actual.time); + EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment)); + EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud, + actual.filtered_gravity_aligned_point_cloud); + EXPECT_EQ(expected.high_resolution_point_cloud, + actual.high_resolution_point_cloud); + EXPECT_EQ(expected.low_resolution_point_cloud, + actual.low_resolution_point_cloud); + EXPECT_EQ(expected.rotational_scan_matcher_histogram, + actual.rotational_scan_matcher_histogram); +} + +} // namespace +} // namespace mapping +} // namespace cartographer