13#include <geometry_msgs/msg/point.h>
14#include <geometry_msgs/msg/quaternion.h>
21 using vec_t = Eigen::Matrix<double, dims, 1>;
23 using pt2_t = vec_t<2>;
24 using vec2_t = vec_t<2>;
25 using pt3_t = vec_t<3>;
26 using vec3_t = vec_t<3>;
28 using mat3_t = Eigen::Matrix3d;
30 using quat_t = Eigen::Quaterniond;
31 using anax_t = Eigen::AngleAxisd;
34 vec_t<dims + 1> toHomogenous(
const vec_t<dims>& vec)
36 const Eigen::Matrix<double, dims + 1, 1> ret((Eigen::Matrix<double, dims + 1, 1>() << vec, 1).finished());
59 const vec3_t rot_vec = rot * vec3_t::UnitX();
60 return std::atan2(rot_vec.y(), rot_vec.x());
79 double angleBetween(
const vec2_t& a,
const vec2_t& b);
93 double angleBetween(
const vec3_t& a,
const vec3_t& b);
146 Eigen::Matrix3d
rotationBetween(
const vec3_t& a,
const vec3_t& b,
const double tolerance = 1e-9);
159 double haversin(
const double angle);
187 double solidAngle(
double a,
double b,
double c);
243 double cross(
const vec2_t& a,
const vec2_t b);
257 double dist(
const vec2_t& a,
const vec2_t& b);
267 double dist(
const vec3_t& a,
const vec3_t& b);
282 double triangleArea(
const double a,
const double b,
const double c);
312 template <
typename T>
315 const mat3_t matrot(rot);
316 return matrot * cov * matrot.transpose();
anax_t angleaxisBetween(const vec3_t &a, const vec3_t &b, const double tolerance=1e-9)
Returns the rotation between two vectors, represented as angle-axis.
double solidAngle(double a, double b, double c)
computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a,...
Definition misc.cpp:130
double dist(const vec2_t &a, const vec2_t &b)
distnace between two 2D Eigen vectors
Definition misc.cpp:170
Eigen::Matrix3d 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.
double invHaversin(const double value)
computes the inverse haversine angle for a given value
Definition misc.cpp:122
double haversin(const double angle)
computes the haversine (half of versine) for a given angle
Definition misc.cpp:113
double triangleArea(const double a, const double b, const double c)
uses Heron's formula to compute area of a given triangle using side lengths
Definition misc.cpp:140
double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c)
compute the area of a 'triangle' drawn on a spherical surface
Definition misc.cpp:149
quat_t quaternionFromHeading(const double heading)
create a quaternion from heading
Definition misc.cpp:92
double headingFromRot(const T &rot)
Returns the heading angle from the rotation.
Definition misc.h:57
quat_t quaternionFromEuler(double x, double y, double z)
create a quaternion from 3 provided Euler angles
Definition misc.cpp:79
mat3_t rotateCovariance(const mat3_t &cov, const T &rot)
Returns the covariance rotated using the specified rotation.
Definition misc.h:313
double cross(const vec2_t &a, const vec2_t b)
Implementation of cross product for 2D vectors.
Definition misc.cpp:19
quat_t quaternionBetween(const vec3_t &a, const vec3_t &b, const double tolerance=1e-9)
Returns the rotation between two vectors, represented as a quaternion.
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24