Constraints visualization: Separate inter constraints between separate trajectories (#634)
parent
ac15e46727
commit
d96aa2105b
|
@ -247,15 +247,33 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
|
||||||
// visible.
|
// visible.
|
||||||
residual_intra_marker.pose.position.z = 0.1;
|
residual_intra_marker.pose.position.z = 0.1;
|
||||||
|
|
||||||
visualization_msgs::Marker constraint_inter_marker = constraint_intra_marker;
|
visualization_msgs::Marker constraint_inter_same_trajectory_marker =
|
||||||
constraint_inter_marker.id = marker_id++;
|
constraint_intra_marker;
|
||||||
constraint_inter_marker.ns = "Inter constraints";
|
constraint_inter_same_trajectory_marker.id = marker_id++;
|
||||||
constraint_inter_marker.pose.position.z = 0.1;
|
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;
|
visualization_msgs::Marker residual_inter_same_trajectory_marker =
|
||||||
residual_inter_marker.id = marker_id++;
|
constraint_intra_marker;
|
||||||
residual_inter_marker.ns = "Inter residuals";
|
residual_inter_same_trajectory_marker.id = marker_id++;
|
||||||
residual_inter_marker.pose.position.z = 0.1;
|
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 =
|
const auto trajectory_node_poses =
|
||||||
map_builder_->pose_graph()->GetTrajectoryNodePoses();
|
map_builder_->pose_graph()->GetTrajectoryNodePoses();
|
||||||
|
@ -278,11 +296,21 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
|
||||||
color_residual.a = 1.0;
|
color_residual.a = 1.0;
|
||||||
color_residual.r = 1.0;
|
color_residual.r = 1.0;
|
||||||
} else {
|
} else {
|
||||||
constraint_marker = &constraint_inter_marker;
|
if (constraint.node_id.trajectory_id ==
|
||||||
residual_marker = &residual_inter_marker;
|
constraint.submap_id.trajectory_id) {
|
||||||
|
constraint_marker = &constraint_inter_same_trajectory_marker;
|
||||||
|
residual_marker = &residual_inter_same_trajectory_marker;
|
||||||
// Bright yellow
|
// Bright yellow
|
||||||
color_constraint.a = 1.0;
|
color_constraint.a = 1.0;
|
||||||
color_constraint.r = color_constraint.g = 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
|
// Bright cyan
|
||||||
color_residual.a = 1.0;
|
color_residual.a = 1.0;
|
||||||
color_residual.b = color_residual.g = 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(constraint_intra_marker);
|
||||||
constraint_list.markers.push_back(residual_intra_marker);
|
constraint_list.markers.push_back(residual_intra_marker);
|
||||||
constraint_list.markers.push_back(constraint_inter_marker);
|
constraint_list.markers.push_back(constraint_inter_same_trajectory_marker);
|
||||||
constraint_list.markers.push_back(residual_inter_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;
|
return constraint_list;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue