Add SubmapToProto to the map builder. (#110)

master
Wolfgang Hess 2016-10-27 16:20:58 +02:00 committed by GitHub
parent a5b81ff159
commit 404bbfcc2b
3 changed files with 35 additions and 0 deletions

View File

@ -76,6 +76,7 @@ google_library(mapping_map_builder
mapping_3d_sparse_pose_graph mapping_3d_sparse_pose_graph
mapping_collated_trajectory_builder mapping_collated_trajectory_builder
mapping_proto_map_builder_options mapping_proto_map_builder_options
mapping_proto_submap_visualization
mapping_sparse_pose_graph mapping_sparse_pose_graph
mapping_submaps mapping_submaps
mapping_trajectory_builder mapping_trajectory_builder

View File

@ -119,6 +119,33 @@ proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
trajectory_ids_); trajectory_ids_);
} }
string MapBuilder::SubmapToProto(const int trajectory_id,
const int submap_index,
proto::SubmapQuery::Response* const response) {
if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) {
return "Requested submap from trajectory " + std::to_string(trajectory_id) +
" but there are only " + std::to_string(num_trajectory_builders()) +
" trajectories.";
}
const Submaps* const submaps =
trajectory_builders_.at(trajectory_id)->submaps();
if (submap_index < 0 || submap_index >= submaps->size()) {
return "Requested submap " + std::to_string(submap_index) +
" from trajectory " + std::to_string(trajectory_id) +
" but there are only " + std::to_string(submaps->size()) +
" submaps in this trajectory.";
}
response->set_submap_version(submaps->Get(submap_index)->end_laser_fan_index);
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());
submaps->SubmapToProto(submap_index, sparse_pose_graph_->GetTrajectoryNodes(),
submap_transforms[submap_index], response);
return "";
}
int MapBuilder::num_trajectory_builders() const { int MapBuilder::num_trajectory_builders() const {
return trajectory_builders_.size(); return trajectory_builders_.size();
} }

View File

@ -29,6 +29,7 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
@ -72,6 +73,12 @@ class MapBuilder {
// Returns the trajectory connectivity. // Returns the trajectory connectivity.
proto::TrajectoryConnectivity GetTrajectoryConnectivity(); proto::TrajectoryConnectivity GetTrajectoryConnectivity();
// Fills the SubmapQuery::Response corresponding to 'submap_index' from
// 'trajectory_id'. Returns an error string on failure, or an empty string on
// success.
string SubmapToProto(int trajectory_id, int submap_index,
proto::SubmapQuery::Response* response);
int num_trajectory_builders() const; int num_trajectory_builders() const;
mapping::SparsePoseGraph* sparse_pose_graph(); mapping::SparsePoseGraph* sparse_pose_graph();