From d96aa2105b67cb2e81f514a63c10fa3260fbbdaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Wed, 24 Jan 2018 09:09:55 +0100 Subject: [PATCH] Constraints visualization: Separate inter constraints between separate trajectories (#634) --- .../cartographer_ros/map_builder_bridge.cc | 60 ++++++++++++++----- 1 file changed, 45 insertions(+), 15 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index b73fb3e..77b3044 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -247,15 +247,33 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { // visible. residual_intra_marker.pose.position.z = 0.1; - visualization_msgs::Marker constraint_inter_marker = constraint_intra_marker; - constraint_inter_marker.id = marker_id++; - constraint_inter_marker.ns = "Inter constraints"; - constraint_inter_marker.pose.position.z = 0.1; + visualization_msgs::Marker constraint_inter_same_trajectory_marker = + constraint_intra_marker; + constraint_inter_same_trajectory_marker.id = marker_id++; + constraint_inter_same_trajectory_marker.ns = + "Inter constraints, same trajectory"; + constraint_inter_same_trajectory_marker.pose.position.z = 0.1; - visualization_msgs::Marker residual_inter_marker = constraint_intra_marker; - residual_inter_marker.id = marker_id++; - residual_inter_marker.ns = "Inter residuals"; - residual_inter_marker.pose.position.z = 0.1; + visualization_msgs::Marker residual_inter_same_trajectory_marker = + constraint_intra_marker; + residual_inter_same_trajectory_marker.id = marker_id++; + residual_inter_same_trajectory_marker.ns = + "Inter residuals, same trajectory"; + residual_inter_same_trajectory_marker.pose.position.z = 0.1; + + visualization_msgs::Marker constraint_inter_diff_trajectory_marker = + constraint_intra_marker; + constraint_inter_diff_trajectory_marker.id = marker_id++; + constraint_inter_diff_trajectory_marker.ns = + "Inter constraints, different trajectories"; + constraint_inter_diff_trajectory_marker.pose.position.z = 0.1; + + visualization_msgs::Marker residual_inter_diff_trajectory_marker = + constraint_intra_marker; + residual_inter_diff_trajectory_marker.id = marker_id++; + residual_inter_diff_trajectory_marker.ns = + "Inter residuals, different trajectories"; + residual_inter_diff_trajectory_marker.pose.position.z = 0.1; const auto trajectory_node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); @@ -278,11 +296,21 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { color_residual.a = 1.0; color_residual.r = 1.0; } else { - constraint_marker = &constraint_inter_marker; - residual_marker = &residual_inter_marker; - // Bright yellow - color_constraint.a = 1.0; - color_constraint.r = color_constraint.g = 1.0; + if (constraint.node_id.trajectory_id == + constraint.submap_id.trajectory_id) { + constraint_marker = &constraint_inter_same_trajectory_marker; + residual_marker = &residual_inter_same_trajectory_marker; + // Bright yellow + color_constraint.a = 1.0; + color_constraint.r = color_constraint.g = 1.0; + } else { + constraint_marker = &constraint_inter_diff_trajectory_marker; + residual_marker = &residual_inter_diff_trajectory_marker; + // Bright orange + color_constraint.a = 1.0; + color_constraint.r = 1.0; + color_constraint.g = 165./255.; + } // Bright cyan color_residual.a = 1.0; color_residual.b = color_residual.g = 1.0; @@ -319,8 +347,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() { constraint_list.markers.push_back(constraint_intra_marker); constraint_list.markers.push_back(residual_intra_marker); - constraint_list.markers.push_back(constraint_inter_marker); - constraint_list.markers.push_back(residual_inter_marker); + constraint_list.markers.push_back(constraint_inter_same_trajectory_marker); + constraint_list.markers.push_back(residual_inter_same_trajectory_marker); + constraint_list.markers.push_back(constraint_inter_diff_trajectory_marker); + constraint_list.markers.push_back(residual_inter_diff_trajectory_marker); return constraint_list; }