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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019