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; }