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),