Classes
struct	DistancesFromCenterToEdge

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 (geometry_msgs::msg::Pose pose0, traffic_simulator_msgs::msg::BoundingBox bbox0, geometry_msgs::msg::Pose pose1, 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 , 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
X
Y
Z

Function Documentation

◆ checkCollision2D()

bool math::geometry::checkCollision2D	(	geometry_msgs::msg::Pose	pose0,
		traffic_simulator_msgs::msg::BoundingBox	bbox0,
		geometry_msgs::msg::Pose	pose1,
		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 )

◆ 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>, 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

points	Points you want to build line segments.
close_start_end	If 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
	)

◆ 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

pose0	pose of the first bounding box
bbox0	size of the first bounding box
pose1	pose of the second bounding box
bbox1	size of the second bounding box

Return values

std::nullopt	bounding 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

from	origin pose
to	target 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>, std::nullptr_t > = nullptr>

auto math::geometry::getRotation	(	T	from,
		U	to
	)

◆ getRotationMatrix()

template<typename T , std::enable_if_t< std::conjunction_v< IsLikeQuaternion< T >>, std::nullptr_t > = nullptr>

auto math::geometry::getRotationMatrix ( T quat ) -> Eigen::Matrix3d

◆ hypot()

template<typename T , typename U , std::enable_if_t< std::conjunction_v< IsLikeVector3< T >, IsLikeVector3>, 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>, 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, 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>, 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>, 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>, 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>, 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>, 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, std::is_scalar< V >>, std::nullptr_t > = nullptr>

auto math::geometry::slerp	(	T	quat1,
		U	quat2,
		V	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

pose	pose world frame
point	point 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

pose	pose in world frame
sensor_pose	sensor pose in world frame
point	point 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

pose	pose in world frame
points	points 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

pose	pose in world frame
points	points 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>, 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, std::is_scalar< V >>, std::nullptr_t > = nullptr>

auto math::geometry::vector3	(	const T &	x,
		const U &	y,
		const V &	z
	)

Classes

Typedefs

Enumerations

Functions

Typedef Documentation

◆ boost_point

◆ boost_polygon

Enumeration Type Documentation

◆ Axis

Function Documentation

◆ checkCollision2D()

◆ contains()

◆ convertEulerAngleToQuaternion()

◆ convertQuaternionToEulerAngle()

◆ filterByAxis()

◆ get2DConvexHull()

◆ getClosestPoses()

◆ getDistance() [1/4]

◆ getDistance() [2/4]

◆ getDistance() [3/4]

◆ getDistance() [4/4]

◆ getDistance2D()

◆ getDistancesFromCenterToEdge()

◆ getInternalAngle()

◆ getIntersection2D() [1/2]

◆ getIntersection2D() [2/2]

◆ getLineSegments()

◆ getMaxValue()

◆ getMinValue()

◆ getPointsFromBbox()

◆ getPolygonDistance()

◆ getRelativePose()

◆ getRotation()

◆ getRotationMatrix()

◆ hypot()

◆ innerProduct()

◆ isIntersect2D() [1/2]

◆ isIntersect2D() [2/2]

◆ makeQuaternion()

◆ norm()

◆ normalize()

◆ operator*()

◆ operator+()

◆ operator+=()

◆ operator-()

◆ operator/()

◆ pointToSegmentProjection()

◆ slerp()

◆ subtractPoses()

◆ toBoostPoint()

◆ toBoostPolygon()

◆ toPolygon2D() [1/2]

◆ toPolygon2D() [2/2]

◆ toPose()

◆ transformPoint() [1/2]

◆ transformPoint() [2/2]

◆ transformPoints() [1/2]

◆ transformPoints() [2/2]

◆ truncate()

◆ vector3()