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

Follow googlecartographer/cartographer#338. (#378)

master
Wolfgang Hess 2017-06-14 15:52:10 +02:00 committed by GitHub
parent 8a6a2216d2
commit a125822a0e
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -130,18 +130,18 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
for (int trajectory_id = 0; for (int trajectory_id = 0;
trajectory_id < map_builder_.num_trajectory_builders(); trajectory_id < map_builder_.num_trajectory_builders();
++trajectory_id) { ++trajectory_id) {
const std::vector<cartographer::transform::Rigid3d> submap_transforms =
map_builder_.sparse_pose_graph()->GetSubmapTransforms(trajectory_id);
const cartographer::mapping::Submaps* submaps = const cartographer::mapping::Submaps* submaps =
map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps(); map_builder_.GetTrajectoryBuilder(trajectory_id)->submaps();
CHECK_LE(submap_transforms.size(), submaps->size()); const int num_submaps =
map_builder_.sparse_pose_graph()->num_submaps(trajectory_id);
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list; cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
for (size_t submap_index = 0; submap_index != submap_transforms.size(); for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
++submap_index) {
cartographer_ros_msgs::SubmapEntry submap_entry; cartographer_ros_msgs::SubmapEntry submap_entry;
submap_entry.submap_version = submaps->Get(submap_index)->num_range_data(); submap_entry.submap_version =
submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]); submaps->Get(submap_index)->num_range_data();
submap_entry.pose = ToGeometryMsgPose(
map_builder_.sparse_pose_graph()->GetSubmapTransform(
::cartographer::mapping::SubmapId{trajectory_id, submap_index}));
trajectory_submap_list.submap.push_back(submap_entry); trajectory_submap_list.submap.push_back(submap_entry);
} }
submap_list.trajectory.push_back(trajectory_submap_list); submap_list.trajectory.push_back(trajectory_submap_list);