#include <Polygon.hpp>
Public Types | |
| enum | TriangulationMethods { TriangulationMethods::FAN } |
Public Member Functions | |
| void | addVertex (const Position &vertex) |
| bool | convertToInequalityConstraints (Eigen::MatrixXd &A, Eigen::VectorXd &b) const |
| double | getArea () const |
| void | getBoundingBox (Position ¢er, Length &length) const |
| Position | getCentroid () const |
| const std::string & | getFrameId () const |
| uint64_t | getTimestamp () const |
| const Position & | getVertex (size_t index) const |
| const std::vector< Position > & | getVertices () const |
| bool | isInside (const Position &point) const |
| size_t | nVertices () const |
| bool | offsetInward (double margin) |
| const Position & | operator[] (size_t index) const |
| Polygon () | |
| Polygon (std::vector< Position > vertices) | |
| void | removeVertices () |
| void | resetTimestamp () |
| void | setFrameId (const std::string &frameId) |
| void | setTimestamp (uint64_t timestamp) |
| bool | thickenLine (double thickness) |
| std::vector< Polygon > | triangulate (const TriangulationMethods &method=TriangulationMethods::FAN) const |
Static Public Member Functions | |
| static Polygon | convexHull (Polygon &polygon1, Polygon &polygon2) |
| static Polygon | convexHullOfTwoCircles (Position center1, Position center2, double radius, int nVertices=20) |
| static Polygon | fromCircle (Position center, double radius, int nVertices=20) |
| static Polygon | monotoneChainConvexHullOfPoints (const std::vector< Position > &points) |
Static Protected Member Functions | |
| static double | computeCrossProduct2D (const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2) |
| static bool | sortVertices (const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2) |
| static double | vectorsMakeClockwiseTurn (const Eigen::Vector2d &pointO, const Eigen::Vector2d &pointA, const Eigen::Vector2d &pointB) |
Protected Attributes | |
| std::string | frameId_ |
| Frame id of the polygon. More... | |
| uint64_t | timestamp_ |
| Timestamp of the polygon (nanoseconds). More... | |
| std::vector< Position > | vertices_ |
| Vertices of the polygon. More... | |
Definition at line 21 of file Polygon.hpp.
|
strong |
| Enumerator | |
|---|---|
| FAN | |
Definition at line 25 of file Polygon.hpp.
| grid_map::Polygon::Polygon | ( | ) |
Default constructor.
Definition at line 19 of file Polygon.cpp.
| grid_map::Polygon::Polygon | ( | std::vector< Position > | vertices | ) |
Constructor with vertices.
| vertices | the points of the polygon. |
Definition at line 24 of file Polygon.cpp.
| void grid_map::Polygon::addVertex | ( | const Position & | vertex | ) |
Add a vertex to the polygon
| vertex | the point to be added. |
Definition at line 44 of file Polygon.cpp.
|
staticprotected |
Returns the 2D cross product of vector1 and vector2.
| [in] | vector1 | the first input vector. |
| [in] | vector2 | the second input vector. |
Definition at line 339 of file Polygon.cpp.
| bool grid_map::Polygon::convertToInequalityConstraints | ( | Eigen::MatrixXd & | A, |
| Eigen::VectorXd & | b | ||
| ) | const |
Convert polygon to inequality constraints which most tightly contain the points; i.e., create constraints to bound the convex hull of polygon. The inequality constraints are represented as A and b, a set of constraints such that A*x <= b defining the region of space enclosing the convex hull. Based on the VERT2CON MATLAB method by Michael Kleder: http://www.mathworks.com/matlabcentral/fileexchange/7895-vert2con-vertices-to-constraints
| A | the A matrix in of the inequality constraint. |
| b | the b matrix in of the inequality constraint. |
Definition at line 146 of file Polygon.cpp.
Computes the convex hull of two polygons and returns it as polygon.
| [in] | polygon1 | the first input polygon. |
| [in] | polygon2 | the second input polygon. |
Definition at line 287 of file Polygon.cpp.
|
static |
Approximates two circles with a convex hull and returns it as polygon.
| [in] | center1 | the center position of the first circle. |
| [in] | center2 | the center position of the second circle. |
| [in] | radius | radius of the circles. |
| [in] | nVertices | number of vertices of the approximation polygon. Default = 20. |
Definition at line 261 of file Polygon.cpp.
Approximates a circle with a polygon.
| [in] | center | the center position of the circle. |
| [in] | radius | radius of the circle. |
| [in] | nVertices | number of vertices of the approximation polygon. Default = 20. |
Definition at line 246 of file Polygon.cpp.
| double grid_map::Polygon::getArea | ( | ) | const |
Get the area of the polygon. The polygon has to be "simple", i.e. not crossing itself.
Definition at line 99 of file Polygon.cpp.
Gets the bounding box of the polygon.
| center | the center of the bounding box. |
| length | the side lengths of the bounding box. |
Definition at line 128 of file Polygon.cpp.
| Position grid_map::Polygon::getCentroid | ( | ) | const |
Get the centroid of polygon. The polygon has to be "simple", i.e. not crossing itself.
Definition at line 111 of file Polygon.cpp.
| const std::string & grid_map::Polygon::getFrameId | ( | ) | const |
| uint64_t grid_map::Polygon::getTimestamp | ( | ) | const |
Get the timestamp of the polygon.
Definition at line 84 of file Polygon.cpp.
| const Position & grid_map::Polygon::getVertex | ( | size_t | index | ) | const |
Get the vertex with index.
| index | the index of the requested vertex. |
Definition at line 49 of file Polygon.cpp.
| const std::vector< Position > & grid_map::Polygon::getVertices | ( | ) | const |
Returns the vertices of the polygon.
Definition at line 64 of file Polygon.cpp.
| bool grid_map::Polygon::isInside | ( | const Position & | point | ) | const |
Check if point is inside polygon.
| point | the point to be checked. |
Definition at line 30 of file Polygon.cpp.
|
static |
Computes the convex hull of given points, using Andrew's monotone chain convex hull algorithm, and returns it as polygon.
| [in] | points | points to use to compute the convex hull used to create the polygon. |
Definition at line 297 of file Polygon.cpp.
| size_t grid_map::Polygon::nVertices | ( | ) | const |
Returns the number of vertices.
Definition at line 69 of file Polygon.cpp.
| bool grid_map::Polygon::offsetInward | ( | double | margin | ) |
Offsets the polygon inward (buffering) by a margin. Use a negative margin to offset the polygon outward.
| margin | the margin to offset the polygon by (in [m]). |
Definition at line 197 of file Polygon.cpp.
| const Position & grid_map::Polygon::operator[] | ( | size_t | index | ) | const |
Get vertex operator overload.
| index | the index of the requested vertex. |
Definition at line 59 of file Polygon.cpp.
| void grid_map::Polygon::removeVertices | ( | ) |
Removes all vertices from the polygon.
Definition at line 54 of file Polygon.cpp.
| void grid_map::Polygon::resetTimestamp | ( | ) |
Resets the timestamp of the polygon (to zero).
Definition at line 94 of file Polygon.cpp.
| void grid_map::Polygon::setFrameId | ( | const std::string & | frameId | ) |
Set the frame id of the polygon.
| frameId | the frame id to set. |
Definition at line 79 of file Polygon.cpp.
| void grid_map::Polygon::setTimestamp | ( | uint64_t | timestamp | ) |
Set the timestamp of the polygon.
| timestamp | the timestamp to set (in nanoseconds). |
Definition at line 89 of file Polygon.cpp.
|
staticprotected |
Returns true if the vector1 and vector2 are sorted lexicographically.
| [in] | vector1 | the first input vector. |
| [in] | vector2 | the second input vector. |
Definition at line 332 of file Polygon.cpp.
| bool grid_map::Polygon::thickenLine | ( | double | thickness | ) |
If only two vertices are given, this methods generates a thickened line polygon with four vertices.
| thickness | the desired thickness of the line. |
Definition at line 182 of file Polygon.cpp.
| std::vector< Polygon > grid_map::Polygon::triangulate | ( | const TriangulationMethods & | method = TriangulationMethods::FAN | ) | const |
Return a triangulated version of the polygon.
Definition at line 221 of file Polygon.cpp.
|
staticprotected |
Returns true if OAB makes a clockwise turn or if the OA and OB vectors are collinear.
| [in] | pointO | point of the origin O, used to compute OA and OB. |
| [in] | pointA | input point A, used to compute OA. |
| [in] | pointB | input point B, used to compute OB. |
Definition at line 345 of file Polygon.cpp.
|
protected |
Frame id of the polygon.
Definition at line 237 of file Polygon.hpp.
|
protected |
Timestamp of the polygon (nanoseconds).
Definition at line 240 of file Polygon.hpp.
|
protected |
Vertices of the polygon.
Definition at line 243 of file Polygon.hpp.