Merge branch 'anymal_research/rsl_feature/polygon_from_points' into '…

…master'

Changes for Project:

See merge request anybotics/anybotics!58

GitOrigin-RevId: 201bb1cc36bf5fd6cbfc0f8d6069ad2434ea3777

grid_map_core/include/grid_map_core/Polygon.hpp

@@ -203,6 +203,13 @@ class Polygon
    */
   static Polygon convexHull(Polygon& polygon1, Polygon& polygon2);
 
+  /*!
+   * Computes the convex hull of given points, using Andrew's monotone chain convex hull algorithm, and returns it as polygon.
+   * @param[in] points points to use to compute the convex hull used to create the polygon.
+   * @return convex hull as polygon.
+   */
+  static Polygon monotoneChainConvexHullOfPoints(const std::vector<Position>& points);
+
  protected:
 
   /*!
@@ -221,6 +228,16 @@ class Polygon
   static double computeCrossProduct2D(const Eigen::Vector2d& vector1,
                                       const Eigen::Vector2d& vector2);
 
+  /*!
+   * Returns true if OAB makes a clockwise turn or if the OA and OB vectors are collinear.
+   * @param[in] pointO point of the origin O, used to compute OA and OB.
+   * @param[in] pointA input point A, used to compute OA.
+   * @param[in] pointB input point B, used to compute OB.
+   */
+  static double vectorsMakeClockwiseTurn(const Eigen::Vector2d& pointO,
+                                         const Eigen::Vector2d& pointA,
+                                         const Eigen::Vector2d& pointB);
+
   //! Frame id of the polygon.
   std::string frameId_;
 

grid_map_core/src/Polygon.cpp

@@ -12,6 +12,7 @@
 #include <Eigen/Geometry>
 
 #include <limits>
+#include <algorithm>
 
 namespace grid_map {
 
@@ -292,32 +293,41 @@ Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
   vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
   vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());
 
-  std::vector<Position> hull(vertices.size()+1);
+  return monotoneChainConvexHullOfPoints(vertices);
+}
+
+Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
+{
+  // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
+  if (points.size() <= 3) {
+    return Polygon(points);
+  }
+  std::vector<Position> pointsConvexHull(2 * points.size());
 
   // Sort points lexicographically.
-  std::sort(vertices.begin(), vertices.end(), sortVertices);
+  auto sortedPoints(points);
+  std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);
+
 
   int k = 0;
   // Build lower hull
-  for (int i = 0; i < vertices.size(); ++i) {
-    while (k >= 2
-        && computeCrossProduct2D(hull.at(k - 1) - hull.at(k - 2),
-                                 vertices.at(i) - hull.at(k - 2)) <= 0)
+  for (int i = 0; i < sortedPoints.size(); ++i) {
+    while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
       k--;
-    hull.at(k++) = vertices.at(i);
+    }
+    pointsConvexHull.at(k++) = sortedPoints.at(i);
   }
 
   // Build upper hull.
-  for (int i = vertices.size() - 2, t = k + 1; i >= 0; i--) {
-    while (k >= t
-        && computeCrossProduct2D(hull.at(k - 1) - hull.at(k - 2),
-                                 vertices.at(i) - hull.at(k - 2)) <= 0)
+  for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
+    while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
       k--;
-    hull.at(k++) = vertices.at(i);
+    }
+    pointsConvexHull.at(k++) = sortedPoints.at(i);
   }
-  hull.resize(k - 1);
+  pointsConvexHull.resize(k - 1);
 
-  Polygon polygon(hull);
+  Polygon polygon(pointsConvexHull);
   return polygon;
 }
 
@@ -334,4 +344,11 @@ double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
   return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
 }
 
+double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
+                                         const Eigen::Vector2d &pointA,
+                                         const Eigen::Vector2d &pointB)
+{
+  return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
+}
+
 } /* namespace grid_map */

grid_map_core/test/PolygonTest.cpp

@@ -67,6 +67,39 @@ TEST(Polygon, getBoundingBox)
   EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
 }
 
+TEST(Polygon, convexHullPoints)
+{
+  // Test that points which already create a convex shape (square) can be used to create a convex polygon.
+  std::vector<Position> points1;
+  points1.push_back(Vector2d(0.0, 0.0));
+  points1.push_back(Vector2d(1.0, 0.0));
+  points1.push_back(Vector2d(1.0, 1.0));
+  points1.push_back(Vector2d(0.0, 1.0));
+  Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
+  EXPECT_EQ(4, polygon1.nVertices());
+  EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5)));
+
+  // Test that a random set of points can be used to create a convex polygon.
+  std::vector<Position> points2;
+  points2.push_back(Vector2d(0.0, 0.0));
+  points2.push_back(Vector2d(1.0, 0.0));
+  points2.push_back(Vector2d(2.0, 1.0));
+  points2.push_back(Vector2d(1.0, 2.0));
+  points2.push_back(Vector2d(-1.0, 2.0));
+  points2.push_back(Vector2d(-1.0, -2.0));
+  points2.push_back(Vector2d(0.0, 1.0));
+  points2.push_back(Vector2d(1.0, 1.0));
+  Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
+  EXPECT_EQ(4, polygon2.nVertices());
+  EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
+  EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0)));
+  EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2)));
+  EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75)));
+}
+
 TEST(Polygon, convexHullPolygon)
 {
   Polygon polygon1;