Public Types | Public Member Functions | Static Public Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
grid_map::Polygon Class Reference

#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 &center, Length &length) const
 
Position getCentroid () const
 
const std::string & getFrameId () const
 
uint64_t getTimestamp () const
 
const PositiongetVertex (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 Positionoperator[] (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< Polygontriangulate (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. More...
 
uint64_t timestamp_
 Timestamp of the polygon (nanoseconds). More...
 
std::vector< Positionvertices_
 Vertices of the polygon. More...
 

Detailed Description

Definition at line 21 of file Polygon.hpp.

Member Enumeration Documentation

Enumerator
FAN 

Definition at line 25 of file Polygon.hpp.

Constructor & Destructor Documentation

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.

Parameters
verticesthe 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.

Member Function Documentation

void grid_map::Polygon::addVertex ( const Position vertex)

Add a vertex to the polygon

Parameters
vertexthe 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 
)
staticprotected

Returns the 2D cross product of vector1 and vector2.

Parameters
[in]vector1the first input vector.
[in]vector2the 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

Parameters
Athe A matrix in of the inequality constraint.
bthe b matrix in of the inequality constraint.
Returns
true if conversion successful, false otherwise.

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.

Parameters
[in]polygon1the first input polygon.
[in]polygon2the second input polygon.
Returns
convex hull as 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.

Parameters
[in]center1the center position of the first circle.
[in]center2the center position of the second circle.
[in]radiusradius of the circles.
[in]nVerticesnumber of vertices of the approximation polygon. Default = 20.
Returns
convex hull of the two circles as polygon.

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.

Parameters
[in]centerthe center position of the circle.
[in]radiusradius of the circle.
[in]nVerticesnumber of vertices of the approximation polygon. Default = 20.
Returns
circle as polygon.

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.

Returns
area of the polygon.

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.

Parameters
centerthe center of the bounding box.
lengththe 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.

Returns
centroid of polygon.

Definition at line 113 of file Polygon.cpp.

const std::string & grid_map::Polygon::getFrameId ( ) const

Get the frameId of the polygon.

Returns
frameId.

Definition at line 76 of file Polygon.cpp.

uint64_t grid_map::Polygon::getTimestamp ( ) const

Get the timestamp of the polygon.

Returns
timestamp in nanoseconds.

Definition at line 86 of file Polygon.cpp.

const Position & grid_map::Polygon::getVertex ( const size_t  index) const

Get the vertex with index.

Parameters
indexthe index of the requested vertex.
Returns
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.

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.

Parameters
pointthe point to be checked.
Returns
true if inside, false otherwise.

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.

Parameters
[in]pointspoints to use to compute the convex hull used to create the polygon.
Returns
convex hull as polygon.

Definition at line 299 of file Polygon.cpp.

size_t grid_map::Polygon::nVertices ( ) const

Returns the number of vertices.

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.

Parameters
marginthe margin to offset the polygon by (in [m]).
Returns
true if successful, false otherwise.

Definition at line 199 of file Polygon.cpp.

const Position & grid_map::Polygon::operator[] ( const size_t  index) const

Get vertex operator overload.

Parameters
indexthe index of the requested vertex.
Returns
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.

Parameters
frameIdthe 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.

Parameters
timestampthe 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 
)
staticprotected

Returns true if the vector1 and vector2 are sorted lexicographically.

Parameters
[in]vector1the first input vector.
[in]vector2the 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.

Parameters
thicknessthe desired thickness of the line.
Returns
true if successful, false otherwise.

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.

Returns
a list of triangle polygons covering the same 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 
)
staticprotected

Returns true if OAB makes a clockwise turn or if the OA and OB vectors are collinear.

Parameters
[in]pointOpoint of the origin O, used to compute OA and OB.
[in]pointAinput point A, used to compute OA.
[in]pointBinput point B, used to compute OB.

Definition at line 347 of file Polygon.cpp.

Member Data Documentation

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.


The documentation for this class was generated from the following files:


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27