Skip to content

Fix Polygon::offsetInward for non-convex polygons and collinear vertices #550

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
andreolf wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

Fix Polygon::offsetInward for non-convex polygons and collinear vertices #550

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion grid_map_core/include/grid_map_core/Polygon.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
74 changes: 58 additions & 16 deletions grid_map_core/src/Polygon.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::Array2i> 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<Position> 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<Position> 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;
}

Expand Down
121 changes: 121 additions & 0 deletions grid_map_core/test/PolygonTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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<grid_map::Position> 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),
Expand Down