MRPT  1.9.9
CPose3D.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/math/CMatrixFixed.h>
12 #include <mrpt/math/CQuaternion.h>
13 #include <mrpt/math/TPoint2D.h>
14 #include <mrpt/math/TPoint3D.h>
15 #include <mrpt/poses/CPose.h>
17 
18 // Add for declaration of mexplus::from template specialization
20 
21 namespace mrpt::poses
22 {
23 class CPose3DQuat;
24 
25 /** A class used to store a 3D pose (a 3D translation + a rotation in 3D).
26  * The 6D transformation in SE(3) stored in this class is kept in two
27  * separate containers: a 3-array for the translation, and a 3x3 rotation
28  * matrix.
29  *
30  * This class allows parameterizing 6D poses as a 6-vector: [x y z yaw pitch
31  * roll] (read below
32  * for the angles convention). Note however,
33  * that the yaw/pitch/roll angles are only computed (on-demand and
34  * transparently)
35  * when the user requests them. Normally, rotations and transformations are
36  * always handled
37  * via the 3x3 rotation matrix.
38  *
39  * Yaw/Pitch/Roll angles are defined as successive rotations around *local*
40  * (dynamic) axes in the Z/Y/X order:
41  *
42  * <div align=center>
43  * <img src="CPose3D.gif">
44  * </div>
45  *
46  * It may be extremely confusing and annoying to find a different criterion also
47  * involving
48  * the names "yaw, pitch, roll" but regarding rotations around *global* (static)
49  * axes.
50  * Fortunately, it's very easy to see (by writing down the product of the three
51  * rotation matrices) that both conventions lead to exactly the same numbers.
52  * Only, that it's conventional to write the numbers in reverse order.
53  * That is, the same rotation can be described equivalently with any of these
54  * two
55  * parameterizations:
56  *
57  * - In local axes Z/Y/X convention: [yaw pitch roll] (This is the convention
58  * used in mrpt::poses::CPose3D)
59  * - In global axes X/Y/Z convention: [roll pitch yaw] (One of the Euler angles
60  * conventions)
61  *
62  * For further descriptions of point & pose classes, see
63  * mrpt::poses::CPoseOrPoint or refer
64  * to the [2D/3D Geometry tutorial](http://www.mrpt.org/2D_3D_Geometry) online.
65  *
66  * To change the individual components of the pose, use CPose3D::setFromValues.
67  * This class assures that the internal
68  * 3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
69  *
70  * Rotations in 3D can be also represented by quaternions. See
71  * mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
72  *
73  * This class and CPose3DQuat are very similar, and they can be converted to the
74  * each other automatically via transformation constructors.
75  *
76  * For Lie algebra methods, see mrpt::poses::Lie.
77  *
78  * \note Read also: "A tutorial on SE(3) transformation parameterizations and
79  * on-manifold optimization", in \cite blanco_se3_tutorial
80  *
81  * \ingroup poses_grp
82  * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
83  */
84 class CPose3D : public CPose<CPose3D, 6>,
86 {
89 
90  // This must be added for declaration of MEX-related functions
92 
93  public:
94  /** The translation vector [x,y,z] access directly or with x(), y(), z()
95  * setter/getter methods. */
97 
98  protected:
99  /** The 3x3 rotation matrix, access with getRotationMatrix(),
100  * setRotationMatrix() (It's not safe to set this field as public) */
102 
103  /** Whether yaw/pitch/roll members are up-to-date since the last rotation
104  * matrix update. */
105  mutable bool m_ypr_uptodate{true};
106  /** These variables are updated every time that the object rotation matrix
107  * is modified (construction, loading from values, pose composition, etc )
108  */
109  mutable double m_yaw{0}, m_pitch{0}, m_roll{0};
110 
111  /** Rebuild the homog matrix from the angles. */
112  void rebuildRotationMatrix();
113 
114  /** Updates Yaw/pitch/roll members from the m_ROT */
115  inline void updateYawPitchRoll() const
116  {
117  if (!m_ypr_uptodate)
118  {
119  m_ypr_uptodate = true;
121  }
122  }
123 
124  public:
125  /** @name Constructors
126  @{ */
127 
128  /** Default constructor, with all the coordinates set to zero. */
129  CPose3D();
130 
131  /** Constructor with initilization of the pose; (remember that angles are
132  * always given in radians!) */
133  CPose3D(
134  const double x, const double y, const double z, const double yaw = 0,
135  const double pitch = 0, const double roll = 0);
136 
137  /** Returns the identity transformation */
138  static CPose3D Identity() { return CPose3D(); }
139 
140  /** Constructor from a 4x4 homogeneous matrix - the passed matrix can be
141  * actually of any size larger than or equal 3x4, since only those first
142  * values are used (the last row of a homogeneous 4x4 matrix are always
143  * fixed). */
144  explicit CPose3D(const math::CMatrixDouble& m);
145 
146  /** Constructor from a 4x4 homogeneous matrix: */
147  explicit CPose3D(const math::CMatrixDouble44& m);
148 
149  /** Constructor from a 3x3 rotation matrix and a the translation given as a
150  * 3-vector, a 3-array, a CPoint3D or a mrpt::math::TPoint3D */
151  template <class MATRIX33, class VECTOR3>
152  inline CPose3D(const MATRIX33& rot, const VECTOR3& xyz)
154  {
155  ASSERT_EQUAL_(rot.rows(), 3);
156  ASSERT_EQUAL_(rot.cols(), 3);
157  ASSERT_EQUAL_(xyz.size(), 3);
158  for (int r = 0; r < 3; r++)
159  for (int c = 0; c < 3; c++) m_ROT(r, c) = rot(r, c);
160  for (int r = 0; r < 3; r++) m_coords[r] = xyz[r];
161  }
162  //! \overload
163  inline CPose3D(
164  const mrpt::math::CMatrixDouble33& rot,
166  : m_coords(xyz), m_ROT(rot), m_ypr_uptodate(false)
167  {
168  }
169 
170  /** Constructor from a CPose2D object.
171  */
172  explicit CPose3D(const CPose2D&);
173 
174  /** Constructor from a CPoint3D object.
175  */
176  explicit CPose3D(const CPoint3D&);
177 
178  /** Constructor from lightweight object.
179  */
180  explicit CPose3D(const mrpt::math::TPose3D&);
181 
183 
184  /** Constructor from a quaternion (which only represents the 3D rotation
185  * part) and a 3D displacement. */
186  CPose3D(
187  const mrpt::math::CQuaternionDouble& q, const double x, const double y,
188  const double z);
189 
190  /** Constructor from a CPose3DQuat. */
191  explicit CPose3D(const CPose3DQuat&);
192 
193  /** Fast constructor that leaves all the data uninitialized - call with
194  * UNINITIALIZED_POSE as argument */
197  {
198  }
199 
200  /** Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22
201  * r32 r13 r23 r33 tx ty tz]
202  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
203  * the 3D translation of the pose
204  * \sa setFrom12Vector, getAs12Vector
205  */
206  inline explicit CPose3D(const mrpt::math::CVectorFixedDouble<12>& vec12)
208  {
209  setFrom12Vector(vec12);
210  }
211 
212  /** @} */ // end Constructors
213 
214  /** @name Access 3x3 rotation and 4x4 homogeneous matrices
215  @{ */
216 
217  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
218  * point(translation) or pose (translation+orientation).
219  * \sa getInverseHomogeneousMatrix, getRotationMatrix
220  */
222 
223  /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
225  {
226  ROT = m_ROT;
227  }
228  //! \overload
230  {
231  return m_ROT;
232  }
233 
234  /** Sets the 3x3 rotation matrix \sa getRotationMatrix, getHomogeneousMatrix
235  */
237  {
238  m_ROT = ROT;
239  m_ypr_uptodate = false;
240  }
241 
242  /** @} */ // end rot and HM
243 
244  /** @name Pose-pose and pose-point compositions and operators
245  @{ */
246 
247  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
248  inline CPose3D operator+(const CPose3D& b) const
249  {
251  ret.composeFrom(*this, b);
252  return ret;
253  }
254 
255  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
256  CPoint3D operator+(const CPoint3D& b) const;
257 
258  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
259  CPoint3D operator+(const CPoint2D& b) const;
260 
261  /** Computes the spherical coordinates of a 3D point as seen from the 6D
262  * pose specified by this object. For the coordinate system see the top of
263  * this page. */
265  const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
266  double& out_pitch) const;
267 
268  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
269  * \f$ with G and L being 3D points and P this 6D pose.
270  * If pointers are provided, the corresponding Jacobians are returned.
271  * "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D
272  * locally Euclidean vector in the tangent space of SE(3).
273  * See [this
274  * report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
275  * for mathematical details.
276  * \param If set to true, the Jacobian "out_jacobian_df_dpose" uses a
277  * fastest linearized appoximation (valid only for small rotations!).
278  */
279  void composePoint(
280  double lx, double ly, double lz, double& gx, double& gy, double& gz,
281  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint =
282  nullptr,
283  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose = nullptr,
284  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3 = nullptr,
285  bool use_small_rot_approx = false) const;
286 
287  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
288  * \f$ with G and L being 3D points and P this 6D pose.
289  * \note local_point is passed by value to allow global and local point to
290  * be the same variable
291  */
292  inline void composePoint(
293  const mrpt::math::TPoint3D& local_point,
294  mrpt::math::TPoint3D& global_point) const
295  {
296  composePoint(
297  local_point.x, local_point.y, local_point.z, global_point.x,
298  global_point.y, global_point.z);
299  }
300  /** \overload Returns global point: "this \oplus l" */
302  const mrpt::math::TPoint3D& l) const
303  {
305  composePoint(l, g);
306  return g;
307  }
308 
309  /** This version of the method assumes that the resulting point has no Z
310  * component (use with caution!) */
311  inline void composePoint(
312  const mrpt::math::TPoint3D& local_point,
313  mrpt::math::TPoint2D& global_point) const
314  {
315  double dummy_z;
316  composePoint(
317  local_point.x, local_point.y, local_point.z, global_point.x,
318  global_point.y, dummy_z);
319  }
320 
321  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
322  * \f$ with G and L being 3D points and P this 6D pose. */
323  inline void composePoint(
324  double lx, double ly, double lz, float& gx, float& gy, float& gz) const
325  {
326  double ggx, ggy, ggz;
327  composePoint(lx, ly, lz, ggx, ggy, ggz);
328  gx = static_cast<float>(ggx);
329  gy = static_cast<float>(ggy);
330  gz = static_cast<float>(ggz);
331  }
332 
333  /** Rotates a vector (i.e. like composePoint(), but ignoring translation) */
335  const mrpt::math::TVector3D& local) const;
336 
337  /** Inverse of rotateVector(), i.e. using the inverse rotation matrix */
339  const mrpt::math::TVector3D& global) const;
340 
341  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
342  * If pointers are provided, the corresponding Jacobians are returned.
343  * "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D
344  * locally Euclidean vector in the tangent space of SE(3).
345  * See [this
346  * report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
347  * for mathematical details.
348  * \sa composePoint, composeFrom
349  */
350  void inverseComposePoint(
351  const double gx, const double gy, const double gz, double& lx,
352  double& ly, double& lz,
353  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint =
354  nullptr,
355  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose = nullptr,
356  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3 =
357  nullptr) const;
358 
359  /** \overload */
360  inline void inverseComposePoint(
362  {
363  inverseComposePoint(g.x, g.y, g.z, l.x, l.y, l.z);
364  }
365  /** \overload Returns local point: `g` as seen from `this` pose */
367  const mrpt::math::TPoint3D& g) const
368  {
371  return l;
372  }
373 
374  /** overload for 2D points \exception If the z component of the result is
375  * greater than some epsilon */
376  inline void inverseComposePoint(
378  const double eps = 1e-6) const
379  {
380  double lz;
381  inverseComposePoint(g.x, g.y, 0, l.x, l.y, lz);
382  ASSERT_BELOW_(std::abs(lz), eps);
383  }
384 
385  /** Makes "this = A (+) B"; this method is slightly more efficient than
386  * "this= A + B;" since it avoids the temporary object.
387  * \note A or B can be "this" without problems.
388  */
389  void composeFrom(const CPose3D& A, const CPose3D& B);
390 
391  /** Make \f$ this = this \oplus b \f$ (\a b can be "this" without problems)
392  */
393  inline CPose3D& operator+=(const CPose3D& b)
394  {
395  composeFrom(*this, b);
396  return *this;
397  }
398 
399  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
400  * than "this= A - B;" since it avoids the temporary object.
401  * \note A or B can be "this" without problems.
402  * \sa composeFrom, composePoint
403  */
404  void inverseComposeFrom(const CPose3D& A, const CPose3D& B);
405 
406  /** Compute \f$ RET = this \oplus b \f$ */
407  inline CPose3D operator-(const CPose3D& b) const
408  {
410  ret.inverseComposeFrom(*this, b);
411  return ret;
412  }
413 
414  /** Convert this pose into its inverse, saving the result in itself. \sa
415  * operator- */
416  void inverse();
417 
418  /** makes: this = p (+) this */
420  {
421  composeFrom(p, CPose3D(*this));
422  }
423 
424  /** @} */ // compositions
425 
426  /** Return the opposite of the current pose instance by taking the negative
427  * of all its components \a individually
428  */
429  CPose3D getOppositeScalar() const;
430 
431  /** @name Access and modify contents
432  @{ */
433 
434  /** Scalar sum of all 6 components: This is diferent from poses composition,
435  * which is implemented as "+" operators.
436  * \sa normalizeAngles
437  */
438  void addComponents(const CPose3D& p);
439 
440  /** Rebuild the internal matrix & update the yaw/pitch/roll angles within
441  * the ]-PI,PI] range (Must be called after using addComponents)
442  * \sa addComponents
443  */
444  void normalizeAngles();
445 
446  /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped
447  * to the ]-pi,pi] interval). */
448  void operator*=(const double s);
449 
450  /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles
451  * (radians) - This method recomputes the internal rotation matrix.
452  * \sa getYawPitchRoll, setYawPitchRoll
453  */
454  void setFromValues(
455  const double x0, const double y0, const double z0, const double yaw = 0,
456  const double pitch = 0, const double roll = 0);
457 
458  /** Set the pose from a 3D position (meters) and a quaternion, stored as [x
459  * y z qr qx qy qz] in a 7-element vector.
460  * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion,
461  * getAsQuaternion
462  */
463  template <typename VECTORLIKE>
464  inline void setFromXYZQ(const VECTORLIKE& v, const size_t index_offset = 0)
465  {
466  ASSERT_ABOVEEQ_(v.size(), 7 + index_offset);
467  // The 3x3 rotation part:
469  v[index_offset + 3], v[index_offset + 4], v[index_offset + 5],
470  v[index_offset + 6]);
471  q.rotationMatrixNoResize(m_ROT);
472  m_ypr_uptodate = false;
473  m_coords[0] = v[index_offset + 0];
474  m_coords[1] = v[index_offset + 1];
475  m_coords[2] = v[index_offset + 2];
476  }
477 
478  /** Set the 3 angles of the 3D pose (in radians) - This method recomputes
479  * the internal rotation coordinates matrix.
480  * \sa getYawPitchRoll, setFromValues
481  */
482  inline void setYawPitchRoll(
483  const double yaw_, const double pitch_, const double roll_)
484  {
485  setFromValues(x(), y(), z(), yaw_, pitch_, roll_);
486  }
487 
488  /** Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32
489  * r13 r23 r33 tx ty tz]
490  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
491  * the 3D translation of the pose
492  * \sa getAs12Vector
493  */
494  template <class ARRAYORVECTOR>
495  inline void setFrom12Vector(const ARRAYORVECTOR& vec12)
496  {
497  m_ROT(0, 0) = vec12[0];
498  m_ROT(0, 1) = vec12[3];
499  m_ROT(0, 2) = vec12[6];
500  m_ROT(1, 0) = vec12[1];
501  m_ROT(1, 1) = vec12[4];
502  m_ROT(1, 2) = vec12[7];
503  m_ROT(2, 0) = vec12[2];
504  m_ROT(2, 1) = vec12[5];
505  m_ROT(2, 2) = vec12[8];
506  m_ypr_uptodate = false;
507  m_coords[0] = vec12[9];
508  m_coords[1] = vec12[10];
509  m_coords[2] = vec12[11];
510  }
511 
512  /** Get the pose representation as an array with these 12 elements: [r11 r21
513  * r31 r12 r22 r32 r13 r23 r33 tx ty tz]
514  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
515  * the 3D translation of the pose
516  * \sa setFrom12Vector
517  */
518  template <class ARRAYORVECTOR>
519  inline void getAs12Vector(ARRAYORVECTOR& vec12) const
520  {
521  vec12[0] = m_ROT(0, 0);
522  vec12[3] = m_ROT(0, 1);
523  vec12[6] = m_ROT(0, 2);
524  vec12[1] = m_ROT(1, 0);
525  vec12[4] = m_ROT(1, 1);
526  vec12[7] = m_ROT(1, 2);
527  vec12[2] = m_ROT(2, 0);
528  vec12[5] = m_ROT(2, 1);
529  vec12[8] = m_ROT(2, 2);
530  vec12[9] = m_coords[0];
531  vec12[10] = m_coords[1];
532  vec12[11] = m_coords[2];
533  }
534 
535  /** Returns the three angles (yaw, pitch, roll), in radians, from the
536  * rotation matrix.
537  * \sa setFromValues, yaw, pitch, roll
538  */
539  void getYawPitchRoll(double& yaw, double& pitch, double& roll) const;
540 
541  /** Get the YAW angle (in radians) \sa setFromValues */
542  inline double yaw() const
543  {
545  return m_yaw;
546  }
547  /** Get the PITCH angle (in radians) \sa setFromValues */
548  inline double pitch() const
549  {
551  return m_pitch;
552  }
553  /** Get the ROLL angle (in radians) \sa setFromValues */
554  inline double roll() const
555  {
557  return m_roll;
558  }
559 
560  /** Returns a 6x1 vector with [x y z yaw pitch roll]' */
561  void asVector(vector_t& v) const;
562 
563  /** Returns the quaternion associated to the rotation of this object (NOTE:
564  * XYZ translation is ignored)
565  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2)
566  * \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin
567  * (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta
568  * /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +
569  * \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos
570  * (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi
571  * /2) \\ \end{array}\right) \f]
572  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw
573  * \f$.
574  * \param out_dq_dr If provided, the 4x3 Jacobian of the transformation
575  * will be computed and stored here. It's the Jacobian of the transformation
576  * from (yaw pitch roll) to (qr qx qy qz).
577  */
578  void getAsQuaternion(
580  mrpt::math::CMatrixFixed<double, 4, 3>* out_dq_dr = nullptr) const;
581 
582  inline const double& operator[](unsigned int i) const
583  {
585  switch (i)
586  {
587  case 0:
588  return m_coords[0];
589  case 1:
590  return m_coords[1];
591  case 2:
592  return m_coords[2];
593  case 3:
594  return m_yaw;
595  case 4:
596  return m_pitch;
597  case 5:
598  return m_roll;
599  default:
600  throw std::runtime_error(
601  "CPose3D::operator[]: Index of bounds.");
602  }
603  }
604  // CPose3D CANNOT have a write [] operator, since it'd leave the object in
605  // an inconsistent state (outdated rotation matrix).
606  // Use setFromValues() instead.
607  // inline double &operator[](unsigned int i)
608 
609  /** Returns a human-readable textual representation of the object (eg: "[x y
610  * z yaw pitch roll]", angles in degrees.)
611  * \sa fromString
612  */
613  void asString(std::string& s) const
614  {
615  using mrpt::RAD2DEG;
617  s = mrpt::format(
618  "[%f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2],
620  }
621  inline std::string asString() const
622  {
623  std::string s;
624  asString(s);
625  return s;
626  }
627 
628  /** Set the current object value from a string generated by 'asString' (eg:
629  * "[x y z yaw pitch roll]", angles in deg. )
630  * \sa asString
631  * \exception std::exception On invalid format
632  */
633  void fromString(const std::string& s);
634 
635  /** Same as fromString, but without requiring the square brackets in the
636  * string */
637  void fromStringRaw(const std::string& s);
638 
639  /** Return true if the 6D pose represents a Z axis almost exactly vertical
640  * (upwards or downwards), with a given tolerance (if set to 0 exact
641  * horizontality is tested). */
642  bool isHorizontal(const double tolerance = 0) const;
643 
644  /** The euclidean distance between two poses taken as two 6-length vectors
645  * (angles in radians). */
646  double distanceEuclidean6D(const CPose3D& o) const;
647 
648  /** @} */ // modif. components
649 
650  void setToNaN() override;
651 
652  /** Used to emulate CPosePDF types, for example, in
653  * mrpt::graphs::CNetworkOfPoses */
655  enum
656  {
658  };
659  static constexpr bool is_3D() { return is_3D_val != 0; }
660  enum
661  {
663  };
664  enum
665  {
667  };
668  static constexpr bool is_PDF() { return is_PDF_val != 0; }
669  inline const type_value& getPoseMean() const { return *this; }
670  inline type_value& getPoseMean() { return *this; }
671  /** @name STL-like methods and typedefs
672  @{ */
673  /** The type of the elements */
674  using value_type = double;
675  using reference = double&;
676  using const_reference = const double&;
677  using size_type = std::size_t;
679 
680  // size is constant
681  enum
682  {
684  };
685  static constexpr size_type size() { return static_size; }
686  static constexpr bool empty() { return false; }
687  static constexpr size_type max_size() { return static_size; }
688  static inline void resize(const size_t n)
689  {
690  if (n != static_size)
691  throw std::logic_error(format(
692  "Try to change the size of CPose3D to %u.",
693  static_cast<unsigned>(n)));
694  }
695  /** @} */
696 
697 }; // End of class def.
698 
699 std::ostream& operator<<(std::ostream& o, const CPose3D& p);
700 
701 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
702  * than a pose with negative x y z yaw pitch roll) */
703 CPose3D operator-(const CPose3D& p);
704 
705 bool operator==(const CPose3D& p1, const CPose3D& p2);
706 bool operator!=(const CPose3D& p1, const CPose3D& p2);
707 
708 } // namespace mrpt::poses
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:764
#define local
Definition: zutil.h:47
static CPose3D Identity()
Returns the identity transformation.
Definition: CPose3D.h:138
static constexpr bool is_PDF()
Definition: CPose3D.h:668
double x
X,Y coordinates.
Definition: TPoint2D.h:23
void inverseComposeFrom(const CPose3D &A, const CPose3D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose3D.cpp:618
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose3D.cpp:327
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CPose3D & operator+=(const CPose3D &b)
Make (b can be "this" without problems)
Definition: CPose3D.h:393
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
double RAD2DEG(const double x)
Radians to degrees.
const type_value & getPoseMean() const
Definition: CPose3D.h:669
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:367
mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D &l) const
Definition: CPose3D.h:301
void setFromXYZQ(const VECTORLIKE &v, const size_t index_offset=0)
Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-eleme...
Definition: CPose3D.h:464
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:96
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It&#39;s not safe to set t...
Definition: CPose3D.h:101
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose3D.cpp:754
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards)...
Definition: CPose3D.cpp:605
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CVectorFixedDouble< 3 > &xyz)
Definition: CPose3D.h:163
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
Definition: CPose3D.h:105
static constexpr bool is_3D()
Definition: CPose3D.h:659
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
static constexpr size_type size()
Definition: CPose3D.h:685
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
GLenum GLsizei n
Definition: glext.h:5136
void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_)
Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinat...
Definition: CPose3D.h:482
mrpt::math::CVectorFixedDouble< DIM > vector_t
Fixed-size vector of the correct size to hold all the coordinates of the point/pose.
Definition: CPoseOrPoint.h:137
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
Definition: CPose3D.h:248
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
Definition: CPose3D.cpp:271
CPose3D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
Definition: CPose3D.h:195
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLdouble s
Definition: glext.h:3682
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
Definition: CPose3D.h:115
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:590
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:236
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
Definition: CPose3D.cpp:450
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
Definition: CPose3D.cpp:351
static constexpr size_type max_size()
Definition: CPose3D.h:687
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
type_value & getPoseMean()
Definition: CPose3D.h:670
static constexpr bool empty()
Definition: CPose3D.h:686
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:646
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:562
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
const double & const_reference
Definition: CPose3D.h:676
const GLubyte * c
Definition: glext.h:6406
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
std::size_t size_type
Definition: CPose3D.h:677
CPose3D(const mrpt::math::CVectorFixedDouble< 12 > &vec12)
Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] wher...
Definition: CPose3D.h:206
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:356
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Definition: CPose3D.cpp:336
void getAsQuaternion(mrpt::math::CQuaternionDouble &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) ...
Definition: CPose3D.cpp:507
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
Definition: CPose3D.cpp:302
static void resize(const size_t n)
Definition: CPose3D.h:688
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
Definition: CPose3D.h:419
GLubyte g
Definition: glext.h:6372
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:24
GLubyte GLubyte b
Definition: glext.h:6372
const double eps
void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint3D &global_point) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.h:292
void inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const
Definition: CPose3D.h:360
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
void asVector(vector_t &v) const
Returns a 6x1 vector with [x y z yaw pitch roll]&#39;.
Definition: CPose3D.cpp:486
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
GLsizei const GLchar ** string
Definition: glext.h:4116
mrpt::math::TPoint3D inverseComposePoint(const mrpt::math::TPoint3D &g) const
Definition: CPose3D.h:366
double m_yaw
These variables are updated every time that the object rotation matrix is modified (construction...
Definition: CPose3D.h:109
A class used to store a 2D point.
Definition: CPoint2D.h:32
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:554
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose3D.cpp:781
_W64 int ptrdiff_t
Definition: glew.h:136
void setFrom12Vector(const ARRAYORVECTOR &vec12)
Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r...
Definition: CPose3D.h:495
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:128
void composePoint(double lx, double ly, double lz, float &gx, float &gy, float &gz) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.h:323
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.h:613
void operator*=(const double s)
Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).
Definition: CPose3D.cpp:279
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
Definition: CPose3D.cpp:294
mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const
Inverse of rotateVector(), i.e.
Definition: CPose3D.cpp:460
const GLdouble * v
Definition: glext.h:3684
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
overload for 2D points
Definition: CPose3D.h:376
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.cpp:769
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
double value_type
The type of the elements.
Definition: CPose3D.h:674
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
CPose3D()
Default constructor, with all the coordinates set to zero.
Definition: CPose3D.cpp:52
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:255
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
CPose3D(const MATRIX33 &rot, const VECTOR3 &xyz)
Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array...
Definition: CPose3D.h:152
CVectorFixed< double, N > CVectorFixedDouble
Specialization of CVectorFixed for double numbers.
Definition: CVectorFixed.h:32
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
GLenum GLint GLint y
Definition: glext.h:3542
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:251
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
Definition: CPose3D.h:229
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
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:224
Lightweight 3D point.
Definition: TPoint3D.h:90
Lightweight 2D point.
Definition: TPoint2D.h:31
void getAs12Vector(ARRAYORVECTOR &vec12) const
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 ...
Definition: CPose3D.h:519
GLfloat GLfloat p
Definition: glext.h:6398
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:786
CPose3D operator-(const CPose3D &b) const
Compute .
Definition: CPose3D.h:407
void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint2D &global_point) const
This version of the method assumes that the resulting point has no Z component (use with caution!) ...
Definition: CPose3D.h:311
double & reference
Definition: CPose3D.h:675
std::string asString() const
Definition: CPose3D.h:621
std::ptrdiff_t difference_type
Definition: CPose3D.h:678
const double & operator[](unsigned int i) const
Definition: CPose3D.h:582



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019