From 9bebeea742696c9e9d2059fdd34bc89231f2ec2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Mon, 5 Feb 2018 09:45:43 +0100 Subject: [PATCH] Serialize and deserialize trajectory builder options (#859) --- cartographer/mapping/map_builder.cc | 55 ++++++++++++++--- cartographer/mapping/map_builder.h | 9 ++- cartographer/mapping/map_builder_interface.h | 7 ++- .../proto/trajectory_builder_options.proto | 23 +++++++ .../mapping/trajectory_builder_interface.cc | 61 +++++++++++++++++++ .../mapping/trajectory_builder_interface.h | 4 ++ cartographer_grpc/mapping/map_builder_stub.cc | 10 ++- cartographer_grpc/mapping/map_builder_stub.h | 7 ++- cartographer_grpc/testing/mock_map_builder.h | 10 ++- 9 files changed, 173 insertions(+), 13 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 69e3d84..477a725 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -129,12 +129,24 @@ int MapBuilder::AddTrajectoryBuilder( transform::ToRigid3(initial_trajectory_pose.relative_pose()), common::FromUniversal(initial_trajectory_pose.timestamp())); } + proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto; + for (const auto& sensor_id : expected_sensor_ids) { + *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id); + } + *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = + trajectory_options; + all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); return trajectory_id; } -int MapBuilder::AddTrajectoryForDeserialization() { +int MapBuilder::AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) { const int trajectory_id = trajectory_builders_.size(); trajectory_builders_.emplace_back(); + all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); return trajectory_id; } @@ -171,6 +183,18 @@ std::string MapBuilder::SubmapToProto( void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) { // We serialize the pose graph followed by all the data referenced in it. writer->WriteProto(pose_graph_->ToProto()); + // Serialize trajectory builder options. + { + proto::AllTrajectoryBuilderOptions all_builder_options_proto; + for (const auto& options_with_sensor_ids : + all_trajectory_builder_options_) { + *all_builder_options_proto.add_options_with_sensor_ids() = + options_with_sensor_ids; + } + CHECK_EQ(all_trajectory_builder_options_.size(), + all_builder_options_proto.options_with_sensor_ids_size()); + writer->WriteProto(all_builder_options_proto); + } // Next we serialize all submap data. { for (const auto& submap_id_data : pose_graph_->GetAllSubmapData()) { @@ -245,12 +269,20 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) { } void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) { - proto::PoseGraph pose_graph; - CHECK(reader->ReadProto(&pose_graph)); + proto::PoseGraph pose_graph_proto; + CHECK(reader->ReadProto(&pose_graph_proto)); + proto::AllTrajectoryBuilderOptions all_builder_options_proto; + CHECK(reader->ReadProto(&all_builder_options_proto)); + CHECK_EQ(pose_graph_proto.trajectory_size(), + all_builder_options_proto.options_with_sensor_ids_size()); std::map trajectory_remapping; - for (auto& trajectory_proto : *pose_graph.mutable_trajectory()) { - const int new_trajectory_id = AddTrajectoryForDeserialization(); + for (auto& trajectory_proto : *pose_graph_proto.mutable_trajectory()) { + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids( + trajectory_proto.trajectory_id()); + const int new_trajectory_id = + AddTrajectoryForDeserialization(options_with_sensor_ids_proto); CHECK(trajectory_remapping .emplace(trajectory_proto.trajectory_id(), new_trajectory_id) .second) @@ -260,7 +292,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) { } MapById submap_poses; - for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) { + for (const proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { for (const proto::Trajectory::Submap& submap_proto : trajectory_proto.submap()) { submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(), @@ -270,7 +303,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) { } MapById node_poses; - for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) { + for (const proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) { node_poses.Insert( NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()}, @@ -305,7 +339,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) { // Add information about which nodes belong to which submap. for (const proto::PoseGraph::Constraint& constraint_proto : - pose_graph.constraint()) { + pose_graph_proto.constraint()) { if (constraint_proto.tag() != mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) { continue; @@ -327,5 +361,10 @@ int MapBuilder::num_trajectory_builders() const { PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_; } +const std::vector& +MapBuilder::GetAllTrajectoryBuilderOptions() const { + return all_trajectory_builder_options_; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 87c3b54..16fda2a 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -50,7 +50,9 @@ class MapBuilder : public MapBuilderInterface { const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; - int AddTrajectoryForDeserialization() override; + int AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) override; mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( int trajectory_id) const override; @@ -68,6 +70,9 @@ class MapBuilder : public MapBuilderInterface { mapping::PoseGraphInterface* pose_graph() override; + const std::vector& + GetAllTrajectoryBuilderOptions() const override; + private: const proto::MapBuilderOptions options_; common::ThreadPool thread_pool_; @@ -79,6 +84,8 @@ class MapBuilder : public MapBuilderInterface { std::unique_ptr sensor_collator_; std::vector> trajectory_builders_; + std::vector + all_trajectory_builder_options_; }; } // namespace mapping diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index ec19cd1..83a2c43 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -58,7 +58,9 @@ class MapBuilderInterface { // Creates a new trajectory and returns its index. Querying the trajectory // builder for it will return 'nullptr'. - virtual int AddTrajectoryForDeserialization() = 0; + virtual int AddTrajectoryForDeserialization( + const proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) = 0; // Returns the 'TrajectoryBuilderInterface' corresponding to the specified // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding @@ -84,6 +86,9 @@ class MapBuilderInterface { virtual int num_trajectory_builders() const = 0; virtual mapping::PoseGraphInterface* pose_graph() = 0; + + virtual const std::vector& + GetAllTrajectoryBuilderOptions() const = 0; }; } // namespace mapping diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 18b72ae..0b26119 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -34,3 +34,26 @@ message TrajectoryBuilderOptions { bool pure_localization = 3; InitialTrajectoryPose initial_trajectory_pose = 4; } + +message SensorId { + enum SensorType { + RANGE = 0; + IMU = 1; + ODOMETRY = 2; + FIXED_FRAME_POSE = 3; + LANDMARK = 4; + LOCAL_SLAM_RESULT = 5; + } + + SensorType type = 1; + string id = 2; +} + +message TrajectoryBuilderOptionsWithSensorIds { + repeated SensorId sensor_id = 1; + TrajectoryBuilderOptions trajectory_builder_options = 2; +} + +message AllTrajectoryBuilderOptions { + repeated TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids = 1; +} diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 0c3cc35..29789f9 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -37,5 +37,66 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( return options; } +proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id) { + proto::SensorId sensor_id_proto; + switch (sensor_id.type) { + case TrajectoryBuilderInterface::SensorId::SensorType::RANGE: + sensor_id_proto.set_type(proto::SensorId::RANGE); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::IMU: + sensor_id_proto.set_type(proto::SensorId::IMU); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY: + sensor_id_proto.set_type(proto::SensorId::ODOMETRY); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE: + sensor_id_proto.set_type(proto::SensorId::FIXED_FRAME_POSE); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK: + sensor_id_proto.set_type(proto::SensorId::LANDMARK); + break; + case TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT: + sensor_id_proto.set_type(proto::SensorId::LOCAL_SLAM_RESULT); + break; + default: + LOG(FATAL) << "Unsupported sensor type."; + } + sensor_id_proto.set_id(sensor_id.id); + return sensor_id_proto; +} + +TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& sensor_id_proto) { + TrajectoryBuilderInterface::SensorId sensor_id; + switch (sensor_id_proto.type()) { + case proto::SensorId::RANGE: + sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::RANGE; + break; + case proto::SensorId::IMU: + sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::IMU; + break; + case proto::SensorId::ODOMETRY: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY; + break; + case proto::SensorId::FIXED_FRAME_POSE: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE; + break; + case proto::SensorId::LANDMARK: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK; + break; + case proto::SensorId::LOCAL_SLAM_RESULT: + sensor_id.type = + TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT; + break; + default: + LOG(FATAL) << "Unsupported sensor type."; + } + sensor_id.id = sensor_id_proto.id(); + return sensor_id; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index cc220bd..fa9002d 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -112,6 +112,10 @@ class TrajectoryBuilderInterface { std::unique_ptr local_slam_result_data) = 0; }; +proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id); +TrajectoryBuilderInterface::SensorId FromProto( + const proto::SensorId& sensor_id_proto); + } // namespace mapping } // namespace cartographer diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc index 1b021e2..f1fcc72 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -59,7 +59,9 @@ int MapBuilderStub::AddTrajectoryBuilder( return client.response().trajectory_id(); } -int MapBuilderStub::AddTrajectoryForDeserialization() { +int MapBuilderStub::AddTrajectoryForDeserialization( + const cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) { LOG(FATAL) << "Not implemented"; } @@ -128,5 +130,11 @@ cartographer::mapping::PoseGraphInterface* MapBuilderStub::pose_graph() { return &pose_graph_stub_; } +const std::vector< + cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>& +MapBuilderStub::GetAllTrajectoryBuilderOptions() const { + LOG(FATAL) << "Not implemented"; +} + } // namespace mapping } // namespace cartographer_grpc diff --git a/cartographer_grpc/mapping/map_builder_stub.h b/cartographer_grpc/mapping/map_builder_stub.h index f9f7447..0a74dd1 100644 --- a/cartographer_grpc/mapping/map_builder_stub.h +++ b/cartographer_grpc/mapping/map_builder_stub.h @@ -39,7 +39,9 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface { const cartographer::mapping::proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; - int AddTrajectoryForDeserialization() override; + int AddTrajectoryForDeserialization( + const cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds& + options_with_sensor_ids_proto) override; cartographer::mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( int trajectory_id) const override; void FinishTrajectory(int trajectory_id) override; @@ -51,6 +53,9 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface { void LoadMap(cartographer::io::ProtoStreamReaderInterface* reader) override; int num_trajectory_builders() const override; cartographer::mapping::PoseGraphInterface* pose_graph() override; + const std::vector< + cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>& + GetAllTrajectoryBuilderOptions() const override; private: std::shared_ptr client_channel_; diff --git a/cartographer_grpc/testing/mock_map_builder.h b/cartographer_grpc/testing/mock_map_builder.h index 1906e2e..aeec670 100644 --- a/cartographer_grpc/testing/mock_map_builder.h +++ b/cartographer_grpc/testing/mock_map_builder.h @@ -37,7 +37,10 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface { &trajectory_options, cartographer::mapping::MapBuilderInterface::LocalSlamResultCallback local_slam_result_callback)); - MOCK_METHOD0(AddTrajectoryForDeserialization, int()); + MOCK_METHOD1(AddTrajectoryForDeserialization, + int(const cartographer::mapping::proto:: + TrajectoryBuilderOptionsWithSensorIds + &options_with_sensor_ids_proto)); MOCK_CONST_METHOD1( GetTrajectoryBuilder, cartographer::mapping::TrajectoryBuilderInterface *(int trajectory_id)); @@ -51,6 +54,11 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface { MOCK_METHOD1(LoadMap, void(cartographer::io::ProtoStreamReaderInterface *)); MOCK_CONST_METHOD0(num_trajectory_builders, int()); MOCK_METHOD0(pose_graph, cartographer::mapping::PoseGraphInterface *()); + MOCK_CONST_METHOD0( + GetAllTrajectoryBuilderOptions, + const std::vector< + cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds> + &()); }; } // namespace testing