refactor(universe_utils): eliminate dependence on Boost.Geometry (aut…

…owarefoundation#8965) * add alt::Polygon2d -> Polygon2d conversion function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * migrate to alt geometry Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * invert orientation of linked list Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * suppress cppcheck unusedFunction error Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * fix parameter to avoid confusion Signed-off-by: mitukou1109 <mitukou1109@gmail.com> --------- Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
1222-takeshi · Sep 27, 2024 · efc87e3 · efc87e3
1 parent 070e4ee
commit efc87e3
Show file tree

Hide file tree

Showing 5 changed files with 68 additions and 33 deletions.
diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp
@@ -112,6 +112,8 @@ class Polygon2d
 
   std::vector<PointList2d> & inners() noexcept { return inners_; }
 
+  autoware::universe_utils::Polygon2d to_boost() const;
+
 protected:
   Polygon2d(const PointList2d & outer, const std::vector<PointList2d> & inners)
   : outer_(outer), inners_(inners)

diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp
@@ -15,26 +15,21 @@
 #ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_
 #define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_
 
-#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+#include "autoware/universe_utils/geometry/alt_geometry.hpp"
 
 #include <optional>
 #include <vector>
 
 namespace autoware::universe_utils
 {
-
-using Polygon2d = autoware::universe_utils::Polygon2d;
-using Point2d = autoware::universe_utils::Point2d;
-using LinearRing2d = autoware::universe_utils::LinearRing2d;
-
 struct LinkedPoint
 {
-  explicit LinkedPoint(const Point2d & point)
+  explicit LinkedPoint(const alt::Point2d & point)
   : pt(point), steiner(false), prev_index(std::nullopt), next_index(std::nullopt)
   {
   }
 
-  Point2d pt;
+  alt::Point2d pt;
   bool steiner;
   std::optional<std::size_t> prev_index;
   std::optional<std::size_t> next_index;
@@ -59,7 +54,7 @@ void split_ear_clipping(
  * @return the last index of the created linked list
  */
 std::size_t linked_list(
-  const LinearRing2d & ring, const bool clockwise, std::size_t & vertices,
+  const alt::PointList2d & ring, const bool clockwise, std::size_t & vertices,
   std::vector<LinkedPoint> & points);
 
 /**
@@ -85,7 +80,7 @@ std::size_t eliminate_hole(
  * @return the updated outer_index after all holes are eliminated
  */
 std::size_t eliminate_holes(
-  const std::vector<LinearRing2d> & inners, std::size_t outer_index, std::size_t & vertices,
+  const std::vector<alt::PointList2d> & inners, std::size_t outer_index, std::size_t & vertices,
   std::vector<LinkedPoint> & points);
 
 /**
@@ -99,6 +94,12 @@ std::size_t eliminate_holes(
  * the final size of `points` vector is: `outer_points + hole_points + 2 * n_holes`.
  * @return A vector of convex triangles representing the triangulated polygon.
  */
+std::vector<alt::ConvexPolygon2d> triangulate(const alt::Polygon2d & polygon);
+
+/**
+ * @brief Boost.Geometry version of triangulate()
+ * @return A vector of convex triangles representing the triangulated polygon.
+ */
 std::vector<Polygon2d> triangulate(const Polygon2d & polygon);
 }  // namespace autoware::universe_utils
 

diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp
@@ -77,6 +77,25 @@ std::optional<Polygon2d> Polygon2d::create(
   return Polygon2d::create(outer, inners);
 }
 
+autoware::universe_utils::Polygon2d Polygon2d::to_boost() const
+{
+  autoware::universe_utils::Polygon2d polygon;
+
+  for (const auto & point : outer_) {
+    polygon.outer().emplace_back(point.x(), point.y());
+  }
+
+  for (const auto & inner : inners_) {
+    autoware::universe_utils::LinearRing2d _inner;
+    for (const auto & point : inner) {
+      _inner.emplace_back(point.x(), point.y());
+    }
+    polygon.inners().push_back(_inner);
+  }
+
+  return polygon;
+}
+
 std::optional<ConvexPolygon2d> ConvexPolygon2d::create(const PointList2d & vertices) noexcept
 {
   ConvexPolygon2d poly(vertices);

diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp
@@ -211,7 +211,7 @@ bool is_valid_diagonal(
 }
 
 std::size_t insert_point(
-  const Point2d & pt, std::vector<LinkedPoint> & points,
+  const alt::Point2d & pt, std::vector<LinkedPoint> & points,
   const std::optional<std::size_t> last_index)
 {
   std::size_t p_idx = points.size();
@@ -236,22 +236,19 @@ std::size_t insert_point(
 }
 
 std::size_t linked_list(
-  const LinearRing2d & ring, const bool clockwise, std::size_t & vertices,
+  const alt::PointList2d & ring, const bool forward, std::size_t & vertices,
   std::vector<LinkedPoint> & points)
 {
-  double sum = 0;
   const std::size_t len = ring.size();
   std::optional<std::size_t> last_index = std::nullopt;
 
-  for (std::size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) {
-    const auto & p1 = ring[i];
-    const auto & p2 = ring[j];
-    sum += (p2.x() - p1.x()) * (p1.y() + p2.y());
-  }
-
-  if (clockwise == (sum > 0)) {
-    for (const auto & point : ring) {
-      last_index = insert_point(point, points, last_index);
+  // create forward linked list if forward is true and ring is counter-clockwise, or
+  //                               forward is false and ring is clockwise
+  // create reverse linked list if forward is true and ring is clockwise, or
+  //                               forward is false and ring is counter-clockwise
+  if (forward == !is_clockwise(ring)) {
+    for (auto it = ring.begin(); it != ring.end(); ++it) {
+      last_index = insert_point(*it, points, last_index);
     }
   } else {
     for (auto it = ring.rbegin(); it != ring.rend(); ++it) {
@@ -417,7 +414,7 @@ std::size_t eliminate_hole(
 }
 
 std::size_t eliminate_holes(
-  const std::vector<LinearRing2d> & inners, std::size_t outer_index, std::size_t & vertices,
+  const std::vector<alt::PointList2d> & inners, std::size_t outer_index, std::size_t & vertices,
   std::vector<LinkedPoint> & points)
 {
   std::vector<std::size_t> queue;
@@ -565,7 +562,7 @@ void ear_clipping_linked(
 }
 
 std::vector<LinkedPoint> perform_triangulation(
-  const Polygon2d & polygon, std::vector<std::size_t> & indices)
+  const alt::Polygon2d & polygon, std::vector<std::size_t> & indices)
 {
   indices.clear();
   std::vector<LinkedPoint> points;
@@ -592,29 +589,43 @@ std::vector<LinkedPoint> perform_triangulation(
   return points;
 }
 
-std::vector<Polygon2d> triangulate(const Polygon2d & poly)
+std::vector<alt::ConvexPolygon2d> triangulate(const alt::Polygon2d & poly)
 {
   std::vector<std::size_t> indices;
   auto points = perform_triangulation(poly, indices);
 
-  std::vector<Polygon2d> triangles;
+  std::vector<alt::ConvexPolygon2d> triangles;
   const std::size_t num_indices = indices.size();
 
   if (num_indices % 3 != 0) {
     throw std::runtime_error("Indices size should be a multiple of 3");
   }
 
   for (std::size_t i = 0; i < num_indices; i += 3) {
-    Polygon2d triangle;
-    triangle.outer().push_back(points[indices[i]].pt);
-    triangle.outer().push_back(points[indices[i + 1]].pt);
-    triangle.outer().push_back(points[indices[i + 2]].pt);
-    triangle.outer().push_back(points[indices[i]].pt);
+    alt::PointList2d vertices;
+    vertices.push_back(points[indices[i]].pt);
+    vertices.push_back(points[indices[i + 1]].pt);
+    vertices.push_back(points[indices[i + 2]].pt);
+    vertices.push_back(points[indices[i]].pt);
 
-    triangles.push_back(triangle);
+    triangles.push_back(alt::ConvexPolygon2d::create(vertices).value());
   }
   points.clear();
   return triangles;
 }
 
+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) {
+    triangles.push_back(alt_triangle.to_boost());
+  }
+  return triangles;
+}
 }  // namespace autoware::universe_utils
diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp
@@ -16,7 +16,7 @@
 
 namespace autoware::universe_utils
 {
-
+// cppcheck-suppress unusedFunction
 visualization_msgs::msg::Marker createDefaultMarker(
   const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id,
   const int32_t type, const geometry_msgs::msg::Vector3 & scale,
@@ -41,6 +41,7 @@ visualization_msgs::msg::Marker createDefaultMarker(
   return marker;
 }
 
+// cppcheck-suppress unusedFunction
 visualization_msgs::msg::Marker createDeletedDefaultMarker(
   const rclcpp::Time & now, const std::string & ns, const int32_t id)
 {
@@ -54,6 +55,7 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker(
   return marker;
 }
 
+// cppcheck-suppress unusedFunction
 void appendMarkerArray(
   const visualization_msgs::msg::MarkerArray & additional_marker_array,
   visualization_msgs::msg::MarkerArray * marker_array,