From c0f476bc88a71176c6b73c4bf739c0a1c326a4c1 Mon Sep 17 00:00:00 2001 From: Francesco Andreoli Date: Fri, 21 Aug 2026 17:34:49 +0200 Subject: [PATCH] Fix Polygon::offsetInward for non-convex polygons and collinear vertices The previous implementation offset each vertex along v1 + v2 (the sum of the unit vectors towards its neighbours) scaled by margin / sin(angle). This is only correct for convex vertices of counter-clockwise ordered polygons: reflex vertices were offset outward instead of inward, and collinear vertices did not move at all because v1 + v2 degenerates to the zero vector (with sin(angle) = 0 additionally producing NaNs). Rework the algorithm following the analysis in #514: offset each vertex along the bisector of the inward normals of its two adjacent edges, at the miter distance margin / cos(alpha / 2). The winding order is determined with the shoelace formula so clockwise polygons are also offset towards the interior. Degenerate inputs (fewer than three vertices, zero area, repeated vertices, spike vertices) now return false and leave the polygon unchanged. The pre-existing convex triangle test passes unchanged. New tests cover the non-convex polygon from the issue (asserting each offset vertex keeps the margin distance to both adjacent original edges on the interior side), collinear vertices, clockwise ordering, outward offset via negative margin, and degenerate inputs. Fixes #514 Co-Authored-By: Claude Fable 5 --- .../include/grid_map_core/Polygon.hpp | 6 +- grid_map_core/src/Polygon.cpp | 74 ++++++++--- grid_map_core/test/PolygonTest.cpp | 121 ++++++++++++++++++ 3 files changed, 184 insertions(+), 17 deletions(-) diff --git a/grid_map_core/include/grid_map_core/Polygon.hpp b/grid_map_core/include/grid_map_core/Polygon.hpp index 9d7ad27a0..88383615d 100644 --- a/grid_map_core/include/grid_map_core/Polygon.hpp +++ b/grid_map_core/include/grid_map_core/Polygon.hpp @@ -157,8 +157,12 @@ class Polygon /*! * Offsets the polygon inward (buffering) by a margin. * Use a negative margin to offset the polygon outward. + * Supports convex and non-convex polygons with clockwise or + * counter-clockwise ordered vertices. * @param margin the margin to offset the polygon by (in [m]). - * @return true if successful, false otherwise. + * @return true if successful, false for degenerate polygons (fewer than + * three vertices, zero area, repeated vertices, or vertices whose adjacent + * edges fold back onto each other). */ bool offsetInward(const double margin); diff --git a/grid_map_core/src/Polygon.cpp b/grid_map_core/src/Polygon.cpp index fff4f500b..4798992dd 100644 --- a/grid_map_core/src/Polygon.cpp +++ b/grid_map_core/src/Polygon.cpp @@ -201,25 +201,67 @@ bool Polygon::thickenLine(const double thickness) bool Polygon::offsetInward(const double margin) { - // Create a list of indices of the neighbours of each vertex. - // TODO(needs_assignment): Assuming counter-clockwise ordered convex polygon. - std::vector neighbourIndices; - const unsigned int n = nVertices(); - neighbourIndices.resize(n); - for (unsigned int i = 0; i < n; ++i) { - neighbourIndices[i] << (i > 0 ? (i - 1) % n : n - 1), (i + 1) % n; + const size_t n = nVertices(); + if (n < 3) { + return false; } - std::vector copy(vertices_); - for (unsigned int i = 0; i < neighbourIndices.size(); ++i) { - Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i]; - Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i]; - v1.normalize(); - v2.normalize(); - const double angle = acos(v1.dot(v2)); - copy[i] += margin / sin(angle) * (v1 + v2); + // Determine the winding order with the shoelace formula, so that clockwise + // and counter-clockwise ordered polygons are both offset towards the + // interior. + double signedAreaTwice = 0.0; + for (size_t i = 0; i < n; ++i) { + const Position & current = vertices_[i]; + const Position & next = vertices_[(i + 1) % n]; + signedAreaTwice += current.x() * next.y() - next.x() * current.y(); + } + if (signedAreaTwice == 0.0) { + return false; + } + const double orientation = signedAreaTwice > 0.0 ? 1.0 : -1.0; + + std::vector offsetVertices(n); + for (size_t i = 0; i < n; ++i) { + const Position & previous = vertices_[(i + n - 1) % n]; + const Position & current = vertices_[i]; + const Position & next = vertices_[(i + 1) % n]; + + Eigen::Vector2d incoming = current - previous; + Eigen::Vector2d outgoing = next - current; + const double incomingNorm = incoming.norm(); + const double outgoingNorm = outgoing.norm(); + if (incomingNorm == 0.0 || outgoingNorm == 0.0) { + return false; + } + incoming /= incomingNorm; + outgoing /= outgoingNorm; + + // Inward normals of the adjacent edges (left-hand normals for + // counter-clockwise ordering). + const Eigen::Vector2d normal1 = + orientation * Eigen::Vector2d(-incoming.y(), incoming.x()); + const Eigen::Vector2d normal2 = + orientation * Eigen::Vector2d(-outgoing.y(), outgoing.x()); + + // The offset vertex keeps the distance `margin` to the lines through both + // adjacent edges: it lies on the bisector of the edge normals at the miter + // distance margin / cos(alpha / 2), where alpha is the angle between the + // normals. This also holds for reflex and collinear vertices, where the + // previous formulation (margin / sin(angle) * (v1 + v2)) offset in the + // wrong direction or degenerated to a zero vector. + Eigen::Vector2d bisector = normal1 + normal2; + const double bisectorNorm = bisector.norm(); // Equals 2 * cos(alpha / 2). + if (bisectorNorm < 1e-12) { + // Spike vertex: the adjacent edges fold back onto each other and the + // miter distance is unbounded. + return false; + } + bisector /= bisectorNorm; + const double cosHalfAngle = 0.5 * bisectorNorm; + offsetVertices[i] = current + margin / cosHalfAngle * bisector; } - vertices_ = copy; + + vertices_ = std::move(offsetVertices); return true; } diff --git a/grid_map_core/test/PolygonTest.cpp b/grid_map_core/test/PolygonTest.cpp index a6cb13b69..96e70efee 100644 --- a/grid_map_core/test/PolygonTest.cpp +++ b/grid_map_core/test/PolygonTest.cpp @@ -227,6 +227,20 @@ TEST(convertToInequalityConstraints, triangle2) EXPECT_NEAR(0.0000, b(2), 1e-4); } +/*! + * Distance of a point to the infinite line through lineStart and lineEnd, + * positive if the point lies to the left of the line direction (the interior + * side for counter-clockwise ordered polygons). + */ +static double signedDistanceToLine( + const grid_map::Position & lineStart, const grid_map::Position & lineEnd, + const grid_map::Position & point) +{ + const Eigen::Vector2d direction = (lineEnd - lineStart).normalized(); + const Eigen::Vector2d toPoint = point - lineStart; + return direction.x() * toPoint.y() - direction.y() * toPoint.x(); +} + TEST(offsetInward, triangle) { grid_map::Polygon polygon({grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 0.0), @@ -240,6 +254,113 @@ TEST(offsetInward, triangle) EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4); } +TEST(offsetInward, nonConvexPolygon) +{ + // Non-convex polygon with collinear vertices from issue #514 + // (counter-clockwise ordered). + const std::vector original{ + grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 1.0), + grid_map::Position(2.0, 1.0), grid_map::Position(3.0, 0.0), + grid_map::Position(3.0, 1.0), grid_map::Position(3.0, 2.0), + grid_map::Position(2.0, 3.0), grid_map::Position(1.0, 3.0), + grid_map::Position(0.0, 2.0), grid_map::Position(0.0, 1.0)}; + const double margin = 0.1; + grid_map::Polygon polygon(original); + ASSERT_TRUE(polygon.offsetInward(margin)); + const size_t n = original.size(); + ASSERT_EQ(n, polygon.nVertices()); + + // Every offset vertex keeps the distance `margin` to the lines through both + // adjacent original edges, on the interior side. + for (size_t i = 0; i < n; ++i) { + const grid_map::Position & previous = original[(i + n - 1) % n]; + const grid_map::Position & current = original[i]; + const grid_map::Position & next = original[(i + 1) % n]; + EXPECT_NEAR(margin, signedDistanceToLine(previous, current, polygon.getVertex(i)), 1e-9); + EXPECT_NEAR(margin, signedDistanceToLine(current, next, polygon.getVertex(i)), 1e-9); + } + + // Collinear vertices (4 and 9) move perpendicularly to their edge. The + // previous implementation left them in place because its offset direction + // (v1 + v2) degenerated to the zero vector. + EXPECT_NEAR(2.9, polygon.getVertex(4).x(), 1e-9); + EXPECT_NEAR(1.0, polygon.getVertex(4).y(), 1e-9); + EXPECT_NEAR(0.1, polygon.getVertex(9).x(), 1e-9); + EXPECT_NEAR(1.0, polygon.getVertex(9).y(), 1e-9); + + // Reflex vertex 1 moves into the interior (upward), where the previous + // implementation moved it outward (downward). + EXPECT_GT(polygon.getVertex(1).y(), original[1].y()); +} + +TEST(offsetInward, collinearVertices) +{ + // Square with an additional collinear vertex on the bottom edge. + grid_map::Polygon polygon({grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 0.0), + grid_map::Position(2.0, 0.0), grid_map::Position(2.0, 2.0), + grid_map::Position(0.0, 2.0)}); + ASSERT_TRUE(polygon.offsetInward(0.1)); + EXPECT_NEAR(0.1, polygon.getVertex(0)(0), 1e-9); + EXPECT_NEAR(0.1, polygon.getVertex(0)(1), 1e-9); + EXPECT_NEAR(1.0, polygon.getVertex(1)(0), 1e-9); + EXPECT_NEAR(0.1, polygon.getVertex(1)(1), 1e-9); + EXPECT_NEAR(1.9, polygon.getVertex(2)(0), 1e-9); + EXPECT_NEAR(0.1, polygon.getVertex(2)(1), 1e-9); + EXPECT_NEAR(1.9, polygon.getVertex(3)(0), 1e-9); + EXPECT_NEAR(1.9, polygon.getVertex(3)(1), 1e-9); + EXPECT_NEAR(0.1, polygon.getVertex(4)(0), 1e-9); + EXPECT_NEAR(1.9, polygon.getVertex(4)(1), 1e-9); +} + +TEST(offsetInward, clockwiseOrder) +{ + // Same triangle as the triangle test, but with clockwise vertex order. + grid_map::Polygon polygon({grid_map::Position(1.0, -1.0), grid_map::Position(0.0, 0.0), + grid_map::Position(1.0, 1.0)}); + ASSERT_TRUE(polygon.offsetInward(0.1)); + EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4); + EXPECT_NEAR(-0.758579, polygon.getVertex(0)(1), 1e-4); + EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4); + EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4); + EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4); + EXPECT_NEAR(0.758579, polygon.getVertex(2)(1), 1e-4); +} + +TEST(offsetInward, outwardWithNegativeMargin) +{ + grid_map::Polygon polygon({grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 0.0), + grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 1.0)}); + ASSERT_TRUE(polygon.offsetInward(-0.1)); + EXPECT_NEAR(-0.1, polygon.getVertex(0)(0), 1e-9); + EXPECT_NEAR(-0.1, polygon.getVertex(0)(1), 1e-9); + EXPECT_NEAR(1.1, polygon.getVertex(1)(0), 1e-9); + EXPECT_NEAR(-0.1, polygon.getVertex(1)(1), 1e-9); + EXPECT_NEAR(1.1, polygon.getVertex(2)(0), 1e-9); + EXPECT_NEAR(1.1, polygon.getVertex(2)(1), 1e-9); + EXPECT_NEAR(-0.1, polygon.getVertex(3)(0), 1e-9); + EXPECT_NEAR(1.1, polygon.getVertex(3)(1), 1e-9); +} + +TEST(offsetInward, degenerateInputs) +{ + // Fewer than three vertices. + grid_map::Polygon line({grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 0.0)}); + EXPECT_FALSE(line.offsetInward(0.1)); + + // Repeated vertex: the edge direction is undefined and the polygon is left + // unchanged. + grid_map::Polygon repeated({grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 0.0), + grid_map::Position(1.0, 0.0), grid_map::Position(1.0, 1.0)}); + EXPECT_FALSE(repeated.offsetInward(0.1)); + EXPECT_NEAR(0.0, repeated.getVertex(0)(0), 1e-9); + EXPECT_NEAR(0.0, repeated.getVertex(0)(1), 1e-9); + + // Zero-area polygon. + grid_map::Polygon degenerate({grid_map::Position(0.0, 0.0), grid_map::Position(1.0, 0.0), + grid_map::Position(2.0, 0.0)}); + EXPECT_FALSE(degenerate.offsetInward(0.1)); +} + TEST(triangulation, triangle) { grid_map::Polygon polygon({grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 0.0),