mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Namespaces | Typedefs | Functions
misc.h File Reference

Defines useful geometry utilities and functions. More...

#include <cmath>
#include <Eigen/Dense>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
+ Include dependency graph for misc.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 mrs_lib
 All mrs_lib functions, classes, variables and definitions are contained in this namespace.
 

Typedefs

template<int dims>
using mrs_lib::geometry::vec_t = Eigen::Matrix< double, dims, 1 >
 
using mrs_lib::geometry::pt2_t = vec_t< 2 >
 
using mrs_lib::geometry::vec2_t = vec_t< 2 >
 
using mrs_lib::geometry::pt3_t = vec_t< 3 >
 
using mrs_lib::geometry::vec3_t = vec_t< 3 >
 
using mrs_lib::geometry::mat3_t = Eigen::Matrix3d
 
using mrs_lib::geometry::quat_t = Eigen::Quaterniond
 
using mrs_lib::geometry::anax_t = Eigen::AngleAxisd
 

Functions

template<int dims>
vec_t< dims+1 > mrs_lib::geometry::toHomogenous (const vec_t< dims > &vec)
 
template<typename T >
double mrs_lib::geometry::headingFromRot (const T &rot)
 Returns the heading angle from the rotation. More...
 
double mrs_lib::geometry::angleBetween (const vec2_t &a, const vec2_t &b)
 Returns the angle between two vectors, taking orientation into account. More...
 
double mrs_lib::geometry::angleBetween (const vec3_t &a, const vec3_t &b)
 Returns the angle between two vectors, taking orientation into account. More...
 
anax_t mrs_lib::geometry::angleaxisBetween (const vec3_t &a, const vec3_t &b, const double tolerance=1e-9)
 Returns the rotation between two vectors, represented as angle-axis. More...
 
quat_t mrs_lib::geometry::quaternionBetween (const vec3_t &a, const vec3_t &b, const double tolerance=1e-9)
 Returns the rotation between two vectors, represented as a quaternion. More...
 
Eigen::Matrix3d mrs_lib::geometry::rotationBetween (const vec3_t &a, const vec3_t &b, const double tolerance=1e-9)
 Returns the rotation between two vectors, represented as a rotation matrix. More...
 
double mrs_lib::geometry::haversin (const double angle)
 computes the haversine (half of versine) for a given angle More...
 
double mrs_lib::geometry::invHaversin (const double value)
 computes the inverse haversine angle for a given value More...
 
double mrs_lib::geometry::solidAngle (double a, double b, double c)
 computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a, b, c More...
 
quat_t mrs_lib::geometry::quaternionFromEuler (double x, double y, double z)
 create a quaternion from 3 provided Euler angles More...
 
quat_t mrs_lib::geometry::quaternionFromEuler (const Eigen::Vector3d &euler)
 create a quaternion from Euler angles provided as a vector More...
 
quat_t mrs_lib::geometry::quaternionFromHeading (const double heading)
 create a quaternion from heading More...
 
double mrs_lib::geometry::cross (const vec2_t &a, const vec2_t b)
 Implementation of cross product for 2D vectors. More...
 
double mrs_lib::geometry::dist (const vec2_t &a, const vec2_t &b)
 distnace between two 2D Eigen vectors More...
 
double mrs_lib::geometry::dist (const vec3_t &a, const vec3_t &b)
 distnace between two 3D Eigen vectors More...
 
double mrs_lib::geometry::triangleArea (const double a, const double b, const double c)
 uses Heron's formula to compute area of a given triangle using side lengths More...
 
double mrs_lib::geometry::sphericalTriangleArea (Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c)
 compute the area of a 'triangle' drawn on a spherical surface More...
 
template<typename T >
mat3_t mrs_lib::geometry::rotateCovariance (const mat3_t &cov, const T &rot)
 Returns the covariance rotated using the specified rotation. More...
 

Detailed Description

Defines useful geometry utilities and functions.

Author
Matouš Vrba - vrbam.nosp@m.ato@.nosp@m.fel.c.nosp@m.vut..nosp@m.cz
Petr Štibinger - stibi.nosp@m.pet@.nosp@m.fel.c.nosp@m.vut..nosp@m.cz

Function Documentation

◆ angleaxisBetween()

anax_t mrs_lib::geometry::angleaxisBetween ( const vec3_t &  a,
const vec3_t &  b,
const double  tolerance = 1e-9 
)

Returns the rotation between two vectors, represented as angle-axis.

To avoid singularities, a tolerance parameter is used:

  • If the absolute angle between the two vectors is less than tolerance, a zero rotation is returned.
  • If the angle between the two vectors is closer to $ \pi $ than tolerance, a $ \pi $ rotation is returned.
Parameters
avector from which the rotation starts.
bvector at which the rotation ends.
Returns
rotation from a to b.

◆ angleBetween() [1/2]

double mrs_lib::geometry::angleBetween ( const vec2_t &  a,
const vec2_t &  b 
)

Returns the angle between two vectors, taking orientation into account.

This implementation uses atan2 instead of just acos and thus it properly takes into account orientation of the vectors, returning angle in all four quadrants.

Parameters
avector from which the angle will be measured.
bvector to which the angle will be measured.
Returns
angle from a to b.

◆ angleBetween() [2/2]

double mrs_lib::geometry::angleBetween ( const vec3_t &  a,
const vec3_t &  b 
)

Returns the angle between two vectors, taking orientation into account.

This implementation uses atan2 instead of just acos and thus it properly takes into account orientation of the vectors, returning angle in all four quadrants.

Parameters
avector from which the angle will be measured.
bvector to which the angle will be measured.
Returns
angle from a to b.

◆ cross()

double mrs_lib::geometry::cross ( const vec2_t &  a,
const vec2_t  b 
)

Implementation of cross product for 2D vectors.

Useful e.g. for finding the sine of an angle between two 2D vectors.

Parameters
afirst vector of the cross product.
bsecond vector of the cross product.
Returns
$ a \times b $ (sine of the angle from a to b).

◆ dist() [1/2]

double mrs_lib::geometry::dist ( const vec2_t &  a,
const vec2_t &  b 
)

distnace between two 2D Eigen vectors

Parameters
a
b
Returns
Euclidean distance

◆ dist() [2/2]

double mrs_lib::geometry::dist ( const vec3_t &  a,
const vec3_t &  b 
)

distnace between two 3D Eigen vectors

Parameters
a
b
Returns
Euclidean distance

◆ haversin()

double mrs_lib::geometry::haversin ( const double  angle)

computes the haversine (half of versine) for a given angle

Parameters
angleangle in radians
Returns

◆ headingFromRot()

template<typename T >
double mrs_lib::geometry::headingFromRot ( const T &  rot)

Returns the heading angle from the rotation.

Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.

Parameters
rotthe rotation to extract the heading angle from.
Returns
the heading angle.

◆ invHaversin()

double mrs_lib::geometry::invHaversin ( const double  value)

computes the inverse haversine angle for a given value

Parameters
value
Returns
angle in radians

◆ quaternionBetween()

quat_t mrs_lib::geometry::quaternionBetween ( const vec3_t &  a,
const vec3_t &  b,
const double  tolerance = 1e-9 
)

Returns the rotation between two vectors, represented as a quaternion.

Works the same as the angleaxisBetween() function (in fact it is used in the implementation).

Parameters
avector from which the rotation starts.
bvector at which the rotation ends.
Returns
rotation from a to b.

◆ quaternionFromEuler() [1/2]

quat_t mrs_lib::geometry::quaternionFromEuler ( const Eigen::Vector3d &  euler)

create a quaternion from Euler angles provided as a vector

Parameters
eulercomponents of the rotation provided as vector of Euler angles
Returns
quaternion

◆ quaternionFromEuler() [2/2]

quat_t mrs_lib::geometry::quaternionFromEuler ( double  x,
double  y,
double  z 
)

create a quaternion from 3 provided Euler angles

Parameters
xEuler angle in radians
yEuler angle in radians
zEuler angle in radians
Returns
quaternion

◆ quaternionFromHeading()

quat_t mrs_lib::geometry::quaternionFromHeading ( const double  heading)

create a quaternion from heading

Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.

Parameters
headingthe heading angle.
Returns
the quaternion corresponding to the heading rotation.

◆ rotateCovariance()

template<typename T >
mat3_t mrs_lib::geometry::rotateCovariance ( const mat3_t &  cov,
const T &  rot 
)

Returns the covariance rotated using the specified rotation.

Parameters
covthe covariance to be rotated.
rotthe rotation use.
Returns
a new matrix object containing the rotated covariance.

◆ rotationBetween()

Eigen::Matrix3d mrs_lib::geometry::rotationBetween ( const vec3_t &  a,
const vec3_t &  b,
const double  tolerance = 1e-9 
)

Returns the rotation between two vectors, represented as a rotation matrix.

Works the same as the angleaxisBetween() function (in fact it is used in the implementation).

Parameters
avector from which the rotation starts.
bvector at which the rotation ends.
Returns
rotation from a to b.

◆ solidAngle()

double mrs_lib::geometry::solidAngle ( double  a,
double  b,
double  c 
)

computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a, b, c

Parameters
a
b
c
Returns
solid angle in steradians

◆ sphericalTriangleArea()

double mrs_lib::geometry::sphericalTriangleArea ( Eigen::Vector3d  a,
Eigen::Vector3d  b,
Eigen::Vector3d  c 
)

compute the area of a 'triangle' drawn on a spherical surface

Parameters
alength of side1
blength of side2
clength of side3
Returns
area in units squared

◆ triangleArea()

double mrs_lib::geometry::triangleArea ( const double  a,
const double  b,
const double  c 
)

uses Heron's formula to compute area of a given triangle using side lengths

Parameters
alength of side1
blength of side2
clength of side3
Returns
area in units squared