From 4a1366501d88126cd67aaff86bc29240724977c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 8 Jan 2018 17:07:39 +0100 Subject: [PATCH] Use GetTrajectoryNodePoses and GetAllSubmapPoses in GetConstraintList (#651) --- .../cartographer_ros/map_builder_bridge.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index e42b10d..bb114e5 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -257,9 +257,9 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { residual_inter_marker.ns = "Inter residuals"; residual_inter_marker.pose.position.z = 0.1; - const auto trajectory_nodes = - map_builder_->pose_graph()->GetTrajectoryNodes(); - const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData(); + const auto trajectory_node_poses = + map_builder_->pose_graph()->GetTrajectoryNodePoses(); + const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses(); const auto constraints = map_builder_->pose_graph()->constraints(); for (const auto& constraint : constraints) { @@ -293,13 +293,13 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { residual_marker->colors.push_back(color_residual); } - const auto submap_it = submap_data.find(constraint.submap_id); - if (submap_it == submap_data.end()) { + const auto submap_it = submap_poses.find(constraint.submap_id); + if (submap_it == submap_poses.end()) { continue; } const auto& submap_pose = submap_it->data.pose; - const auto node_it = trajectory_nodes.find(constraint.node_id); - if (node_it == trajectory_nodes.end()) { + const auto node_it = trajectory_node_poses.find(constraint.node_id); + if (node_it == trajectory_node_poses.end()) { continue; } const auto& trajectory_node_pose = node_it->data.global_pose;