scenario_simulator_v2 C++ API
Classes | Typedefs | Enumerations | Functions
math::geometry Namespace Reference

Classes

struct  DistancesFromCenterToEdge
 
class  Plane
 Represents a plane in 3D space, defined by a normal vector and a point on the plane. More...
 
class  LineSegment
 
struct  IsLikeQuaternion
 
struct  IsLikeQuaternion< T, std::void_t< decltype(std::declval< T >().x), decltype(std::declval< T >().y), decltype(std::declval< T >().z), decltype(std::declval< T >().w)> >
 
class  PolynomialSolver
 
class  CatmullRomSpline
 
class  CatmullRomSplineInterface
 
class  CatmullRomSubspline
 
class  HermiteCurve
 
struct  HasMemberW
 
struct  HasMemberW< T, std::void_t< decltype(std::declval< T >().w)> >
 
struct  IsLikeVector3
 
struct  IsLikeVector3< T, std::void_t< decltype(std::declval< T >().x), decltype(std::declval< T >().y), decltype(std::declval< T >().z), std::enable_if_t<!HasMemberW< T >::value > > >
 

Typedefs

using boost_point = boost::geometry::model::d2::point_xy< double >
 
using boost_polygon = boost::geometry::model::polygon< boost_point >
 

Enumerations

enum class  Axis { X = 0 , Y = 1 , Z = 2 }
 

Functions

std::optional< double > getPolygonDistance (const geometry_msgs::msg::Pose &pose0, const traffic_simulator_msgs::msg::BoundingBox &bbox0, const geometry_msgs::msg::Pose &pose1, const traffic_simulator_msgs::msg::BoundingBox &bbox1)
 Get the Polygon Distance object. More...
 
std::optional< std::pair< geometry_msgs::msg::Pose, geometry_msgs::msg::Pose > > getClosestPoses (const geometry_msgs::msg::Pose &pose0, const traffic_simulator_msgs::msg::BoundingBox &bbox0, const geometry_msgs::msg::Pose &pose1, const traffic_simulator_msgs::msg::BoundingBox &bbox1)
 
boost_point pointToSegmentProjection (const boost_point &p, const boost_point &p1, const boost_point &p2)
 
boost_point toBoostPoint (const geometry_msgs::msg::Point &point)
 
boost_polygon toBoostPolygon (const std::vector< geometry_msgs::msg::Point > &points)
 
geometry_msgs::msg::Pose toPose (const boost_point &point)
 
geometry_msgs::msg::Pose subtractPoses (const geometry_msgs::msg::Pose &pose1, const geometry_msgs::msg::Pose &pose2)
 
auto toPolygon2D (const traffic_simulator_msgs::msg::BoundingBox &bounding_box) -> std::vector< geometry_msgs::msg::Point >
 
auto toPolygon2D (const geometry_msgs::msg::Pose &pose, const traffic_simulator_msgs::msg::BoundingBox &bounding_box) -> boost_polygon
 
std::vector< geometry_msgs::msg::Point > getPointsFromBbox (const traffic_simulator_msgs::msg::BoundingBox &bounding_box, const double width_extension_right=0.0, const double width_extension_left=0.0, const double length_extension_front=0.0, const double length_extension_rear=0.0)
 
DistancesFromCenterToEdge getDistancesFromCenterToEdge (const traffic_simulator_msgs::msg::BoundingBox &bounding_box)
 
double getDistance (const geometry_msgs::msg::Point &p0, const geometry_msgs::msg::Point &p1)
 
double getDistance (const geometry_msgs::msg::Pose &p0, const geometry_msgs::msg::Point &p1)
 
double getDistance (const geometry_msgs::msg::Point &p0, const geometry_msgs::msg::Pose &p1)
 
double getDistance (const geometry_msgs::msg::Pose &p0, const geometry_msgs::msg::Pose &p1)
 
double getDistance2D (const std::vector< geometry_msgs::msg::Point > &polygon0, const std::vector< geometry_msgs::msg::Point > &polygon1)
 
bool checkCollision2D (const geometry_msgs::msg::Pose &pose0, const traffic_simulator_msgs::msg::BoundingBox &bbox0, const geometry_msgs::msg::Pose &pose1, const traffic_simulator_msgs::msg::BoundingBox &bbox1)
 
bool contains (const std::vector< geometry_msgs::msg::Point > &polygon, const geometry_msgs::msg::Point &point)
 
bool isIntersect2D (const LineSegment &line0, const LineSegment &line1)
 
bool isIntersect2D (const std::vector< LineSegment > &lines)
 
std::optional< geometry_msgs::msg::Point > getIntersection2D (const LineSegment &line0, const LineSegment &line1)
 
std::vector< geometry_msgs::msg::Point > getIntersection2D (const std::vector< LineSegment > &lines)
 
auto getLineSegments (const std::vector< geometry_msgs::msg::Point > &points, const bool close_start_end) -> std::vector< LineSegment >
 Get the line segments from points. More...
 
double getMaxValue (const std::vector< geometry_msgs::msg::Point > &points, const Axis &axis)
 
double getMinValue (const std::vector< geometry_msgs::msg::Point > &points, const Axis &axis)
 
std::vector< double > filterByAxis (const std::vector< geometry_msgs::msg::Point > &points, const Axis &axis)
 
std::vector< geometry_msgs::msg::Point > get2DConvexHull (const std::vector< geometry_msgs::msg::Point > &points)
 
template<typename T , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >>, std::nullptr_t > = nullptr>
auto convertEulerAngleToQuaternion (const T &v)
 
template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto getAngleDifference (const T &quat1, const T &quat2) -> double
 
template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto getNormalVector (const T &orientation) -> geometry_msgs::msg::Vector3
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto getRotation (T from, U to)
 
template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto getRotationMatrix (T quat) -> Eigen::Matrix3d
 
template<typename T , typename U , typename V , typename W , std::enable_if_t< std::conjunction_v< std::is_scalar< T >, std::is_scalar< U >, std::is_scalar< V >, std::is_scalar< W >>, std::nullptr_t > = nullptr>
auto makeQuaternion (const T &x, const U &y, const V &z, const W &w)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto operator+ (const T &a, const U &b)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto operator- (const T &a, const U &b)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto operator* (const T &a, const U &b)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto operator+= (T &a, const U &b) -> decltype(auto)
 
template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto convertQuaternionToEulerAngle (const T &q)
 
template<typename T , typename U , typename V , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >, std::is_scalar< V >>, std::nullptr_t > = nullptr>
auto slerp (T quat1, U quat2, V t)
 
const geometry_msgs::msg::Pose getRelativePose (const geometry_msgs::msg::Pose &from, const geometry_msgs::msg::Pose &to)
 Get the relative pose between two poses. More...
 
const geometry_msgs::msg::Point transformPoint (const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Point &point)
 Get transformed pose in world frame. More...
 
const geometry_msgs::msg::Point transformPoint (const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Pose &sensor_pose, const geometry_msgs::msg::Point &point)
 Get transformed pose in sensor frame. More...
 
std::vector< geometry_msgs::msg::Point > transformPoints (const geometry_msgs::msg::Pose &pose, const std::vector< geometry_msgs::msg::Point > &points)
 Get transformed points in world frame. More...
 
std::vector< geometry_msgs::msg::Point > transformPoints (const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Pose &sensor_pose, const std::vector< geometry_msgs::msg::Point > &points)
 Get transformed points in sensor frame. More...
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto hypot (const T &from, const U &to)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto innerProduct (const T &v0, const U &v1)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto getInternalAngle (const T &v0, const U &v1)
 
template<typename T , std::enable_if_t< IsLikeVector3< T >::value, std::nullptr_t > = nullptr>
auto norm (const T &v)
 
template<typename T , std::enable_if_t< IsLikeVector3< T >::value, std::nullptr_t > = nullptr>
auto normalize (const T &v)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, std::is_scalar< U >>, std::nullptr_t > = nullptr>
auto operator/ (const T &a, const U &b)
 
template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, std::is_scalar< U >>, std::nullptr_t > = nullptr>
auto truncate (const T &v, const U &max)
 
template<typename T , typename U , typename V , std::enable_if_t< std::conjunction_v< std::is_scalar< T >, std::is_scalar< U >, std::is_scalar< V >>, std::nullptr_t > = nullptr>
auto vector3 (const T &x, const U &y, const V &z)
 

Typedef Documentation

◆ boost_point

typedef boost::geometry::model::d2::point_xy< double > math::geometry::boost_point

◆ boost_polygon

typedef boost::geometry::model::polygon< boost_point > math::geometry::boost_polygon

Enumeration Type Documentation

◆ Axis

enum math::geometry::Axis
strong
Enumerator

Function Documentation

◆ checkCollision2D()

bool math::geometry::checkCollision2D ( const geometry_msgs::msg::Pose &  pose0,
const traffic_simulator_msgs::msg::BoundingBox &  bbox0,
const geometry_msgs::msg::Pose &  pose1,
const traffic_simulator_msgs::msg::BoundingBox &  bbox1 
)

◆ contains()

bool math::geometry::contains ( const std::vector< geometry_msgs::msg::Point > &  polygon,
const geometry_msgs::msg::Point &  point 
)

◆ convertEulerAngleToQuaternion()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >>, std::nullptr_t > = nullptr>
auto math::geometry::convertEulerAngleToQuaternion ( const T &  v)

◆ convertQuaternionToEulerAngle()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto math::geometry::convertQuaternionToEulerAngle ( const T &  q)

◆ filterByAxis()

std::vector< double > math::geometry::filterByAxis ( const std::vector< geometry_msgs::msg::Point > &  points,
const Axis axis 
)

◆ get2DConvexHull()

std::vector< geometry_msgs::msg::Point > math::geometry::get2DConvexHull ( const std::vector< geometry_msgs::msg::Point > &  points)

◆ getAngleDifference()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto math::geometry::getAngleDifference ( const T &  quat1,
const T &  quat2 
) -> double

◆ getClosestPoses()

std::optional< std::pair< geometry_msgs::msg::Pose, geometry_msgs::msg::Pose > > math::geometry::getClosestPoses ( const geometry_msgs::msg::Pose &  pose0,
const traffic_simulator_msgs::msg::BoundingBox &  bbox0,
const geometry_msgs::msg::Pose &  pose1,
const traffic_simulator_msgs::msg::BoundingBox &  bbox1 
)

◆ getDistance() [1/4]

double math::geometry::getDistance ( const geometry_msgs::msg::Point &  p0,
const geometry_msgs::msg::Point &  p1 
)

◆ getDistance() [2/4]

double math::geometry::getDistance ( const geometry_msgs::msg::Point &  p0,
const geometry_msgs::msg::Pose &  p1 
)

◆ getDistance() [3/4]

double math::geometry::getDistance ( const geometry_msgs::msg::Pose &  p0,
const geometry_msgs::msg::Point &  p1 
)

◆ getDistance() [4/4]

double math::geometry::getDistance ( const geometry_msgs::msg::Pose &  p0,
const geometry_msgs::msg::Pose &  p1 
)

◆ getDistance2D()

double math::geometry::getDistance2D ( const std::vector< geometry_msgs::msg::Point > &  polygon0,
const std::vector< geometry_msgs::msg::Point > &  polygon1 
)

◆ getDistancesFromCenterToEdge()

DistancesFromCenterToEdge math::geometry::getDistancesFromCenterToEdge ( const traffic_simulator_msgs::msg::BoundingBox &  bounding_box)

◆ getInternalAngle()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto math::geometry::getInternalAngle ( const T &  v0,
const U &  v1 
)

◆ getIntersection2D() [1/2]

std::optional< geometry_msgs::msg::Point > math::geometry::getIntersection2D ( const LineSegment line0,
const LineSegment line1 
)

◆ getIntersection2D() [2/2]

std::vector< geometry_msgs::msg::Point > math::geometry::getIntersection2D ( const std::vector< LineSegment > &  lines)

◆ getLineSegments()

auto math::geometry::getLineSegments ( const std::vector< geometry_msgs::msg::Point > &  points,
const bool  close_start_end 
) -> std::vector< LineSegment >

Get the line segments from points.

Parameters
pointsPoints you want to build line segments.
close_start_endIf true, returned line segments are connected their start and end.
Returns
std::vector<LineSegment>
Note
If true, the end point(points[points.size() - 1]) and start point(points[0]) was connected.
If true, the end point(points[points.size() - 1]) and start point(points[0]) was connected.

◆ getMaxValue()

double math::geometry::getMaxValue ( const std::vector< geometry_msgs::msg::Point > &  points,
const Axis axis 
)

◆ getMinValue()

double math::geometry::getMinValue ( const std::vector< geometry_msgs::msg::Point > &  points,
const Axis axis 
)

◆ getNormalVector()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto math::geometry::getNormalVector ( const T &  orientation) -> geometry_msgs::msg::Vector3

◆ getPointsFromBbox()

std::vector< geometry_msgs::msg::Point > math::geometry::getPointsFromBbox ( const traffic_simulator_msgs::msg::BoundingBox &  bounding_box,
const double  width_extension_right = 0.0,
const double  width_extension_left = 0.0,
const double  length_extension_front = 0.0,
const double  length_extension_rear = 0.0 
)

◆ getPolygonDistance()

std::optional< double > math::geometry::getPolygonDistance ( const geometry_msgs::msg::Pose &  pose0,
const traffic_simulator_msgs::msg::BoundingBox &  bbox0,
const geometry_msgs::msg::Pose &  pose1,
const traffic_simulator_msgs::msg::BoundingBox &  bbox1 
)

Get the Polygon Distance object.

Parameters
pose0pose of the first bounding box
bbox0size of the first bounding box
pose1pose of the second bounding box
bbox1size of the second bounding box
Return values
std::nulloptbounding box intersects
0<= distance between two bounding boxes

◆ getRelativePose()

const geometry_msgs::msg::Pose math::geometry::getRelativePose ( const geometry_msgs::msg::Pose &  from,
const geometry_msgs::msg::Pose &  to 
)

Get the relative pose between two poses.

Parameters
fromorigin pose
totarget pose
Returns
const geometry_msgs::msg::Pose relative pose from origin to target

◆ getRotation()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto math::geometry::getRotation ( from,
to 
)

◆ getRotationMatrix()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>
auto math::geometry::getRotationMatrix ( quat) -> Eigen::Matrix3d

◆ hypot()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto math::geometry::hypot ( const T &  from,
const U &  to 
)

◆ innerProduct()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3< U >>, std::nullptr_t > = nullptr>
auto math::geometry::innerProduct ( const T &  v0,
const U &  v1 
)

◆ isIntersect2D() [1/2]

bool math::geometry::isIntersect2D ( const LineSegment line0,
const LineSegment line1 
)

◆ isIntersect2D() [2/2]

bool math::geometry::isIntersect2D ( const std::vector< LineSegment > &  lines)

◆ makeQuaternion()

template<typename T , typename U , typename V , typename W , std::enable_if_t< std::conjunction_v< std::is_scalar< T >, std::is_scalar< U >, std::is_scalar< V >, std::is_scalar< W >>, std::nullptr_t > = nullptr>
auto math::geometry::makeQuaternion ( const T &  x,
const U &  y,
const V &  z,
const W &  w 
)

◆ norm()

template<typename T , std::enable_if_t< IsLikeVector3< T >::value, std::nullptr_t > = nullptr>
auto math::geometry::norm ( const T &  v)

◆ normalize()

template<typename T , std::enable_if_t< IsLikeVector3< T >::value, std::nullptr_t > = nullptr>
auto math::geometry::normalize ( const T &  v)

◆ operator*()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto math::geometry::operator* ( const T &  a,
const U &  b 
)

◆ operator+()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto math::geometry::operator+ ( const T &  a,
const U &  b 
)

◆ operator+=()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto math::geometry::operator+= ( T &  a,
const U &  b 
) -> decltype(auto)

◆ operator-()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >>, std::nullptr_t > = nullptr>
auto math::geometry::operator- ( const T &  a,
const U &  b 
)

◆ operator/()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, std::is_scalar< U >>, std::nullptr_t > = nullptr>
auto math::geometry::operator/ ( const T &  a,
const U &  b 
)

◆ pointToSegmentProjection()

boost_point math::geometry::pointToSegmentProjection ( const boost_point p,
const boost_point p1,
const boost_point p2 
)

◆ slerp()

template<typename T , typename U , typename V , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >, IsLikeQuaternion< U >, std::is_scalar< V >>, std::nullptr_t > = nullptr>
auto math::geometry::slerp ( quat1,
quat2,
t 
)

◆ subtractPoses()

geometry_msgs::msg::Pose math::geometry::subtractPoses ( const geometry_msgs::msg::Pose &  pose1,
const geometry_msgs::msg::Pose &  pose2 
)

◆ toBoostPoint()

boost_point math::geometry::toBoostPoint ( const geometry_msgs::msg::Point &  point)

◆ toBoostPolygon()

boost_polygon math::geometry::toBoostPolygon ( const std::vector< geometry_msgs::msg::Point > &  points)

◆ toPolygon2D() [1/2]

auto math::geometry::toPolygon2D ( const geometry_msgs::msg::Pose &  pose,
const traffic_simulator_msgs::msg::BoundingBox &  bounding_box 
) -> boost_polygon

◆ toPolygon2D() [2/2]

auto math::geometry::toPolygon2D ( const traffic_simulator_msgs::msg::BoundingBox &  bounding_box) -> std::vector<geometry_msgs::msg::Point>

◆ toPose()

geometry_msgs::msg::Pose math::geometry::toPose ( const boost_point point)

◆ transformPoint() [1/2]

const geometry_msgs::msg::Point math::geometry::transformPoint ( const geometry_msgs::msg::Pose &  pose,
const geometry_msgs::msg::Point &  point 
)

Get transformed pose in world frame.

Parameters
posepose world frame
pointpoint in local frame
Returns
const geometry_msgs::msg::Point transformed point

◆ transformPoint() [2/2]

const geometry_msgs::msg::Point math::geometry::transformPoint ( const geometry_msgs::msg::Pose &  pose,
const geometry_msgs::msg::Pose &  sensor_pose,
const geometry_msgs::msg::Point &  point 
)

Get transformed pose in sensor frame.

Parameters
posepose in world frame
sensor_posesensor pose in world frame
pointpoint in local frame
Returns
const geometry_msgs::msg::Point transformed point

◆ transformPoints() [1/2]

std::vector< geometry_msgs::msg::Point > math::geometry::transformPoints ( const geometry_msgs::msg::Pose &  pose,
const geometry_msgs::msg::Pose &  sensor_pose,
const std::vector< geometry_msgs::msg::Point > &  points 
)

Get transformed points in sensor frame.

Parameters
posepose in world frame
pointspoints in local frame
Returns
std::vector<geometry_msgs::msg::Point> transformed points

◆ transformPoints() [2/2]

std::vector< geometry_msgs::msg::Point > math::geometry::transformPoints ( const geometry_msgs::msg::Pose &  pose,
const std::vector< geometry_msgs::msg::Point > &  points 
)

Get transformed points in world frame.

Parameters
posepose in world frame
pointspoints in local frame
Returns
std::vector<geometry_msgs::msg::Point> transformed points

◆ truncate()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, std::is_scalar< U >>, std::nullptr_t > = nullptr>
auto math::geometry::truncate ( const T &  v,
const U &  max 
)

◆ vector3()

template<typename T , typename U , typename V , std::enable_if_t< std::conjunction_v< std::is_scalar< T >, std::is_scalar< U >, std::is_scalar< V >>, std::nullptr_t > = nullptr>
auto math::geometry::vector3 ( const T &  x,
const U &  y,
const V &  z 
)