Autoware.Auto
autoware::common::geometry Namespace Reference

Namespaces

 bounding_box
 Functions and types for generating enclosing bounding boxes around a set of points.
 
 details
 Contains computation geometry functions not intended for the end user to directly use.
 
 point_adapter
 Temporary namespace for point adapter methods, for use with nonstandard point types.
 
 spatial_hash
 All objects related to the spatial hash data structure for efficient near neighbor lookup.
 

Classes

class  Interval
 Data structure to contain scalar interval bounds. More...
 

Typedefs

using Point = geometry_msgs::msg::Point32
 
typedef Interval< autoware::common::types::float64_tInterval_d
 
typedef Interval< autoware::common::types::float32_tInterval_f
 

Functions

template<typename T1 , typename T2 , typename T3 >
auto ccw (const T1 &pt, const T2 &q, const T3 &r)
 compute whether line segment rp is counter clockwise relative to line segment qp More...
 
template<typename T1 , typename T2 >
auto cross_2d (const T1 &pt, const T2 &q)
 compute p x q = p1 * q2 - p2 * q1 More...
 
template<typename T1 , typename T2 >
auto dot_2d (const T1 &pt, const T2 &q)
 compute p * q = p1 * q1 + p2 * q2 More...
 
template<typename T >
minus_2d (const T &p, const T &q)
 Compute the 2d difference between two points, p - q. More...
 
template<typename T >
minus_2d (const T &p)
 The unary minus or negation operator applied to a single point's 2d fields. More...
 
template<typename T >
plus_2d (const T &p, const T &q)
 The 2d addition operation, p + q. More...
 
template<typename T >
times_2d (const T &p, const float32_t a)
 The scalar multiplication operation, p * a. More...
 
template<typename T >
intersection_2d (const T &pt, const T &u, const T &q, const T &v)
 solve p + t * u = q + s * v Ref: https://stackoverflow.com/questions/563198/ whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect More...
 
template<typename T >
void rotate_2d (T &pt, const float32_t cos_th, const float32_t sin_th)
 rotate point given precomputed sin and cos More...
 
template<typename T >
rotate_2d (const T &pt, const float32_t th_rad)
 rotate by radian angle th in z direction with ccw positive More...
 
template<typename T >
get_normal (const T &pt)
 compute q s.t. p T q, or p * q = 0 This is the equivalent of a 90 degree ccw rotation More...
 
template<typename T >
auto norm_2d (const T &pt)
 get magnitude of x and y components: More...
 
template<typename T >
closest_segment_point_2d (const T &p, const T &q, const T &r)
 Compute the closest point on line segment p-q to point r Based on equations from https://stackoverflow.com/a/1501725 and http://paulbourke.net/geometry/pointlineplane/. More...
 
template<typename T >
closest_line_point_2d (const T &p, const T &q, const T &r)
 Compute the closest point on the line going through p-q to point r. More...
 
template<typename T >
auto point_line_segment_distance_2d (const T &p, const T &q, const T &r)
 Compute the distance from line segment p-q to point r. More...
 
template<typename T >
make_unit_vector2d (float th)
 Make a 2D unit vector given an angle. More...
 
template<typename OUT = float32_t, typename T1 , typename T2 >
OUT squared_distance_2d (const T1 &a, const T2 &b)
 Compute squared euclidean distance between two points. More...
 
template<typename OUT = float32_t, typename T1 , typename T2 >
OUT distance_2d (const T1 &a, const T2 &b)
 Compute euclidean distance between two points. More...
 
template<typename T >
auto check_point_position_to_line_2d (const T &p1, const T &p2, const T &q)
 Check the given point's position relative the infinite line passing from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() More...
 
template<typename IT >
bool all_ordered (const IT begin, const IT end) noexcept
 
template<typename IT >
auto area_2d (const IT begin, const IT end) noexcept
 
template<typename IT >
auto area_checked_2d (const IT begin, const IT end)
 
template<typename IteratorType , typename PointType >
bool is_point_inside_polygon_2d (const IteratorType &start_it, const IteratorType &end_it, const PointType &p)
 Check if the given point is inside or on the edge of the given polygon. More...
 
template<typename T1 , typename T2 >
auto dot_3d (const T1 &pt, const T2 &q)
 compute p * q = p1 * q1 + p2 * q2 + p3 * 13 More...
 
template<typename OUT = float32_t, typename T1 , typename T2 >
OUT squared_distance_3d (const T1 &a, const T2 &b)
 Compute 3D squared euclidean distance between two points. More...
 
template<typename OUT = float32_t, typename T1 , typename T2 >
OUT distance_3d (const T1 &a, const T2 &b)
 Compute 3D euclidean distance between two points. More...
 
template<typename PointT >
std::list< PointT >::const_iterator convex_hull (std::list< PointT > &list)
 A static memory implementation of convex hull computation. Shuffles points around the deque such that the points of the convex hull of the deque of points are first in the deque, with the internal points following in an unspecified order. More...
 
template<typename Iter1 , typename Iter2 >
std::vector< std::vector< typename std::iterator_traits< Iter1 >::value_type > > hull_pockets (const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, const Iter2 convex_hull_end)
 Compute a list of "pockets" of a simple polygon (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make up the difference between the polygon and its convex hull. This currently has a bug: More...
 
template<typename Iter >
bool intersect (const Iter begin1, const Iter end1, const Iter begin2, const Iter end2)
 Check if polyhedra defined by two given sets of points intersect. More...
 
template<template< typename ... > class Iterable1T, template< typename ... > class Iterable2T, typename PointT >
std::list< PointT > convex_polygon_intersection2d (const Iterable1T< PointT > &polygon1, const Iterable2T< PointT > &polygon2)
 Get the intersection between two polygons. The polygons should be provided in an identical format to the output of convex_hull function as in the corners should be ordered in a CCW fashion. The computation is done by: More...
 
template<template< typename ... > class Iterable1T, template< typename ... > class Iterable2T, typename PointT >
common::types::float32_t convex_intersection_over_union_2d (const Iterable1T< PointT > &polygon1, const Iterable2T< PointT > &polygon2)
 Compute the intersection over union of two 2d convex polygons. If any of the polygons span a zero area, the result is 0.0. More...
 

Typedef Documentation

◆ Interval_d

◆ Interval_f

◆ Point

using autoware::common::geometry::Point = typedef geometry_msgs::msg::Point32

Function Documentation

◆ all_ordered()

template<typename IT >
bool autoware::common::geometry::all_ordered ( const IT  begin,
const IT  end 
)
noexcept

Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise direction): This function does not check for convexity

Template Parameters
ITIterator type pointing to a point containing float x and float y
Parameters
[in]beginBeginning of point sequence
[in]endOne past the last of the point sequence
Returns
Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. Returns true for collinear points as well

◆ area_2d()

template<typename IT >
auto autoware::common::geometry::area_2d ( const IT  begin,
const IT  end 
)
noexcept

Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW)

Template Parameters
ITIterator type pointing to a point containing float x and float y
Parameters
[in]beginIterator pointing to the beginning of the polygon points
[in]endIterator pointing to one past the last of the polygon points
Returns
The area of the polygon, in squared of whatever units your points are in

◆ area_checked_2d()

template<typename IT >
auto autoware::common::geometry::area_checked_2d ( const IT  begin,
const IT  end 
)

Compute area of convex hull, throw if points are not ordered (convexity check is not implemented)

Exceptions
std::domain_errorif points are not ordered either CW or CCW
Template Parameters
ITIterator type pointing to a point containing float x and float y
Parameters
[in]beginIterator pointing to the beginning of the polygon points
[in]endIterator pointing to one past the last of the polygon points
Returns
The area of the polygon, in squared of whatever units your points are in

◆ ccw()

template<typename T1 , typename T2 , typename T3 >
auto autoware::common::geometry::ccw ( const T1 &  pt,
const T2 &  q,
const T3 &  r 
)
inline

compute whether line segment rp is counter clockwise relative to line segment qp

Template Parameters
T1,T2,T3point type. Must have point adapters defined or have float members x and y
Parameters
[in]ptshared point for both line segments
[in]rpoint to check if it forms a ccw angle
[in]qreference point
Returns
whether angle formed is ccw. Three collinear points is considered ccw

◆ check_point_position_to_line_2d()

template<typename T >
auto autoware::common::geometry::check_point_position_to_line_2d ( const T &  p1,
const T &  p2,
const T &  q 
)
inline

Check the given point's position relative the infinite line passing from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft()

Template Parameters
TT point type. Must have point adapters defined or have float members x and y
Parameters
p1point 1 laying on the infinite line
p2point 2 laying on the infinite line
qpoint to be checked against the line
Returns
> 0 for point q left of the line through p1 to p2 = 0 for point q on the line through p1 to p2 < 0 for point q right of the line through p1 to p2

◆ closest_line_point_2d()

template<typename T >
T autoware::common::geometry::closest_line_point_2d ( const T &  p,
const T &  q,
const T &  r 
)
inline

Compute the closest point on the line going through p-q to point r.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pFirst point defining the line
[in]qSecond point defining the line
[in]rReference point to find the closest point to
Returns
Closest point on line p-q to point r
Exceptions
std::runtime_errorif the two points coincide and hence don't uniquely

◆ closest_segment_point_2d()

template<typename T >
T autoware::common::geometry::closest_segment_point_2d ( const T &  p,
const T &  q,
const T &  r 
)
inline

Compute the closest point on line segment p-q to point r Based on equations from https://stackoverflow.com/a/1501725 and http://paulbourke.net/geometry/pointlineplane/.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pFirst point defining the line segment
[in]qSecond point defining the line segment
[in]rReference point to find the closest point to
Returns
Closest point on line segment p-q to point r

◆ convex_hull()

template<typename PointT >
std::list<PointT>::const_iterator autoware::common::geometry::convex_hull ( std::list< PointT > &  list)

A static memory implementation of convex hull computation. Shuffles points around the deque such that the points of the convex hull of the deque of points are first in the deque, with the internal points following in an unspecified order.

The head of the deque will be the point with the smallest x value, with the other points following in a counter-clockwise manner (from a top down view/facing -z direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result as previously stated does not hold).

Parameters
[in,out]listA list of nodes that will be reordered into a ccw convex hull
Returns
An iterator pointing to one after the last point contained in the hull
Template Parameters
PointTType of a point, must have x and y float members

◆ convex_intersection_over_union_2d()

template<template< typename ... > class Iterable1T, template< typename ... > class Iterable2T, typename PointT >
common::types::float32_t autoware::common::geometry::convex_intersection_over_union_2d ( const Iterable1T< PointT > &  polygon1,
const Iterable2T< PointT > &  polygon2 
)

Compute the intersection over union of two 2d convex polygons. If any of the polygons span a zero area, the result is 0.0.

Template Parameters
Iterable1TA container class that has stl style iterators defined.
Iterable2TA container class that has stl style iterators defined.
Point1TPoint type that have the adapters for the x and y fields.
Point2TPoint type that have the adapters for the x and y fields.
Parameters
polygon1A convex polygon
polygon2A convex polygon
Returns
(Intersection / Union) between two given polygons.
Exceptions
std::domain_errorIf there is any inconsistency on the undderlying geometrical computation.

◆ convex_polygon_intersection2d()

template<template< typename ... > class Iterable1T, template< typename ... > class Iterable2T, typename PointT >
std::list<PointT> autoware::common::geometry::convex_polygon_intersection2d ( const Iterable1T< PointT > &  polygon1,
const Iterable2T< PointT > &  polygon2 
)

Get the intersection between two polygons. The polygons should be provided in an identical format to the output of convex_hull function as in the corners should be ordered in a CCW fashion. The computation is done by:

  • Find the corners of each polygon that are contained by the other polygon.
  • Find the intersection points between two polygons
  • Combine these points and order CCW to get the final polygon. The criteria for intersection is better explained in: "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) See https://www.osti.gov/servlets/purl/7309916/, chapter II - B TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient algorithm: #1230
    Template Parameters
    Iterable1TA container class that has stl style iterators defined.
    Iterable2TA container class that has stl style iterators defined.
    PointTPoint type that have the adapters for the x and y fields. set to Point1T
    Parameters
    polygon1A convex polygon
    polygon2A convex polygon
    Returns
    The resulting conv

◆ cross_2d()

template<typename T1 , typename T2 >
auto autoware::common::geometry::cross_2d ( const T1 &  pt,
const T2 &  q 
)
inline

compute p x q = p1 * q2 - p2 * q1

Template Parameters
T1,T2point type. Must have point adapters defined or have float members x and y
Parameters
[in]ptfirst point
[in]qsecond point
Returns
2d cross product

◆ distance_2d()

template<typename OUT = float32_t, typename T1 , typename T2 >
OUT autoware::common::geometry::distance_2d ( const T1 &  a,
const T2 &  b 
)
inline

Compute euclidean distance between two points.

Template Parameters
OUTreturn type. Type of the returned distance.
T1point type. Must have point adapters defined or have float members x and y
T2point type. Must have point adapters defined or have float members x and y
Parameters
apoint 1
bpoint 2
Returns
euclidean distance

◆ distance_3d()

template<typename OUT = float32_t, typename T1 , typename T2 >
OUT autoware::common::geometry::distance_3d ( const T1 &  a,
const T2 &  b 
)
inline

Compute 3D euclidean distance between two points.

Template Parameters
T1point type. Must have point adapters defined or have float members x y and z
T2point type. Must have point adapters defined or have float members x y and z
Parameters
apoint 1
bpoint 2
Returns
3D euclidean distance

◆ dot_2d()

template<typename T1 , typename T2 >
auto autoware::common::geometry::dot_2d ( const T1 &  pt,
const T2 &  q 
)
inline

compute p * q = p1 * q1 + p2 * q2

Template Parameters
T1,T2point type. Must have point adapters defined or have float members x and y
Parameters
[in]ptfirst point
[in]qsecond point
Returns
2d scalar dot product

◆ dot_3d()

template<typename T1 , typename T2 >
auto autoware::common::geometry::dot_3d ( const T1 &  pt,
const T2 &  q 
)
inline

compute p * q = p1 * q1 + p2 * q2 + p3 * 13

Template Parameters
T1,T2point type. Must have point adapters defined or have float members x, y and z
Parameters
[in]ptfirst point
[in]qsecond point
Returns
3d scalar dot product

◆ get_normal()

template<typename T >
T autoware::common::geometry::get_normal ( const T &  pt)
inline

compute q s.t. p T q, or p * q = 0 This is the equivalent of a 90 degree ccw rotation

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]ptpoint to get normal point of
Returns
point normal to p (unnormalized)

◆ hull_pockets()

template<typename Iter1 , typename Iter2 >
std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type> > autoware::common::geometry::hull_pockets ( const Iter1  polygon_start,
const Iter1  polygon_end,
const Iter2  convex_hull_start,
const Iter2  convex_hull_end 
)

Compute a list of "pockets" of a simple polygon (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make up the difference between the polygon and its convex hull. This currently has a bug:

Parameters
[in]polygon_startStart iterator for the simple polygon (has to be on convex hull)
[in]polygon_endEnd iterator for the simple polygon
[in]convex_hull_startStart iterator for the convex hull of the simple polygon
[in]convex_hull_endEnd iterator for the convex hull of the simple polygon
Returns
A vector of vectors of the iterator value type. Each inner vector contains the points for one pocket. We return by value instead of as iterator pairs, because it is possible that the edge connecting the final point in the list and the first point in the list is part of a pocket as well, and this is awkward to represent using iterators into the original collection.
Template Parameters
Iter1Iterator to a point type
Iter2Iterator to a point type (can be the same as Iter1, but does not need to be)

◆ intersect()

template<typename Iter >
bool autoware::common::geometry::intersect ( const Iter  begin1,
const Iter  end1,
const Iter  begin2,
const Iter  end2 
)

Check if polyhedra defined by two given sets of points intersect.

Template Parameters
IterIterator over point-types that must have point adapters
Parameters
[in]begin1Start iterator to first list of point types
[in]end1End iterator to first list of point types
[in]begin2Start iterator to first list of point types
[in]end2End iterator to first list of point types
Returns
true if the boxes collide, false otherwise.

◆ intersection_2d()

template<typename T >
T autoware::common::geometry::intersection_2d ( const T &  pt,
const T &  u,
const T &  q,
const T &  v 
)
inline

solve p + t * u = q + s * v Ref: https://stackoverflow.com/questions/563198/ whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]ptanchor point of first line
[in]udirection of first line
[in]qanchor point of second line
[in]vdirection of second line
Returns
intersection point
Exceptions
std::runtime_errorif lines are (nearly) collinear or parallel

◆ is_point_inside_polygon_2d()

template<typename IteratorType , typename PointType >
bool autoware::common::geometry::is_point_inside_polygon_2d ( const IteratorType &  start_it,
const IteratorType &  end_it,
const PointType &  p 
)

Check if the given point is inside or on the edge of the given polygon.

Template Parameters
IteratorTypeiterator type. The value pointed to by this must have point adapters defined or have float members x and y
PointTypepoint type. Must have point adapters defined or have float members x and y
Parameters
start_ititerator pointing to the first vertex of the polygon
end_ititerator pointing to the last vertex of the polygon. The vertices should be in order.
ppoint to be searched
Returns
True if the point is inside or on the edge of the polygon. False otherwise

◆ make_unit_vector2d()

template<typename T >
T autoware::common::geometry::make_unit_vector2d ( float  th)
inline

Make a 2D unit vector given an angle.

Template Parameters
TPoint type. Must have point adapters defined or have float members x and y
Parameters
thAngle in radians
Returns
Unit vector in the direction of the given angle.

◆ minus_2d() [1/2]

template<typename T >
T autoware::common::geometry::minus_2d ( const T &  p)

The unary minus or negation operator applied to a single point's 2d fields.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pThe left hand side
Returns
A point with the negation in the x and y fields, all other fields are default initialized

◆ minus_2d() [2/2]

template<typename T >
T autoware::common::geometry::minus_2d ( const T &  p,
const T &  q 
)

Compute the 2d difference between two points, p - q.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pThe left hand side
[in]qThe right hand side
Returns
A point with the difference in the x and y fields, all other fields are default initialized

◆ norm_2d()

template<typename T >
auto autoware::common::geometry::norm_2d ( const T &  pt)
inline

get magnitude of x and y components:

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]ptpoint to get magnitude of
Returns
magitude of x and y components together

◆ plus_2d()

template<typename T >
T autoware::common::geometry::plus_2d ( const T &  p,
const T &  q 
)

The 2d addition operation, p + q.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pThe left hand side
[in]qThe right hand side
Returns
A point with the sum in the x and y fields, all other fields are default initialized

◆ point_line_segment_distance_2d()

template<typename T >
auto autoware::common::geometry::point_line_segment_distance_2d ( const T &  p,
const T &  q,
const T &  r 
)
inline

Compute the distance from line segment p-q to point r.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pFirst point defining the line segment
[in]qSecond point defining the line segment
[in]rReference point to find the distance from the line segment to
Returns
Distance from point r to line segment p-q

◆ rotate_2d() [1/2]

template<typename T >
T autoware::common::geometry::rotate_2d ( const T &  pt,
const float32_t  th_rad 
)
inline

rotate by radian angle th in z direction with ccw positive

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]ptreference point to rotate
[in]th_radangle by which to rotate point
Returns
rotated point

◆ rotate_2d() [2/2]

template<typename T >
void autoware::common::geometry::rotate_2d ( T &  pt,
const float32_t  cos_th,
const float32_t  sin_th 
)
inline

rotate point given precomputed sin and cos

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in,out]ptpoint to rotate
[in]cos_thprecomputed cosine value
[in]sin_thprecompined sine value

◆ squared_distance_2d()

template<typename OUT = float32_t, typename T1 , typename T2 >
OUT autoware::common::geometry::squared_distance_2d ( const T1 &  a,
const T2 &  b 
)
inline

Compute squared euclidean distance between two points.

Template Parameters
OUTreturn type. Type of the returned distance.
T1point type. Must have point adapters defined or have float members x and y
T2point type. Must have point adapters defined or have float members x and y
Parameters
apoint 1
bpoint 2
Returns
squared euclidean distance

◆ squared_distance_3d()

template<typename OUT = float32_t, typename T1 , typename T2 >
OUT autoware::common::geometry::squared_distance_3d ( const T1 &  a,
const T2 &  b 
)
inline

Compute 3D squared euclidean distance between two points.

Template Parameters
OUTreturn type. Type of the returned distance.
T1point type. Must have point adapters defined or have float members x y and z
T2point type. Must have point adapters defined or have float members x y and z
Parameters
apoint 1
bpoint 2
Returns
squared 3D euclidean distance

◆ times_2d()

template<typename T >
T autoware::common::geometry::times_2d ( const T &  p,
const float32_t  a 
)

The scalar multiplication operation, p * a.

Template Parameters
Tpoint type. Must have point adapters defined or have float members x and y
Parameters
[in]pThe point value
[in]aThe scalar value
Returns
A point with the scaled x and y fields, all other fields are default initialized