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

Add map trajectory without a trajectory builder. (#605)

master
Wolfgang Hess 2017-10-19 16:59:04 +02:00 committed by GitHub
parent 7c03467a78
commit a96511464e
2 changed files with 19 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
cartographer/mapping/map_builder.cc
View File

@ -103,6 +103,12 @@ int MapBuilder::AddTrajectoryBuilder(
return trajectory_id; return trajectory_id;
} }
int MapBuilder::AddTrajectoryForDeserialization() {
const int trajectory_id = trajectory_builders_.size();
trajectory_builders_.emplace_back();
return trajectory_id;
}
TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder( TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
const int trajectory_id) const { const int trajectory_id) const {
return trajectory_builders_.at(trajectory_id).get(); return trajectory_builders_.at(trajectory_id).get();
@ -158,8 +164,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
auto* const node_proto = proto.mutable_node(); auto* const node_proto = proto.mutable_node();
node_proto->mutable_node_id()->set_trajectory_id( node_proto->mutable_node_id()->set_trajectory_id(
node_id_data.id.trajectory_id); node_id_data.id.trajectory_id);
node_proto->mutable_node_id()->set_node_index( node_proto->mutable_node_id()->set_node_index(node_id_data.id.node_index);
node_id_data.id.node_index);
*node_proto->mutable_node_data() = *node_proto->mutable_node_data() =
ToProto(*node_id_data.data.constant_data); ToProto(*node_id_data.data.constant_data);
writer->WriteProto(proto); writer->WriteProto(proto);
@ -172,19 +177,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto::SparsePoseGraph pose_graph; proto::SparsePoseGraph pose_graph;
CHECK(reader->ReadProto(&pose_graph)); CHECK(reader->ReadProto(&pose_graph));
// TODO(whess): Not all trajectories should be builders, i.e. support should const int map_trajectory_id = AddTrajectoryForDeserialization();
// be added for trajectories without latest pose, options, etc. Appease the
// trajectory builder for now.
proto::TrajectoryBuilderOptions unused_options;
unused_options.mutable_trajectory_builder_2d_options()
->mutable_submaps_options()
->set_resolution(0.05);
unused_options.mutable_trajectory_builder_3d_options();
const std::unordered_set<string> unused_sensor_ids;
const int map_trajectory_id =
AddTrajectoryBuilder(unused_sensor_ids, unused_options);
FinishTrajectory(map_trajectory_id);
sparse_pose_graph_->FreezeTrajectory(map_trajectory_id); sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
MapById<SubmapId, transform::Rigid3d> submap_poses; MapById<SubmapId, transform::Rigid3d> submap_poses;
@ -199,11 +192,10 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
MapById<NodeId, transform::Rigid3d> node_poses; MapById<NodeId, transform::Rigid3d> node_poses;
for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) { for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
for (const proto::Trajectory::Node& node_proto : for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) {
trajectory_proto.node()) { node_poses.Insert(
node_poses.Insert(NodeId{trajectory_proto.trajectory_id(), NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()},
node_proto.node_index()}, transform::ToRigid3(node_proto.pose()));
transform::ToRigid3(node_proto.pose()));
} }
} }

9
cartographer/mapping/map_builder.h
View File

@ -55,13 +55,18 @@ class MapBuilder {
MapBuilder(const MapBuilder&) = delete; MapBuilder(const MapBuilder&) = delete;
MapBuilder& operator=(const MapBuilder&) = delete; MapBuilder& operator=(const MapBuilder&) = delete;
// Create a new trajectory and return its index. // Creates a new trajectory builder and returns its index.
int AddTrajectoryBuilder( int AddTrajectoryBuilder(
const std::unordered_set<string>& expected_sensor_ids, const std::unordered_set<string>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options); const proto::TrajectoryBuilderOptions& trajectory_options);
// Creates a new trajectory and returns its index. Querying the trajectory
// builder for it will return 'nullptr'.
int AddTrajectoryForDeserialization();
// Returns the TrajectoryBuilder corresponding to the specified // Returns the TrajectoryBuilder corresponding to the specified
// 'trajectory_id'. // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding
// builder.
mapping::TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id) const; mapping::TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id) const;
// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished, // Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,