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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019