12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Serialize and deserialize trajectory builder options (#859)

master
Juraj Oršulić 2018-02-05 09:45:43 +01:00 committed by Christoph Schütte
parent df518b80c8
commit 9bebeea742
9 changed files with 173 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer/mapping/map_builder.cc
View File

@ -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<int, int> 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<SubmapId, transform::Rigid3d> 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<NodeId, transform::Rigid3d> 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<proto::TrajectoryBuilderOptionsWithSensorIds>&
MapBuilder::GetAllTrajectoryBuilderOptions() const {
return all_trajectory_builder_options_;
}
} // namespace mapping
} // namespace cartographer

9
cartographer/mapping/map_builder.h
View File

@ -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<proto::TrajectoryBuilderOptionsWithSensorIds>&
GetAllTrajectoryBuilderOptions() const override;
private:
const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_;
@ -79,6 +84,8 @@ class MapBuilder : public MapBuilderInterface {
std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_;
std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
all_trajectory_builder_options_;
};
} // namespace mapping

7
cartographer/mapping/map_builder_interface.h
View File

@ -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<proto::TrajectoryBuilderOptionsWithSensorIds>&
GetAllTrajectoryBuilderOptions() const = 0;
};
} // namespace mapping

23
cartographer/mapping/proto/trajectory_builder_options.proto
View File

@ -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;
}

61
cartographer/mapping/trajectory_builder_interface.cc
View File

@ -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

4
cartographer/mapping/trajectory_builder_interface.h
View File

@ -112,6 +112,10 @@ class TrajectoryBuilderInterface {
std::unique_ptr<mapping::LocalSlamResultData> 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

10
cartographer_grpc/mapping/map_builder_stub.cc
View File

@ -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

7
cartographer_grpc/mapping/map_builder_stub.h
View File

@ -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<grpc::Channel> client_channel_;

10
cartographer_grpc/testing/mock_map_builder.h
View File

@ -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