chore: add boost geometry lib for rounded functions on triangle list and line list 2d bounding box

Signed-off-by: KhalilSelyan <khalil@leodrive.ai>

Author: KhalilSelyan

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -32,6 +32,13 @@
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/strategies/buffer.hpp>
+#include <boost/geometry/strategies/transform.hpp>
+#include <boost/polygon/polygon.hpp>
+
 #include <algorithm>
 #include <map>
 #include <string>
@@ -198,6 +205,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_lin
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_triangle_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -549,15 +549,16 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
 {
   auto marker_ptr = std::make_shared<Marker>();
   marker_ptr->ns = std::string("shape");
+  marker_ptr->scale.x = line_width;
 
   using autoware_auto_perception_msgs::msg::Shape;
   if (shape_msg.type == Shape::BOUNDING_BOX) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points);
+    marker_ptr->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+    calc_2d_bounding_box_bottom_triangle_list(shape_msg, marker_ptr->points);
+    marker_ptr->scale.x = 1.0;  // Set scale to 1 for TRIANGLE_LIST
+    marker_ptr->scale.y = 1.0;  // Set scale to 1 for TRIANGLE_LIST
+    marker_ptr->scale.z = 1.0;  // Set scale to 1 for TRIANGLE_LIST
     if (is_orientation_available) {
-      calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points);
-    } else {
-      calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points);
     }
   } else if (shape_msg.type == Shape::CYLINDER) {
     marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
@@ -573,12 +574,82 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
   marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
   marker_ptr->pose = to_pose(centroid, orientation);
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
-  marker_ptr->scale.x = line_width;
+
+  marker_ptr->mesh_use_embedded_materials = false;  // Ensure the material is not embedded
+
   marker_ptr->color = color_rgba;
+  marker_ptr->color.a = 0.75f;
 
   return marker_ptr;
 }
 
+void calc_2d_bounding_box_bottom_triangle_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points)
+{
+  const double length_half = shape.dimensions.x * 0.5;
+  const double width_half = shape.dimensions.y * 0.5;
+  const double height_half = shape.dimensions.z * 0.5;
+
+  // Define Boost.Geometry types
+  using BoostPoint = boost::geometry::model::d2::point_xy<double>;
+  using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
+
+  // Create a rectangle polygon
+  BoostPolygon boost_polygon;
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::correct(boost_polygon);
+
+  // Define buffer strategy with larger rounded corners and smoother approximation
+  boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(
+    0.4);  // Adjust distance as needed
+  boost::geometry::strategy::buffer::join_round join_strategy(
+    16);  // Increase points per circle for smoother roundness
+  boost::geometry::strategy::buffer::end_flat end_strategy;
+  boost::geometry::strategy::buffer::point_circle point_strategy(16);  // Points per circle
+  boost::geometry::strategy::buffer::side_straight side_strategy;
+
+  // Create buffered polygon with rounded corners
+  std::vector<BoostPolygon> buffered_polygons;
+  boost::geometry::buffer(
+    boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy,
+    point_strategy);
+
+  if (buffered_polygons.empty()) {
+    RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output.");
+    return;
+  }
+
+  // Convert buffered polygon to RViz marker points
+  const auto & outer_ring = buffered_polygons.front().outer();
+  std::vector<geometry_msgs::msg::Point> polygon_points;
+
+  for (const auto & point : outer_ring) {
+    geometry_msgs::msg::Point p;
+    p.x = point.x();
+    p.y = point.y();
+    p.z = -height_half;  // Use the bottom part of the shape
+    polygon_points.push_back(p);
+  }
+
+  // Create triangles from polygon points
+  for (size_t i = 1; i < polygon_points.size() - 1; ++i) {
+    // Add front face
+    points.push_back(polygon_points[0]);
+    points.push_back(polygon_points[i]);
+    points.push_back(polygon_points[i + 1]);
+
+    // Add back face
+    points.push_back(polygon_points[0]);
+    points.push_back(polygon_points[i + 1]);
+    points.push_back(polygon_points[i]);
+  }
+}
+
 void calc_line_list_from_points(
   const double point_list[][3], const int point_pairs[][2], const int & num_pairs,
   std::vector<geometry_msgs::msg::Point> & points)
@@ -670,30 +741,104 @@ void calc_bounding_box_orientation_line_list(
   calc_line_list_from_points(point_list, point_pairs, 2, points);
 }
 
+// void calc_2d_bounding_box_bottom_line_list(
+//   const autoware_auto_perception_msgs::msg::Shape & shape,
+//   std::vector<geometry_msgs::msg::Point> & points)
+// {
+//   const double length_half = shape.dimensions.x * 0.5;
+//   const double width_half = shape.dimensions.y * 0.5;
+//   const double height_half = shape.dimensions.z * 0.5;
+//   geometry_msgs::msg::Point point;
+
+//   // bounding box corner points
+//   // top surface, clockwise
+//   const double point_list[4][3] = {
+//     {length_half, width_half, -height_half},
+//     {length_half, -width_half, -height_half},
+//     {-length_half, -width_half, -height_half},
+//     {-length_half, width_half, -height_half},
+//   };
+//   const int point_pairs[4][2] = {
+//     {0, 1},
+//     {1, 2},
+//     {2, 3},
+//     {3, 0},
+//   };
+//   calc_line_list_from_points(point_list, point_pairs, 4, points);
+// }
+
 void calc_2d_bounding_box_bottom_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points)
 {
   const double length_half = shape.dimensions.x * 0.5;
   const double width_half = shape.dimensions.y * 0.5;
   const double height_half = shape.dimensions.z * 0.5;
-  geometry_msgs::msg::Point point;
 
-  // bounding box corner points
-  // top surface, clockwise
-  const double point_list[4][3] = {
-    {length_half, width_half, -height_half},
-    {length_half, -width_half, -height_half},
-    {-length_half, -width_half, -height_half},
-    {-length_half, width_half, -height_half},
-  };
-  const int point_pairs[4][2] = {
-    {0, 1},
-    {1, 2},
-    {2, 3},
-    {3, 0},
-  };
-  calc_line_list_from_points(point_list, point_pairs, 4, points);
+  // Define Boost.Geometry types
+  using BoostPoint = boost::geometry::model::d2::point_xy<double>;
+  using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
+
+  // Create a rectangle polygon
+  BoostPolygon boost_polygon;
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half));
+  boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half));
+  boost::geometry::correct(boost_polygon);
+
+  // Define buffer strategy with rounded corners
+  boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(
+    0.4);  // Adjust distance as needed
+  boost::geometry::strategy::buffer::join_round join_strategy(
+    16);  // Adjust points per circle for roundness
+  boost::geometry::strategy::buffer::end_flat end_strategy;
+  boost::geometry::strategy::buffer::point_circle point_strategy(16);  // Points per circle
+  boost::geometry::strategy::buffer::side_straight side_strategy;
+
+  // Create buffered polygon with rounded corners
+  std::vector<BoostPolygon> buffered_polygons;
+  boost::geometry::buffer(
+    boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy,
+    point_strategy);
+
+  if (buffered_polygons.empty()) {
+    RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output.");
+    return;
+  }
+
+  // Convert buffered polygon to RViz marker points
+  const auto & outer_ring = buffered_polygons.front().outer();
+  for (size_t i = 0; i < outer_ring.size() - 1; ++i) {  // -1 to avoid duplicating the closing point
+    geometry_msgs::msg::Point point;
+    point.x = outer_ring[i].x();
+    point.y = outer_ring[i].y();
+    point.z = -height_half;  // Use the bottom part of the shape
+    points.push_back(point);
+
+    point.x = outer_ring[i + 1].x();
+    point.y = outer_ring[i + 1].y();
+    point.z = -height_half;  // Use the bottom part of the shape
+    points.push_back(point);
+  }
+
+  // Closing the polygon
+  if (!outer_ring.empty()) {
+    geometry_msgs::msg::Point point;
+    point.x = outer_ring.back().x();
+    point.y = outer_ring.back().y();
+    point.z = -height_half;
+    points.push_back(point);
+
+    point.x = outer_ring.front().x();
+    point.y = outer_ring.front().y();
+    point.z = -height_half;
+    points.push_back(point);
+  }
+
+  RCLCPP_INFO(
+    rclcpp::get_logger("ObjectPolygonDisplayBase"), "Marker points size: %zu", points.size());
 }
 
 void calc_2d_bounding_box_bottom_direction_line_list(