mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
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/Point.h>
14 #include <geometry_msgs/Quaternion.h>
15 
16 namespace 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
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition: attitude_converter.h:29
mrs_lib::geometry::headingFromRot
double headingFromRot(const T &rot)
Returns the heading angle from the rotation.
Definition: misc.h:57
mrs_lib::geometry::rotateCovariance
mat3_t rotateCovariance(const mat3_t &cov, const T &rot)
Returns the covariance rotated using the specified rotation.
Definition: misc.h:313