#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.