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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0a2592f5f Fri May 24 16:11:07 2019 +0200 at vie may 24 16:20:17 CEST 2019