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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019