#include <Polygon.hpp>
Public Types | |
enum | 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 (const size_t index) const |
const std::vector< Position > & | getVertices () const |
bool | isInside (const Position &point) const |
size_t | nVertices () const |
bool | offsetInward (const double margin) |
const Position & | operator[] (const size_t index) const |
Polygon () | |
Polygon (std::vector< Position > vertices) | |
void | removeVertices () |
void | resetTimestamp () |
void | setFrameId (const std::string &frameId) |
void | setTimestamp (const uint64_t timestamp) |
bool | thickenLine (const double thickness) |
std::vector< Polygon > | triangulate (const TriangulationMethods &method=TriangulationMethods::FAN) const |
virtual | ~Polygon () |
Static Public Member Functions | |
static Polygon | convexHull (Polygon &polygon1, Polygon &polygon2) |
static Polygon | convexHullOfTwoCircles (const Position center1, const Position center2, const double radius, const int nVertices=20) |
static Polygon | fromCircle (const Position center, const double radius, const 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. | |
uint64_t | timestamp_ |
Timestamp of the polygon (nanoseconds). | |
std::vector< Position > | vertices_ |
Vertices of the polygon. |
Definition at line 21 of file Polygon.hpp.
Definition at line 25 of file Polygon.hpp.
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.
grid_map::Polygon::~Polygon | ( | ) | [virtual] |
Destructor.
Definition at line 30 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 46 of file Polygon.cpp.
double grid_map::Polygon::computeCrossProduct2D | ( | const Eigen::Vector2d & | vector1, |
const Eigen::Vector2d & | vector2 | ||
) | [static, protected] |
Returns the 2D cross product of vector1 and vector2.
[in] | vector1 | the first input vector. |
[in] | vector2 | the second input vector. |
Definition at line 341 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 148 of file Polygon.cpp.
Polygon grid_map::Polygon::convexHull | ( | Polygon & | polygon1, |
Polygon & | polygon2 | ||
) | [static] |
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 289 of file Polygon.cpp.
Polygon grid_map::Polygon::convexHullOfTwoCircles | ( | const Position | center1, |
const Position | center2, | ||
const double | radius, | ||
const int | nVertices = 20 |
||
) | [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 263 of file Polygon.cpp.
Polygon grid_map::Polygon::fromCircle | ( | const Position | center, |
const double | radius, | ||
const int | nVertices = 20 |
||
) | [static] |
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 248 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 101 of file Polygon.cpp.
void grid_map::Polygon::getBoundingBox | ( | Position & | center, |
Length & | length | ||
) | const |
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 130 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 113 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 86 of file Polygon.cpp.
const Position & grid_map::Polygon::getVertex | ( | const size_t | index | ) | const |
Get the vertex with index.
index | the index of the requested vertex. |
Definition at line 51 of file Polygon.cpp.
const std::vector< Position > & grid_map::Polygon::getVertices | ( | ) | const |
Returns the vertices of the polygon.
Definition at line 66 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 32 of file Polygon.cpp.
Polygon grid_map::Polygon::monotoneChainConvexHullOfPoints | ( | const std::vector< Position > & | points | ) | [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 299 of file Polygon.cpp.
size_t grid_map::Polygon::nVertices | ( | ) | const |
Returns the number of vertices.
Definition at line 71 of file Polygon.cpp.
bool grid_map::Polygon::offsetInward | ( | const 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 199 of file Polygon.cpp.
const Position & grid_map::Polygon::operator[] | ( | const size_t | index | ) | const |
Get vertex operator overload.
index | the index of the requested vertex. |
Definition at line 61 of file Polygon.cpp.
void grid_map::Polygon::removeVertices | ( | ) |
Removes all vertices from the polygon.
Definition at line 56 of file Polygon.cpp.
void grid_map::Polygon::resetTimestamp | ( | ) |
Resets the timestamp of the polygon (to zero).
Definition at line 96 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 81 of file Polygon.cpp.
void grid_map::Polygon::setTimestamp | ( | const uint64_t | timestamp | ) |
Set the timestamp of the polygon.
timestamp | the timestamp to set (in nanoseconds). |
Definition at line 91 of file Polygon.cpp.
bool grid_map::Polygon::sortVertices | ( | const Eigen::Vector2d & | vector1, |
const Eigen::Vector2d & | vector2 | ||
) | [static, protected] |
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 334 of file Polygon.cpp.
bool grid_map::Polygon::thickenLine | ( | const double | thickness | ) |
If only two verices are given, this methods generates a `thickened` line polygon with four vertices.
thickness | the desired thickness of the line. |
Definition at line 184 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 223 of file Polygon.cpp.
double grid_map::Polygon::vectorsMakeClockwiseTurn | ( | const Eigen::Vector2d & | pointO, |
const Eigen::Vector2d & | pointA, | ||
const Eigen::Vector2d & | pointB | ||
) | [static, protected] |
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 347 of file Polygon.cpp.
std::string grid_map::Polygon::frameId_ [protected] |
Frame id of the polygon.
Definition at line 242 of file Polygon.hpp.
uint64_t grid_map::Polygon::timestamp_ [protected] |
Timestamp of the polygon (nanoseconds).
Definition at line 245 of file Polygon.hpp.
std::vector<Position> grid_map::Polygon::vertices_ [protected] |
Vertices of the polygon.
Definition at line 248 of file Polygon.hpp.