fix(autoware_utils): address self-intersecting polygons in random_con…

…cave_generator and handle empty inners() during triangulation (#8995)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>

common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_concave_polygon.hpp

@@ -17,15 +17,17 @@
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <optional>
 #include <vector>
+
 namespace autoware::universe_utils
 {
 /// @brief generate a random non-convex polygon
 /// @param vertices number of vertices for the desired polygon
 /// @param max points will be generated in the range [-max, max]
 /// @details algorithm from
 /// https://digitalscholarship.unlv.edu/cgi/viewcontent.cgi?article=3183&context=thesesdissertations
-Polygon2d random_concave_polygon(const size_t vertices, const double max);
+std::optional<Polygon2d> random_concave_polygon(const size_t vertices, const double max);
 
 /// @brief checks for collisions between two vectors of convex polygons using a specified collision
 /// detection algorithm

common/autoware_universe_utils/src/geometry/alt_geometry.cpp

@@ -68,6 +68,9 @@ std::optional<Polygon2d> Polygon2d::create(
   std::vector<PointList2d> inners;
   for (const auto & inner : polygon.inners()) {
     PointList2d _inner;
+    if (inner.empty()) {
+      continue;
+    }
     for (const auto & point : inner) {
       _inner.push_back(Point2d(point));
     }

common/autoware_universe_utils/src/geometry/ear_clipping.cpp

@@ -420,6 +420,9 @@ std::size_t eliminate_holes(
   std::vector<std::size_t> queue;
 
   for (const auto & ring : inners) {
+    if (ring.empty()) {
+      continue;
+    }
     auto inner_index = linked_list(ring, false, vertices, points);
 
     if (points[inner_index].next_index.value() == inner_index) {
@@ -617,10 +620,6 @@ std::vector<alt::ConvexPolygon2d> triangulate(const alt::Polygon2d & poly)
 std::vector<Polygon2d> triangulate(const Polygon2d & poly)
 {
   const auto alt_poly = alt::Polygon2d::create(poly);
-  if (!alt_poly.has_value()) {
-    return {};
-  }
-
   const auto alt_triangles = triangulate(alt_poly.value());
   std::vector<Polygon2d> triangles;
   for (const auto & alt_triangle : alt_triangles) {

common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp

@@ -19,6 +19,7 @@
 #include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/is_valid.hpp>
 #include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>
 
 #include <random>
@@ -138,30 +139,23 @@ bool intersecting(const Edge & e, const Polygon2d & polygon)
 }
 
 /// @brief checks if an edge is valid for a given polygon and set of points
-bool is_valid(const Edge & e, const Polygon2d & P, const std::vector<Point2d> & Q)
+bool is_valid(const Edge & e, const Polygon2d & P, const std::list<Point2d> & Q)
 {
-  bool valid = false;
-  size_t i = 0;
-
-  while (!valid && i < Q.size()) {
-    const Point2d & q = Q[i];
+  for (const Point2d & q : Q) {
     Edge e1 = {e.first, q};
     Edge e2 = {q, e.second};
     bool intersects_e1 = intersecting(e1, P);
     bool intersects_e2 = intersecting(e2, P);
-
-    if (!intersects_e1 && !intersects_e2) {
-      valid = true;
+    if (intersects_e1 || intersects_e2) {
+      return false;
     }
-
-    ++i;
   }
 
-  return valid;
+  return true;
 }
 
 /// @brief finds the nearest node from a set of points to an edge
-Point2d get_nearest_node(const std::vector<Point2d> & Q, const Edge & e)
+Point2d get_nearest_node(const std::list<Point2d> & Q, const Edge & e)
 {
   double min_distance = std::numeric_limits<double>::max();
   Point2d nearest_node(0, 0);
@@ -178,7 +172,7 @@ Point2d get_nearest_node(const std::vector<Point2d> & Q, const Edge & e)
 }
 
 /// @brief finds the edge that is closest to the given set of points
-Edge get_breaking_edge(const PolygonWithEdges & polygon_with_edges, const std::vector<Point2d> & Q)
+Edge get_breaking_edge(const PolygonWithEdges & polygon_with_edges, const std::list<Point2d> & Q)
 {
   double min_distance = std::numeric_limits<double>::max();
   Edge e_breaking;
@@ -229,7 +223,7 @@ void insert_node(PolygonWithEdges & polygon_with_edges, const Point2d & w, const
 }
 
 /// @brief removes a node from a set of points
-void remove_node(std::vector<Point2d> & Q, const Point2d & w)
+void remove_node(std::list<Point2d> & Q, const Point2d & w)
 {
   const double epsilon = 1e-9;
 
@@ -243,7 +237,7 @@ void remove_node(std::vector<Point2d> & Q, const Point2d & w)
 }
 
 /// @brief marks edges as valid if they are valid according to the polygon and points
-void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::vector<Point2d> & Q)
+void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::list<Point2d> & Q)
 {
   for (auto & edge : polygon_with_edges.edges) {
     if (is_valid(edge, polygon_with_edges.polygon, Q)) {
@@ -256,8 +250,7 @@ void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::vector<P
 Polygon2d inward_denting(LinearRing2d & ring)
 {
   LinearRing2d convex_ring;
-  std::vector<Point2d> q;
-  q.reserve(ring.size());
+  std::list<Point2d> q;
   boost::geometry::strategy::convex_hull::graham_andrew<LinearRing2d, Point2d> strategy;
   boost::geometry::convex_hull(ring, convex_ring, strategy);
   PolygonWithEdges polygon_with_edges;
@@ -342,7 +335,7 @@ bool test_intersection(
   return false;
 }
 
-Polygon2d random_concave_polygon(const size_t vertices, const double max)
+std::optional<Polygon2d> random_concave_polygon(const size_t vertices, const double max)
 {
   if (vertices < 4) {
     return Polygon2d();
@@ -376,7 +369,7 @@ Polygon2d random_concave_polygon(const size_t vertices, const double max)
     // apply inward denting algorithm
     poly = inward_denting(points);
     // check for convexity
-    if (!is_convex(poly)) {
+    if (!is_convex(poly) && boost::geometry::is_valid(poly) && poly.outer().size() != vertices) {
       is_non_convex = true;
     }
     LinearRing2d poly_outer = poly.outer();

common/autoware_universe_utils/test/src/geometry/test_geometry.cpp

@@ -2021,6 +2021,105 @@ TEST(geometry, intersectPolygonRand)
   }
 }
 
+double calculate_total_polygon_area(
+  const std::vector<autoware::universe_utils::Polygon2d> & polygons)
+{
+  double totalArea = 0.0;
+  for (const auto & polygon : polygons) {
+    totalArea += boost::geometry::area(polygon);
+  }
+  return totalArea;
+}
+
+TEST(geometry, PolygonTriangulation)
+{
+  using autoware::universe_utils::Polygon2d;
+  using autoware::universe_utils::triangulate;
+
+  {  // concave polygon
+    Polygon2d poly;
+
+    poly.outer().emplace_back(0.0, 0.0);
+    poly.outer().emplace_back(4.0, 0.0);
+    poly.outer().emplace_back(4.0, 4.0);
+    poly.outer().emplace_back(2.0, 2.0);
+    poly.outer().emplace_back(0.0, 4.0);
+    boost::geometry::correct(poly);
+
+    const auto triangles = triangulate(poly);
+
+    const auto triangle_area = calculate_total_polygon_area(triangles);
+    const auto poly_area = boost::geometry::area(poly);
+    EXPECT_NEAR(triangle_area, poly_area, epsilon);
+  }
+
+  {  // concave polygon with empty inners
+    Polygon2d poly;
+
+    poly.outer().emplace_back(0.0, 0.0);
+    poly.outer().emplace_back(4.0, 0.0);
+    poly.outer().emplace_back(4.0, 4.0);
+    poly.outer().emplace_back(2.0, 2.0);
+    poly.outer().emplace_back(0.0, 4.0);
+    boost::geometry::correct(poly);
+
+    poly.inners().emplace_back();
+
+    const auto triangles = triangulate(poly);
+
+    const auto triangle_area = calculate_total_polygon_area(triangles);
+    const auto poly_area = boost::geometry::area(poly);
+    EXPECT_NEAR(triangle_area, poly_area, epsilon);
+  }
+
+  {  // concave polygon with hole
+    Polygon2d poly;
+
+    poly.outer().emplace_back(0.0, 0.0);
+    poly.outer().emplace_back(4.0, 0.0);
+    poly.outer().emplace_back(4.0, 4.0);
+    poly.outer().emplace_back(2.0, 2.0);
+    poly.outer().emplace_back(0.0, 4.0);
+
+    poly.inners().emplace_back();
+    poly.inners().back().emplace_back(1.0, 1.0);
+    poly.inners().back().emplace_back(1.5, 1.0);
+    poly.inners().back().emplace_back(1.5, 1.5);
+    poly.inners().back().emplace_back(1.0, 1.5);
+    boost::geometry::correct(poly);
+
+    const auto triangles = triangulate(poly);
+
+    const auto triangle_area = calculate_total_polygon_area(triangles);
+    const auto poly_area = boost::geometry::area(poly);
+    EXPECT_NEAR(triangle_area, poly_area, epsilon);
+  }
+
+  {  // concave polygon with one empty inner followed by one hole
+    Polygon2d poly;
+
+    poly.outer().emplace_back(0.0, 0.0);
+    poly.outer().emplace_back(4.0, 0.0);
+    poly.outer().emplace_back(4.0, 4.0);
+    poly.outer().emplace_back(2.0, 2.0);
+    poly.outer().emplace_back(0.0, 4.0);
+
+    poly.inners().emplace_back();
+    poly.inners().emplace_back();
+    poly.inners().back().emplace_back(1.0, 1.0);
+    poly.inners().back().emplace_back(1.5, 1.0);
+    poly.inners().back().emplace_back(1.5, 1.5);
+    poly.inners().back().emplace_back(1.0, 1.5);
+    boost::geometry::correct(poly);
+
+    const auto triangles = triangulate(poly);
+
+    const auto triangle_area = calculate_total_polygon_area(triangles);
+    const auto poly_area = boost::geometry::area(poly);
+    EXPECT_NEAR(triangle_area, poly_area, epsilon);
+  }
+}
+
 TEST(geometry, intersectPolygonWithHoles)
 {
   using autoware::universe_utils::Polygon2d;
@@ -2246,7 +2345,10 @@ TEST(geometry, intersectConcavePolygonRand)
     triangulations.clear();
 
     for (auto i = 0; i < polygons_nb; ++i) {
-      polygons.push_back(autoware::universe_utils::random_concave_polygon(vertices, max_values));
+      auto polygon_opt = autoware::universe_utils::random_concave_polygon(vertices, max_values);
+      if (polygon_opt.has_value()) {
+        polygons.push_back(polygon_opt.value());
+      }
     }
 
     for (const auto & polygon : polygons) {