Skip to content

Commit

Permalink
chore: add boost geometry lib for rounded functions on triangle list …
Browse files Browse the repository at this point in the history 
…and line list 2d bounding box

Signed-off-by: KhalilSelyan <[email protected]>
  • Loading branch information
@KhalilSelyan
KhalilSelyan committed May 22, 2024
1 parent 3a5577f commit 647d1e3
Show file tree
Hide file tree
Showing 2 changed files with 178 additions and 22 deletions.
11 changes: 11 additions & 0 deletions ...n/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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);
Expand Down
189 changes: 167 additions & 22 deletions common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -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(
Expand Down

0 comments on commit 647d1e3

Please sign in to comment.