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

Use GetTrajectoryNodePoses and GetAllSubmapPoses in GetConstraintList (#651)

master
Christoph Schütte 2018-01-08 17:07:39 +01:00 committed by Wally B. Feed
parent a6095979aa
commit 4a1366501d
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -257,9 +257,9 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
residual_inter_marker.ns = "Inter residuals"; residual_inter_marker.ns = "Inter residuals";
residual_inter_marker.pose.position.z = 0.1; residual_inter_marker.pose.position.z = 0.1;
const auto trajectory_nodes = const auto trajectory_node_poses =
map_builder_->pose_graph()->GetTrajectoryNodes(); map_builder_->pose_graph()->GetTrajectoryNodePoses();
const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData(); const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
const auto constraints = map_builder_->pose_graph()->constraints(); const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
@ -293,13 +293,13 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
residual_marker->colors.push_back(color_residual); residual_marker->colors.push_back(color_residual);
} }
const auto submap_it = submap_data.find(constraint.submap_id); const auto submap_it = submap_poses.find(constraint.submap_id);
if (submap_it == submap_data.end()) { if (submap_it == submap_poses.end()) {
continue; continue;
} }
const auto& submap_pose = submap_it->data.pose; const auto& submap_pose = submap_it->data.pose;
const auto node_it = trajectory_nodes.find(constraint.node_id); const auto node_it = trajectory_node_poses.find(constraint.node_id);
if (node_it == trajectory_nodes.end()) { if (node_it == trajectory_node_poses.end()) {
continue; continue;
} }
const auto& trajectory_node_pose = node_it->data.global_pose; const auto& trajectory_node_pose = node_it->data.global_pose;