MRPT  2.0.4
TPose3D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/bits_math.h>
12 #include <mrpt/core/optional_ref.h>
13 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/TPoseOrPoint.h>
15 #include <mrpt/math/wrap2pi.h>
16 
17 namespace mrpt::math
18 {
19 /**
20  * Lightweight 3D pose (three spatial coordinates, plus three angular
21  * coordinates). Allows coordinate access using [] operator.
22  * \sa mrpt::poses::CPose3D
23  */
24 struct TPose3D : public TPoseOrPoint,
25  public internal::ProvideStaticResize<TPose3D>
26 {
27  enum
28  {
30  };
31  /** X,Y,Z, coords */
32  double x{.0}, y{.0}, z{.0};
33  /** Yaw coordinate (rotation angle over Z axis). */
34  double yaw{.0};
35  /** Pitch coordinate (rotation angle over Y axis). */
36  double pitch{.0};
37  /** Roll coordinate (rotation angle over X coordinate). */
38  double roll{.0};
39 
40  /** Returns the identity transformation, T=eye(4) */
41  static constexpr TPose3D Identity() { return TPose3D(); }
42 
43  /** Implicit constructor from TPoint2D. Zeroes all the unprovided
44  * information.
45  * \sa TPoint2D
46  */
47  TPose3D(const TPoint2D& p);
48  /**
49  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi,
50  * zeroing all the unprovided information.
51  * \sa TPose2D
52  */
53  TPose3D(const TPose2D& p);
54  /**
55  * Implicit constructor from TPoint3D. Zeroes angular information.
56  * \sa TPoint3D
57  */
58  TPose3D(const TPoint3D& p);
59  /**
60  * Constructor from coordinates.
61  */
62  constexpr TPose3D(
63  double _x, double _y, double _z, double _yaw, double _pitch,
64  double _roll)
65  : x(_x), y(_y), z(_z), yaw(_yaw), pitch(_pitch), roll(_roll)
66  {
67  }
68  /**
69  * Default fast constructor. Initializes to zeros.
70  */
71  constexpr TPose3D() = default;
72  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
73  double& operator[](size_t i)
74  {
75  switch (i)
76  {
77  case 0:
78  return x;
79  case 1:
80  return y;
81  case 2:
82  return z;
83  case 3:
84  return yaw;
85  case 4:
86  return pitch;
87  case 5:
88  return roll;
89  default:
90  throw std::out_of_range("index out of range");
91  }
92  }
93  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
94  constexpr double operator[](size_t i) const
95  {
96  switch (i)
97  {
98  case 0:
99  return x;
100  case 1:
101  return y;
102  case 2:
103  return z;
104  case 3:
105  return yaw;
106  case 4:
107  return pitch;
108  case 5:
109  return roll;
110  default:
111  throw std::out_of_range("index out of range");
112  }
113  }
114  /**
115  * Pose's spatial coordinates norm.
116  */
117  double norm() const { return std::sqrt(square(x) + square(y) + square(z)); }
118  /**
119  * Gets the pose as a vector of doubles.
120  */
121  void asVector(std::vector<double>& v) const
122  {
123  v.resize(6);
124  v[0] = x;
125  v[1] = y;
126  v[2] = z;
127  v[3] = yaw;
128  v[4] = pitch;
129  v[5] = roll;
130  }
131  /** Returns a human-readable textual representation of the object (eg: "[x y
132  * z yaw pitch roll]", angles in degrees.)
133  * \sa fromString
134  */
135  void asString(std::string& s) const;
136  std::string asString() const
137  {
138  std::string s;
139  asString(s);
140  return s;
141  }
142 
143  /** Returns the quaternion associated to the rotation of this object (NOTE:
144  * XYZ translation is ignored)
145  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2)
146  * \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin
147  * (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta
148  * /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +
149  * \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos
150  * (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi
151  * /2) \\ \end{array}\right) \f]
152  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw
153  * \f$.
154  * \param out_dq_dr If provided, the 4x3 Jacobian of the transformation
155  * will be computed and stored here. It's the Jacobian of the transformation
156  * from (yaw pitch roll) to (qr qx qy qz).
157  */
158  void getAsQuaternion(
161  std::nullopt) const;
162 
163  void composePoint(const TPoint3D& l, TPoint3D& g) const;
164  TPoint3D composePoint(const TPoint3D& l) const;
165  void inverseComposePoint(const TPoint3D& g, TPoint3D& l) const;
166  TPoint3D inverseComposePoint(const TPoint3D& g) const;
167  void composePose(const TPose3D other, TPose3D& result) const;
170  {
173  return R;
174  }
177  {
180  return H;
181  }
184  {
187  return H;
188  }
190  static void SO3_to_yaw_pitch_roll(
191  const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
192  double& roll);
193  /** Set the current object value from a string generated by 'asString' (eg:
194  * "[0.02 1.04 -0.8]" )
195  * \sa asString
196  * \exception std::exception On invalid format
197  */
198  void fromString(const std::string& s);
199 
200  static TPose3D FromString(const std::string& s)
201  {
202  TPose3D o;
203  o.fromString(s);
204  return o;
205  }
206 };
207 
208 /** Unary $\ominus\$ operator: computes inverse SE(3) element */
209 TPose3D operator-(const TPose3D& p);
210 /** Binary $\ominus\$ operator: \$b \ominus a\$ computes the relative SE(3) pose
211  * of `b` "as seen from" `a` */
212 TPose3D operator-(const TPose3D& b, const TPose3D& a);
213 
214 /** Exact comparison between 3D poses, taking possible cycles into account */
215 inline bool operator==(const TPose3D& p1, const TPose3D& p2)
216 {
217  return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z) &&
222  mrpt::math::wrapTo2Pi(p2.roll)); //-V550
223 }
224 /** Exact comparison between 3D poses, taking possible cycles into account */
225 inline bool operator!=(const TPose3D& p1, const TPose3D& p2)
226 {
227  return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z) ||
232  mrpt::math::wrapTo2Pi(p2.roll)); //-V550
233 }
234 
235 } // namespace mrpt::math
236 
237 namespace mrpt::typemeta
238 {
239 // Specialization must occur in the same namespace
241 } // namespace mrpt::typemeta
std::vector< T1 > operator-(const std::vector< T1 > &v1, const std::vector< T2 > &v2)
Definition: ops_vectors.h:92
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
constexpr double operator[](size_t i) const
Coordinate access using operator[].
Definition: TPose3D.h:94
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
Base type of all TPoseXX and TPointXX classes in mrpt::math.
Definition: TPoseOrPoint.h:24
mrpt::math::CMatrixDouble33 getRotationMatrix() const
Definition: TPose3D.h:169
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
Definition: TPose3D.cpp:201
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const
Definition: TPose3D.h:176
double norm() const
Pose&#39;s spatial coordinates norm.
Definition: TPose3D.h:117
static constexpr TPose3D Identity()
Returns the identity transformation, T=eye(4)
Definition: TPose3D.h:41
This base provides a set of functions for maths stuff.
void composePose(const TPose3D other, TPose3D &result) const
Definition: TPose3D.cpp:209
constexpr TPose3D()=default
Default fast constructor.
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:38
void composePoint(const TPoint3D &l, TPoint3D &g) const
Definition: TPose3D.cpp:80
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: TPose3D.cpp:42
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
constexpr TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
Constructor from coordinates.
Definition: TPose3D.h:62
constexpr bool operator==(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:223
Base template for TPoint2D and TPoint2Df.
Definition: TPoint2D.h:32
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
static void SO3_to_yaw_pitch_roll(const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)
Definition: TPose3D.cpp:137
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
Definition: TPose3D.cpp:234
return_t square(const num_t x)
Inline function for the square of a number.
constexpr bool operator!=(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:230
static TPose3D FromString(const std::string &s)
Definition: TPose3D.h:200
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
const float R
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: TPose3D.h:183
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
void asVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
Definition: TPose3D.h:121
double & operator[](size_t i)
Coordinate access using operator[].
Definition: TPose3D.h:73
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void inverseComposePoint(const TPoint3D &g, TPoint3D &l) const
Definition: TPose3D.cpp:98
std::string asString() const
Definition: TPose3D.h:136



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020