mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
misc.h
Go to the documentation of this file.
1// clang: MatousFormat
8#ifndef GEOMETRY_MISC_H
9#define GEOMETRY_MISC_H
10
11#include <cmath>
12#include <Eigen/Dense>
13#include <geometry_msgs/msg/point.h>
14#include <geometry_msgs/msg/quaternion.h>
15
16namespace mrs_lib
17{
18 namespace geometry
19 {
20 template <int dims>
21 using vec_t = Eigen::Matrix<double, dims, 1>;
22
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>;
27
28 using mat3_t = Eigen::Matrix3d;
29
30 using quat_t = Eigen::Quaterniond;
31 using anax_t = Eigen::AngleAxisd;
32
33 template <int dims>
34 vec_t<dims + 1> toHomogenous(const vec_t<dims>& vec)
35 {
36 const Eigen::Matrix<double, dims + 1, 1> ret((Eigen::Matrix<double, dims + 1, 1>() << vec, 1).finished());
37 return ret;
38 }
39
40 // | ----------------- Angle-related functions ---------------- |
41
42 /* angle-related functions //{ */
43
44 /* headingFromRot() //{ */
45
56 template <typename T>
57 double headingFromRot(const T& rot)
58 {
59 const vec3_t rot_vec = rot * vec3_t::UnitX();
60 return std::atan2(rot_vec.y(), rot_vec.x());
61 }
62
63 //}
64
65 /* angleBetween() //{ */
66
79 double angleBetween(const vec2_t& a, const vec2_t& b);
80
93 double angleBetween(const vec3_t& a, const vec3_t& b);
94
95 //}
96
97 /* angleaxisBetween() //{ */
98
112 anax_t angleaxisBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
113
114 //}
115
116 /* quaternionBetween() //{ */
117
129 quat_t quaternionBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
130
131 //}
132
133 /* rotationBetween() //{ */
134
146 Eigen::Matrix3d rotationBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
147
148 //}
149
150 /* haversin() //{ */
151
159 double haversin(const double angle);
160
161 //}
162
163 /* invHaversin() //{ */
164
172 double invHaversin(const double value);
173
174 //}
175
176 /* solidAngle //{ */
177
187 double solidAngle(double a, double b, double c);
188
189 //}
190
191 /* quaternionFromEuler() //{ */
192
202 quat_t quaternionFromEuler(double x, double y, double z);
203
211 quat_t quaternionFromEuler(const Eigen::Vector3d& euler);
212
213 //}
214
224 quat_t quaternionFromHeading(const double heading);
225
226 //}
227
228 // | ----------------- Miscellaneous functions ---------------- |
229
230 /* 2D cross() //{ */
231
243 double cross(const vec2_t& a, const vec2_t b);
244
245 //}
246
247 /* vector distance //{ */
248
257 double dist(const vec2_t& a, const vec2_t& b);
258
267 double dist(const vec3_t& a, const vec3_t& b);
268
269 //}
270
271 /* triangleArea() //{ */
272
282 double triangleArea(const double a, const double b, const double c);
283
284 //}
285
286 /* sphericalTriangleArea //{ */
287
297 double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
298
299 //}
300
301 /* rotateCovariance() //{ */
302
312 template <typename T>
313 mat3_t rotateCovariance(const mat3_t& cov, const T& rot)
314 {
315 const mat3_t matrot(rot);
316 return matrot * cov * matrot.transpose();
317 }
318
319 //}
320
321 } // namespace geometry
322} // namespace mrs_lib
323
324#endif
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