Skip to content

Commit a3f10c6

Browse files
committed
fix(autoware_auto_perception_rviz_plugin): adjust bbox orientation indicator
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 9801aa0 commit a3f10c6

File tree

1 file changed

+50
-48
lines changed

1 file changed

+50
-48
lines changed

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

+50-48
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
8585
marker_ptr->scale.x = 0.015;
8686
calc_path_line_list(predicted_path, marker_ptr->points, is_simple);
8787
for (size_t k = 0; k < marker_ptr->points.size(); ++k) {
88-
marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0;
88+
marker_ptr->points.at(k).z -= shape.dimensions.z * 0.5;
8989
}
9090
return marker_ptr;
9191
}
@@ -160,8 +160,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr(
160160
// ellipse orientation
161161
marker_ptr->pose.orientation.x = 0.0;
162162
marker_ptr->pose.orientation.y = 0.0;
163-
marker_ptr->pose.orientation.z = std::sin(phi / 2.0);
164-
marker_ptr->pose.orientation.w = std::cos(phi / 2.0);
163+
marker_ptr->pose.orientation.z = std::sin(phi * 0.5);
164+
marker_ptr->pose.orientation.w = std::cos(phi * 0.5);
165165

166166
// ellipse size
167167
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
@@ -378,8 +378,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
378378
// ellipse orientation
379379
marker_ptr->pose.orientation.x = 0.0;
380380
marker_ptr->pose.orientation.y = 0.0;
381-
marker_ptr->pose.orientation.z = std::sin(yaw / 2.0);
382-
marker_ptr->pose.orientation.w = std::cos(yaw / 2.0);
381+
marker_ptr->pose.orientation.z = std::sin(yaw * 0.5);
382+
marker_ptr->pose.orientation.w = std::cos(yaw * 0.5);
383383

384384
// ellipse size
385385
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
@@ -588,9 +588,9 @@ void calc_bounding_box_line_list(
588588
const autoware_auto_perception_msgs::msg::Shape & shape,
589589
std::vector<geometry_msgs::msg::Point> & points)
590590
{
591-
const double length_half = shape.dimensions.x / 2.0;
592-
const double width_half = shape.dimensions.y / 2.0;
593-
const double height_half = shape.dimensions.z / 2.0;
591+
const double length_half = shape.dimensions.x * 0.5;
592+
const double width_half = shape.dimensions.y * 0.5;
593+
const double height_half = shape.dimensions.z * 0.5;
594594
geometry_msgs::msg::Point point;
595595

596596
// bounding box corner points
@@ -612,10 +612,10 @@ void calc_bounding_box_direction_line_list(
612612
std::vector<geometry_msgs::msg::Point> & points)
613613
{
614614
// direction triangle
615-
const double length_half = shape.dimensions.x / 2.0;
616-
const double width_half = shape.dimensions.y / 2.0;
617-
const double height_half = shape.dimensions.z / 2.0;
618-
const double triangle_size_half = shape.dimensions.y / 1.4;
615+
const double length_half = shape.dimensions.x * 0.5;
616+
const double width_half = shape.dimensions.y * 0.5;
617+
const double height_half = shape.dimensions.z * 0.5;
618+
const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x);
619619
geometry_msgs::msg::Point point;
620620

621621
// triangle-shaped direction indicator
@@ -637,22 +637,23 @@ void calc_bounding_box_orientation_line_list(
637637
const autoware_auto_perception_msgs::msg::Shape & shape,
638638
std::vector<geometry_msgs::msg::Point> & points)
639639
{
640-
const double length_half = shape.dimensions.x / 2.0;
641-
const double width_half = shape.dimensions.y / 2.0;
642-
const double height_half = shape.dimensions.z / 2.0;
643-
const double tick_width = width_half * 0.4;
644-
const double tick_length = std::min(tick_width * 1.4, length_half);
640+
const double length_half = shape.dimensions.x * 0.5;
641+
const double width_half = shape.dimensions.y * 0.5;
642+
const double height_half = shape.dimensions.z * 0.5;
643+
const double tick_width = width_half * 0.5;
644+
const double tick_length = std::min(tick_width, length_half);
645645
geometry_msgs::msg::Point point;
646646

647-
// triangle-shaped direction indicator
648-
const double point_list[3][3] = {
649-
{length_half, 0, height_half},
650-
{length_half - tick_length, tick_width, height_half},
651-
{length_half - tick_length, -tick_width, height_half},
647+
// front corner cuts for orientation
648+
const double point_list[4][3] = {
649+
{length_half, width_half - tick_width, height_half},
650+
{length_half - tick_length, width_half, height_half},
651+
{length_half, -width_half + tick_width, height_half},
652+
{length_half - tick_length, -width_half, height_half},
652653
};
653654
const int point_pairs[2][2] = {
654655
{0, 1},
655-
{0, 2},
656+
{2, 3},
656657
};
657658
calc_line_list_from_points(point_list, point_pairs, 2, points);
658659
}
@@ -661,9 +662,9 @@ void calc_2d_bounding_box_bottom_line_list(
661662
const autoware_auto_perception_msgs::msg::Shape & shape,
662663
std::vector<geometry_msgs::msg::Point> & points)
663664
{
664-
const double length_half = shape.dimensions.x / 2.0;
665-
const double width_half = shape.dimensions.y / 2.0;
666-
const double height_half = shape.dimensions.z / 2.0;
665+
const double length_half = shape.dimensions.x * 0.5;
666+
const double width_half = shape.dimensions.y * 0.5;
667+
const double height_half = shape.dimensions.z * 0.5;
667668
geometry_msgs::msg::Point point;
668669

669670
// bounding box corner points
@@ -687,9 +688,9 @@ void calc_2d_bounding_box_bottom_direction_line_list(
687688
const autoware_auto_perception_msgs::msg::Shape & shape,
688689
std::vector<geometry_msgs::msg::Point> & points)
689690
{
690-
const double length_half = shape.dimensions.x / 2.0;
691-
const double width_half = shape.dimensions.y / 2.0;
692-
const double height_half = shape.dimensions.z / 2.0;
691+
const double length_half = shape.dimensions.x * 0.5;
692+
const double width_half = shape.dimensions.y * 0.5;
693+
const double height_half = shape.dimensions.z * 0.5;
693694
const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x);
694695
geometry_msgs::msg::Point point;
695696

@@ -711,22 +712,23 @@ void calc_2d_bounding_box_bottom_orientation_line_list(
711712
const autoware_auto_perception_msgs::msg::Shape & shape,
712713
std::vector<geometry_msgs::msg::Point> & points)
713714
{
714-
const double length_half = shape.dimensions.x / 2.0;
715-
const double width_half = shape.dimensions.y / 2.0;
716-
const double height_half = shape.dimensions.z / 2.0;
717-
const double tick_width = width_half * 0.4;
718-
const double tick_length = std::min(tick_width * 1.4, length_half);
715+
const double length_half = shape.dimensions.x * 0.5;
716+
const double width_half = shape.dimensions.y * 0.5;
717+
const double height_half = shape.dimensions.z * 0.5;
718+
const double tick_width = width_half * 0.5;
719+
const double tick_length = std::min(tick_width, length_half);
719720
geometry_msgs::msg::Point point;
720721

721-
// triangle-shaped direction indicator
722-
const double point_list[3][3] = {
723-
{length_half, 0, -height_half},
724-
{length_half - tick_length, tick_width, -height_half},
725-
{length_half - tick_length, -tick_width, -height_half},
722+
// front corner cuts for orientation
723+
const double point_list[4][3] = {
724+
{length_half, width_half - tick_width, height_half},
725+
{length_half - tick_length, width_half, height_half},
726+
{length_half, -width_half + tick_width, height_half},
727+
{length_half - tick_length, -width_half, height_half},
726728
};
727729
const int point_pairs[2][2] = {
728730
{0, 1},
729-
{0, 2},
731+
{2, 3},
730732
};
731733
calc_line_list_from_points(point_list, point_pairs, 2, points);
732734
}
@@ -836,41 +838,41 @@ void calc_polygon_line_list(
836838
geometry_msgs::msg::Point point;
837839
point.x = shape.footprint.points.at(i).x;
838840
point.y = shape.footprint.points.at(i).y;
839-
point.z = shape.dimensions.z / 2.0;
841+
point.z = shape.dimensions.z * 0.5;
840842
points.push_back(point);
841843
point.x = shape.footprint.points
842844
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
843845
.x;
844846
point.y = shape.footprint.points
845847
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
846848
.y;
847-
point.z = shape.dimensions.z / 2.0;
849+
point.z = shape.dimensions.z * 0.5;
848850
points.push_back(point);
849851
}
850852
for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
851853
geometry_msgs::msg::Point point;
852854
point.x = shape.footprint.points.at(i).x;
853855
point.y = shape.footprint.points.at(i).y;
854-
point.z = -shape.dimensions.z / 2.0;
856+
point.z = -shape.dimensions.z * 0.5;
855857
points.push_back(point);
856858
point.x = shape.footprint.points
857859
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
858860
.x;
859861
point.y = shape.footprint.points
860862
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
861863
.y;
862-
point.z = -shape.dimensions.z / 2.0;
864+
point.z = -shape.dimensions.z * 0.5;
863865
points.push_back(point);
864866
}
865867
for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
866868
geometry_msgs::msg::Point point;
867869
point.x = shape.footprint.points.at(i).x;
868870
point.y = shape.footprint.points.at(i).y;
869-
point.z = shape.dimensions.z / 2.0;
871+
point.z = shape.dimensions.z * 0.5;
870872
points.push_back(point);
871873
point.x = shape.footprint.points.at(i).x;
872874
point.y = shape.footprint.points.at(i).y;
873-
point.z = -shape.dimensions.z / 2.0;
875+
point.z = -shape.dimensions.z * 0.5;
874876
points.push_back(point);
875877
}
876878
}
@@ -889,15 +891,15 @@ void calc_2d_polygon_bottom_line_list(
889891
geometry_msgs::msg::Point point;
890892
point.x = shape.footprint.points.at(i).x;
891893
point.y = shape.footprint.points.at(i).y;
892-
point.z = -shape.dimensions.z / 2.0;
894+
point.z = -shape.dimensions.z * 0.5;
893895
points.push_back(point);
894896
point.x = shape.footprint.points
895897
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
896898
.x;
897899
point.y = shape.footprint.points
898900
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
899901
.y;
900-
point.z = -shape.dimensions.z / 2.0;
902+
point.z = -shape.dimensions.z * 0.5;
901903
points.push_back(point);
902904
}
903905
}

0 commit comments

Comments
 (0)