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

Classes

struct  array_size
 Helper struct for compile time introspection of array size from stackoverflow.com/questions/16866033/getting-the-number-of-elements-in-stdarray-at-compile-time. More...
 
struct  array_size< std::array< T, N > >
 
struct  Covariance2d
 Simplified 2d covariance matrix. More...
 
class  LFitCompare
 
struct  LFitWs
 A representation of the M matrix for the L-fit algorithm. More...
 

Typedefs

template<typename T >
using base_type = typename std::remove_cv< typename std::remove_reference< T >::type >::type
 Templated alias for getting a type without CV or reference qualification. More...
 
template<typename PointT >
using Point4 = std::array< PointT, 4U >
 Templated alias for an array of 4 objects of type PointT. More...
 

Functions

void GEOMETRY_PUBLIC size_2d (const decltype(BoundingBox::corners) &corners, geometry_msgs::msg::Point32 &ret)
 Compute length, width, area of bounding box. More...
 
void GEOMETRY_PUBLIC finalize_box (const decltype(BoundingBox::corners) &corners, BoundingBox &box)
 Computes centroid and orientation of a box given corners. More...
 
template<typename IT , typename PointT >
void compute_corners (decltype(BoundingBox::corners) &corners, const Point4< IT > &supports, const Point4< PointT > &directions)
 given support points and two orthogonal directions, compute corners of bounding box More...
 
autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape (const BoundingBox &box)
 Copy vertices of the given box into a Shape type. More...
 
geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose (const BoundingBox &box)
 Copy centroid and orientation info of the box into Pose type. More...
 
autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC make_detected_object (const BoundingBox &box)
 Fill DetectedObject type with contents from a BoundingBox type. More...
 
std::vector< geometry_msgs::msg::Point32 > GEOMETRY_PUBLIC get_transformed_corners (const autoware_auto_perception_msgs::msg::Shape &shape_msg, const geometry_msgs::msg::Point &centroid, const geometry_msgs::msg::Quaternion &orientation)
 Transform corners from object-local coordinates using the given centroid and orientation. More...
 
template<typename IT >
Covariance2d covariance_2d (const IT begin, const IT end)
 Compute 2d covariance matrix of a list of points using Welford's online algorithm. More...
 
template<typename PointT >
std::pair< float32_t, float32_t > eig_2d (const Covariance2d &cov, PointT &eigvec1, PointT &eigvec2)
 Compute eigenvectors and eigenvalues. More...
 
template<typename IT , typename PointT >
bool8_t compute_supports (const IT begin, const IT end, const PointT &eig1, const PointT &eig2, Point4< IT > &support)
 Given eigenvectors, compute support (furthest) point in each direction. More...
 
template<typename IT , typename PointT >
BoundingBox compute_bounding_box (const PointT &ax1, const PointT &ax2, const Point4< IT > &supports)
 Compute bounding box given a pair of basis directions. More...
 
template<typename IT >
void init_lfit_ws (const IT begin, const IT end, const std::size_t size, LFitWs &ws)
 Initialize M matrix for L-fit algorithm. More...
 
template<typename PointT >
float32_t solve_lfit (const LFitWs &ws, PointT &dir)
 Solves the L fit problem for a given M matrix. More...
 
template<typename PointT >
void increment_lfit_ws (const PointT &pt, LFitWs &ws)
 Increments L fit M matrix with information in the point. More...
 
template<typename IT >
BoundingBox lfit_bounding_box_2d_impl (const IT begin, const IT end, const std::size_t size)
 The main implementation of L-fitting a bounding box to a list of points. Assumes sufficiently valid, large enough, and appropriately ordered point list. More...
 
template<typename PointT >
uint32_t update_angles (const Point4< PointT > &edges, Point4< PointT > &directions)
 Find which (next) edge has smallest angle delta to directions, rotate directions. More...
 
template<typename IT , typename PointT >
void init_edges (const IT begin, const IT end, const Point4< IT > &support, Point4< PointT > &edges)
 Given support points i, find direction of edge: e = P[i+1] - P[i]. More...
 
template<typename IT >
void init_bbox (const IT begin, const IT end, Point4< IT > &support)
 Scan through list to find support points for bounding box oriented in direction of u = P[1] - P[0]. More...
 
template<typename IT , typename MetricF >
BoundingBox rotating_calipers_impl (const IT begin, const IT end, const MetricF metric_fn)
 Compute the minimum bounding box for a convex hull using the rotating calipers method. This function may possibly also be used for computing the width of a convex hull, as it uses the external supports of a single convex hull. More...
 

Typedef Documentation

◆ base_type

template<typename T >
using autoware::common::geometry::bounding_box::details::base_type = typedef typename std::remove_cv<typename std::remove_reference<T>::type>::type

Templated alias for getting a type without CV or reference qualification.

◆ Point4

template<typename PointT >
using autoware::common::geometry::bounding_box::details::Point4 = typedef std::array<PointT, 4U>

Templated alias for an array of 4 objects of type PointT.

Function Documentation

◆ compute_bounding_box()

template<typename IT , typename PointT >
BoundingBox autoware::common::geometry::bounding_box::details::compute_bounding_box ( const PointT &  ax1,
const PointT &  ax2,
const Point4< IT > &  supports 
)

Compute bounding box given a pair of basis directions.

Template Parameters
ITAn iterator type dereferencable into a point with float members x and y
PointTPoint type of the lists, must have float members x and y
Parameters
[in]ax1First basis direction, assumed to be normal to ax2
[in]ax2Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1
[in]supportsArray of iterators referring to points that are most extreme in each basis direction. Should be result of compute_supports function
Returns
A bounding box based on the basis axes and the support points

◆ compute_corners()

template<typename IT , typename PointT >
void autoware::common::geometry::bounding_box::details::compute_corners ( decltype(BoundingBox::corners) &  corners,
const Point4< IT > &  supports,
const Point4< PointT > &  directions 
)

given support points and two orthogonal directions, compute corners of bounding box

Template Parameters
PointTType of a point, must have float members x and y`
ITAn iterator dereferencable into PointT
Parameters
[out]cornersGets filled with corner points of bounding box
[in]supportsIterators referring to support points of current bounding box e.g. bounding box is touching these points
[in]directionsDirections of each edge of the bounding box from each support point

◆ compute_supports()

template<typename IT , typename PointT >
bool8_t autoware::common::geometry::bounding_box::details::compute_supports ( const IT  begin,
const IT  end,
const PointT &  eig1,
const PointT &  eig2,
Point4< IT > &  support 
)

Given eigenvectors, compute support (furthest) point in each direction.

Template Parameters
ITAn iterator type dereferencable into a point with float members x and y
PointTtype of a point with float members x and y
Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator pointing to one past the last point in the point list
[in]eig1First principal component of cluster
[in]eig2Second principal component of cluster
[out]supportArray to get filled with extreme points in each of the principal components
Returns
whether or not eig2 is ccw wrt eig1

◆ covariance_2d()

template<typename IT >
Covariance2d autoware::common::geometry::bounding_box::details::covariance_2d ( const IT  begin,
const IT  end 
)

Compute 2d covariance matrix of a list of points using Welford's online algorithm.

Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator pointing to one past the last point in the point list
Template Parameters
ITAn iterator type dereferencable into a point with float members x and y
Returns
A 2d covariance matrix for all points inthe list

◆ eig_2d()

template<typename PointT >
std::pair<float32_t, float32_t> autoware::common::geometry::bounding_box::details::eig_2d ( const Covariance2d cov,
PointT &  eigvec1,
PointT &  eigvec2 
)

Compute eigenvectors and eigenvalues.

Parameters
[in]cov2d Covariance matrix
[out]eigvec1First eigenvector
[out]eigvec2Second eigenvector
Template Parameters
PointTPoint type that has at least float members x and y
Returns
A pairt of eigenvalues: The first is the larger eigenvalue
Exceptions
std::runtimeerror if you would get degenerate covariance

◆ finalize_box()

void autoware::common::geometry::bounding_box::details::finalize_box ( const decltype(BoundingBox::corners) &  corners,
BoundingBox box 
)

Computes centroid and orientation of a box given corners.

Parameters
[in]cornersArray of final corners of bounding box
[out]boxMessage to have corners, orientation, and centroid updated

◆ get_transformed_corners()

std::vector< geometry_msgs::msg::Point32 > GEOMETRY_PUBLIC autoware::common::geometry::bounding_box::details::get_transformed_corners ( const autoware_auto_perception_msgs::msg::Shape &  shape_msg,
const geometry_msgs::msg::Point centroid,
const geometry_msgs::msg::Quaternion &  orientation 
)

Transform corners from object-local coordinates using the given centroid and orientation.

Parameters
shape_msgMsg containing corners in object-local coordinates
centroidCentroid of the polygon whose corners are defined in shape_msg
orientationOrientation of the polygon
Returns
corners transformed such that their centroid and orientation correspond to the given inputs

◆ increment_lfit_ws()

template<typename PointT >
void autoware::common::geometry::bounding_box::details::increment_lfit_ws ( const PointT &  pt,
LFitWs ws 
)

Increments L fit M matrix with information in the point.

Template Parameters
PointTThe point type
Parameters
[in]ptThe point to increment the M matrix with
[in,out]wsA representation of the M matrix

◆ init_bbox()

template<typename IT >
void autoware::common::geometry::bounding_box::details::init_bbox ( const IT  begin,
const IT  end,
Point4< IT > &  support 
)

Scan through list to find support points for bounding box oriented in direction of u = P[1] - P[0].

Template Parameters
ITThe iterator type, should dereference to a point type with float members x and y
Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator pointing to one past the last point in the point list
[out]supportArray that gets filled with pointers to points that are most extreme in each direction aligned with and orthogonal to the first polygon edge. Most extreme = max/min wrt u = P[1]-P0

◆ init_edges()

template<typename IT , typename PointT >
void autoware::common::geometry::bounding_box::details::init_edges ( const IT  begin,
const IT  end,
const Point4< IT > &  support,
Point4< PointT > &  edges 
)

Given support points i, find direction of edge: e = P[i+1] - P[i].

Template Parameters
PointTPoint type of the lists, must have float members x and y
ITThe iterator type, should dereference to a point type with float members x and y
Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator pointing to one past the last point in the point list
[in]supportArray of points that are most extreme in 4 directions for current OBB
[out]edgesAn array to be filled with the polygon edges next from support points

◆ init_lfit_ws()

template<typename IT >
void autoware::common::geometry::bounding_box::details::init_lfit_ws ( const IT  begin,
const IT  end,
const std::size_t  size,
LFitWs ws 
)

Initialize M matrix for L-fit algorithm.

Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator one past the last point in the point list
[in]sizeThe number of points in the point list
[out]wsA representation of the M matrix to get initialized
Template Parameters
ITThe iterator type, should dereference to a point type with float members x and y

◆ lfit_bounding_box_2d_impl()

template<typename IT >
BoundingBox autoware::common::geometry::bounding_box::details::lfit_bounding_box_2d_impl ( const IT  begin,
const IT  end,
const std::size_t  size 
)

The main implementation of L-fitting a bounding box to a list of points. Assumes sufficiently valid, large enough, and appropriately ordered point list.

Parameters
[in]beginAn iterator pointing to the first point in a point list
[in]endAn iterator pointing to one past the last point in the point list
[in]sizeThe number of points in the point list
Returns
A bounding box that minimizes the LFit residual
Template Parameters
ITAn iterator type dereferencable into a point with float members x and y

◆ make_detected_object()

autoware_auto_perception_msgs::msg::DetectedObject autoware::common::geometry::bounding_box::details::make_detected_object ( const BoundingBox box)

Fill DetectedObject type with contents from a BoundingBox type.

Parameters
boxBoundingBox to be converted
Returns
Filled DetectedObject type

◆ make_pose()

geometry_msgs::msg::Pose GEOMETRY_PUBLIC autoware::common::geometry::bounding_box::details::make_pose ( const BoundingBox box)

Copy centroid and orientation info of the box into Pose type.

Parameters
boxBoundingBox to be converted
Returns
Pose type filled with centroid and orientation from box

◆ make_shape()

autoware_auto_perception_msgs::msg::Shape autoware::common::geometry::bounding_box::details::make_shape ( const BoundingBox box)

Copy vertices of the given box into a Shape type.

Parameters
boxBox to be converted
Returns
Shape type filled with box vertices

◆ rotating_calipers_impl()

template<typename IT , typename MetricF >
BoundingBox autoware::common::geometry::bounding_box::details::rotating_calipers_impl ( const IT  begin,
const IT  end,
const MetricF  metric_fn 
)

Compute the minimum bounding box for a convex hull using the rotating calipers method. This function may possibly also be used for computing the width of a convex hull, as it uses the external supports of a single convex hull.

Parameters
[in]beginAn iterator to the first point on a convex hull
[in]endAn iterator to one past the last point on a convex hull
[in]metric_fnA functor determining what measure the bounding box is minimum with respect to
Template Parameters
ITAn iterator type dereferencable into a point type with float members x and y
MetricFA functor that computes a float measure from the x and y size (width and length) of a bounding box, assumed to be packed in a Point32 message.
Exceptions
std::domain_errorif the list of points is too small to reasonable generate a bounding box

◆ size_2d()

void autoware::common::geometry::bounding_box::details::size_2d ( const decltype(BoundingBox::corners) &  corners,
geometry_msgs::msg::Point32 &  ret 
)

Compute length, width, area of bounding box.

Parameters
[in]cornersCorners of the current bounding box
[out]retA point struct used to hold size of box defined by corners

◆ solve_lfit()

template<typename PointT >
float32_t autoware::common::geometry::bounding_box::details::solve_lfit ( const LFitWs ws,
PointT &  dir 
)

Solves the L fit problem for a given M matrix.

Template Parameters
PointTThe point type of the cluster being L-fitted
Parameters
[in]wsA representation of the M Matrix
[out]dirThe best fit direction for this partition/M matrix
Returns
The L2 residual for this fit (the score, lower is better)

◆ update_angles()

template<typename PointT >
uint32_t autoware::common::geometry::bounding_box::details::update_angles ( const Point4< PointT > &  edges,
Point4< PointT > &  directions 
)

Find which (next) edge has smallest angle delta to directions, rotate directions.

Parameters
[in,out]edgesArray of edges on polygon after each anchor point (in ccw order). E.g. if anchor point = p_i, edge = P[i+1] - P[i]
[in,out]directionsArray of directions of current bounding box (in ccw order)
Template Parameters
PointTPoint type of the lists, must have float members x and y
Returns
index of array to advance, e.g. the one with the smallest angle between edge and dir