Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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>
15 
16 // Add for declaration of mexplus::from template specialization
18 
19 namespace mrpt
20 {
21 namespace poses
22 {
23  class CPose3DQuat;
24  class CPose3DRotVec;
25 
26  DEFINE_SERIALIZABLE_PRE( CPose3D )
27 
28  /** A class used to store a 3D pose (a 3D translation + a rotation in 3D).
29  * The 6D transformation in SE(3) stored in this class is kept in two
30  * separate containers: a 3-array for the translation, and a 3x3 rotation matrix.
31  *
32  * This class allows parameterizing 6D poses as a 6-vector: [x y z yaw pitch roll] (read below
33  * for the angles convention). Note however,
34  * that the yaw/pitch/roll angles are only computed (on-demand and transparently)
35  * when the user requests them. Normally, rotations and transformations are always handled
36  * via the 3x3 rotation matrix.
37  *
38  * Yaw/Pitch/Roll angles are defined as successive rotations around *local* (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 involving
45  * the names "yaw, pitch, roll" but regarding rotations around *global* (static) axes.
46  * Fortunately, it's very easy to see (by writing down the product of the three
47  * rotation matrices) that both conventions lead to exactly the same numbers.
48  * Only, that it's conventional to write the numbers in reverse order.
49  * That is, the same rotation can be described equivalently with any of these two
50  * parameterizations:
51  *
52  * - In local axes Z/Y/X convention: [yaw pitch roll] (This is the convention used in mrpt::poses::CPose3D)
53  * - In global axes X/Y/Z convention: [roll pitch yaw] (One of the Euler angles conventions)
54  *
55  * For further descriptions of point & pose classes, see mrpt::poses::CPoseOrPoint or refer
56  * to the [2D/3D Geometry tutorial](http://www.mrpt.org/2D_3D_Geometry) online.
57  *
58  * To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal
59  * 3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
60  *
61  * Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
62  *
63  * This class and CPose3DQuat are very similar, and they can be converted to the each other automatically via transformation constructors.
64  *
65  * There are Lie algebra methods: \a exp and \a ln (see the methods for documentation).
66  *
67  * \note Read also: "A tutorial on SE(3) transformation parameterizations and on-manifold optimization", (Technical report), 2010-2014. [PDF](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
68  *
69  * \ingroup poses_grp
70  * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
71  */
72  class BASE_IMPEXP CPose3D : public CPose<CPose3D>, public mrpt::utils::CSerializable
73  {
74  // This must be added to any CSerializable derived class:
76 
77  // This must be added for declaration of MEX-related functions
79 
80  public:
81  mrpt::math::CArrayDouble<3> m_coords; //!< The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
82  protected:
83  mrpt::math::CMatrixDouble33 m_ROT; //!< The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It's not safe to set this field as public)
84 
85  mutable bool m_ypr_uptodate; //!< Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
86  mutable double m_yaw, m_pitch, m_roll; //!< These variables are updated every time that the object rotation matrix is modified (construction, loading from values, pose composition, etc )
87 
88  /** Rebuild the homog matrix from the angles. */
89  void rebuildRotationMatrix();
90 
91  /** Updates Yaw/pitch/roll members from the m_ROT */
92  inline void updateYawPitchRoll() const { if (!m_ypr_uptodate) { m_ypr_uptodate=true; getYawPitchRoll( m_yaw, m_pitch, m_roll ); } }
93 
94  public:
95  /** @name Constructors
96  @{ */
97 
98  /** Default constructor, with all the coordinates set to zero. */
99  CPose3D();
100 
101  /** Constructor with initilization of the pose; (remember that angles are always given in radians!) */
102  CPose3D(const double x,const double y,const double z,const double yaw=0, const double pitch=0, const double roll=0);
103 
104  /** Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed). */
105  explicit CPose3D(const math::CMatrixDouble &m);
106 
107  /** Constructor from a 4x4 homogeneous matrix: */
108  explicit CPose3D(const math::CMatrixDouble44 &m);
109 
110  /** Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a mrpt::math::TPoint3D */
111  template <class MATRIX33,class VECTOR3>
112  inline CPose3D(const MATRIX33 &rot, const VECTOR3& xyz) : m_ROT(mrpt::math::UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
113  {
115  for (int r=0;r<3;r++)
116  for (int c=0;c<3;c++)
117  m_ROT(r,c)=rot.get_unsafe(r,c);
118  for (int r=0;r<3;r++) m_coords[r]=xyz[r];
119  }
120  //! \overload
121  inline CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CArrayDouble<3>& xyz) : m_coords(xyz),m_ROT(rot), m_ypr_uptodate(false)
122  { }
123 
124  /** Constructor from a CPose2D object.
125  */
126  explicit CPose3D(const CPose2D &);
127 
128  /** Constructor from a CPoint3D object.
129  */
130  explicit CPose3D(const CPoint3D &);
131 
132  /** Constructor from lightweight object.
133  */
134  explicit CPose3D(const mrpt::math::TPose3D &);
135 
136  /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
137  CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );
138 
139  /** Constructor from a CPose3DQuat. */
140  explicit CPose3D(const CPose3DQuat &);
141 
142  /** Constructor from a CPose3DRotVec. */
143  explicit CPose3D(const CPose3DRotVec &p );
144 
145  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
146  inline CPose3D(TConstructorFlags_Poses ) : m_ROT(mrpt::math::UNINITIALIZED_MATRIX), m_ypr_uptodate(false) { }
147 
148  /** Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
149  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
150  * \sa setFrom12Vector, getAs12Vector
151  */
152  inline explicit CPose3D(const mrpt::math::CArrayDouble<12> &vec12) : m_ROT( mrpt::math::UNINITIALIZED_MATRIX ), m_ypr_uptodate(false) {
153  setFrom12Vector(vec12);
154  }
155 
156  /** @} */ // end Constructors
157 
158 
159 
160  /** @name Access 3x3 rotation and 4x4 homogeneous matrices
161  @{ */
162 
163  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
164  * \sa getInverseHomogeneousMatrix, getRotationMatrix
165  */
167  {
168  out_HM.insertMatrix(0,0,m_ROT);
169  for (int i=0;i<3;i++) out_HM(i,3)=m_coords[i];
170  out_HM(3,0)=out_HM(3,1)=out_HM(3,2)=0.; out_HM(3,3)=1.;
171  }
172 
173  inline mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const { mrpt::math::CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}
174 
175  /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
176  inline void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const { ROT = m_ROT; }
177  //! \overload
178  inline const mrpt::math::CMatrixDouble33 & getRotationMatrix() const { return m_ROT; }
179 
180  /** Sets the 3x3 rotation matrix \sa getRotationMatrix, getHomogeneousMatrix */
181  inline void setRotationMatrix( const mrpt::math::CMatrixDouble33 & ROT ) { m_ROT = ROT; m_ypr_uptodate = false; }
182 
183  /** @} */ // end rot and HM
184 
185 
186  /** @name Pose-pose and pose-point compositions and operators
187  @{ */
188 
189  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
190  inline CPose3D operator + (const CPose3D& b) const
191  {
193  ret.composeFrom(*this,b);
194  return ret;
195  }
196 
197  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
198  CPoint3D operator + (const CPoint3D& b) const;
199 
200  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
201  CPoint3D operator + (const CPoint2D& b) const;
202 
203  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. For the coordinate system see the top of this page. */
204  void sphericalCoordinates(
205  const mrpt::math::TPoint3D &point,
206  double &out_range,
207  double &out_yaw,
208  double &out_pitch ) const;
209 
210  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
211  * If pointers are provided, the corresponding Jacobians are returned.
212  * "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
213  * See [this report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf) for mathematical details.
214  * \param If set to true, the Jacobian "out_jacobian_df_dpose" uses a fastest linearized appoximation (valid only for small rotations!).
215  */
216  void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
217  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
218  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL,
219  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3=NULL,
220  bool use_small_rot_approx = false) const;
221 
222  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
223  * \note local_point is passed by value to allow global and local point to be the same variable
224  */
225  inline void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint3D &global_point) const {
226  composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,global_point.z );
227  }
228  /** This version of the method assumes that the resulting point has no Z component (use with caution!) */
229  inline void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint2D &global_point) const {
230  double dummy_z;
231  composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,dummy_z );
232  }
233 
234  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose. */
235  inline void composePoint(double lx,double ly,double lz, float &gx, float &gy, float &gz ) const {
236  double ggx, ggy,ggz;
237  composePoint(lx,ly,lz,ggx,ggy,ggz);
238  gx = static_cast<float>(ggx); gy = static_cast<float>(ggy); gz = static_cast<float>(ggz);
239  }
240 
241  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
242  * If pointers are provided, the corresponding Jacobians are returned.
243  * "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
244  * See [this report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf) for mathematical details.
245  * \sa composePoint, composeFrom
246  */
247  void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
248  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
249  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL,
250  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3=NULL ) const;
251 
252  /** \overload */
254  inverseComposePoint(g.x,g.y,g.z, l.x,l.y,l.z);
255  }
256 
257  /** overload for 2D points \exception If the z component of the result is greater than some epsilon */
258  inline void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const {
259  double lz;
260  inverseComposePoint(g.x,g.y,0, l.x,l.y,lz);
261  ASSERT_BELOW_(std::abs(lz),eps)
262  }
263 
264  /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
265  * \note A or B can be "this" without problems.
266  */
267  void composeFrom(const CPose3D& A, const CPose3D& B );
268 
269  /** Make \f$ this = this \oplus b \f$ (\a b can be "this" without problems) */
270  inline CPose3D& operator += (const CPose3D& b)
271  {
272  composeFrom(*this,b);
273  return *this;
274  }
275 
276  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
277  * \note A or B can be "this" without problems.
278  * \sa composeFrom, composePoint
279  */
280  void inverseComposeFrom(const CPose3D& A, const CPose3D& B );
281 
282  /** Compute \f$ RET = this \oplus b \f$ */
283  inline CPose3D operator - (const CPose3D& b) const
284  {
286  ret.inverseComposeFrom(*this,b);
287  return ret;
288  }
289 
290  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
291  void inverse();
292 
293  /** makes: this = p (+) this */
294  inline void changeCoordinatesReference( const CPose3D & p ) { composeFrom(p,CPose3D(*this)); }
295 
296  /** @} */ // compositions
297 
298 
299  /** @name Access and modify contents
300  @{ */
301 
302  /** Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators.
303  * \sa normalizeAngles
304  */
305  void addComponents(const CPose3D &p);
306 
307  /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)
308  * \sa addComponents
309  */
310  void normalizeAngles();
311 
312  /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). */
313  void operator *=(const double s);
314 
315  /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
316  * \sa getYawPitchRoll, setYawPitchRoll
317  */
318  void setFromValues(
319  const double x0,
320  const double y0,
321  const double z0,
322  const double yaw=0,
323  const double pitch=0,
324  const double roll=0);
325 
326  /** Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector.
327  * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion, getAsQuaternion
328  */
329  template <typename VECTORLIKE>
330  inline void setFromXYZQ(
331  const VECTORLIKE &v,
332  const size_t index_offset = 0)
333  {
334  ASSERT_ABOVEEQ_(v.size(), 7+index_offset)
335  // The 3x3 rotation part:
336  mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q( v[index_offset+3],v[index_offset+4],v[index_offset+5],v[index_offset+6] );
337  q.rotationMatrixNoResize(m_ROT);
338  m_ypr_uptodate=false;
339  m_coords[0] = v[index_offset+0];
340  m_coords[1] = v[index_offset+1];
341  m_coords[2] = v[index_offset+2];
342  }
343 
344  /** Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.
345  * \sa getYawPitchRoll, setFromValues
346  */
347  inline void setYawPitchRoll(
348  const double yaw_,
349  const double pitch_,
350  const double roll_)
351  {
352  setFromValues(x(),y(),z(),yaw_,pitch_,roll_);
353  }
354 
355  /** Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
356  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
357  * \sa getAs12Vector
358  */
359  template <class ARRAYORVECTOR>
360  inline void setFrom12Vector(const ARRAYORVECTOR &vec12)
361  {
362  m_ROT.set_unsafe(0,0, vec12[0]); m_ROT.set_unsafe(0,1, vec12[3]); m_ROT.set_unsafe(0,2, vec12[6]);
363  m_ROT.set_unsafe(1,0, vec12[1]); m_ROT.set_unsafe(1,1, vec12[4]); m_ROT.set_unsafe(1,2, vec12[7]);
364  m_ROT.set_unsafe(2,0, vec12[2]); m_ROT.set_unsafe(2,1, vec12[5]); m_ROT.set_unsafe(2,2, vec12[8]);
365  m_ypr_uptodate = false;
366  m_coords[0] = vec12[ 9];
367  m_coords[1] = vec12[10];
368  m_coords[2] = vec12[11];
369  }
370 
371  /** Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
372  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
373  * \sa setFrom12Vector
374  */
375  template <class ARRAYORVECTOR>
376  inline void getAs12Vector(ARRAYORVECTOR &vec12) const
377  {
378  vec12[0] = m_ROT.get_unsafe(0,0); vec12[3] = m_ROT.get_unsafe(0,1); vec12[6] = m_ROT.get_unsafe(0,2);
379  vec12[1] = m_ROT.get_unsafe(1,0); vec12[4] = m_ROT.get_unsafe(1,1); vec12[7] = m_ROT.get_unsafe(1,2);
380  vec12[2] = m_ROT.get_unsafe(2,0); vec12[5] = m_ROT.get_unsafe(2,1); vec12[8] = m_ROT.get_unsafe(2,2);
381  vec12[ 9] = m_coords[0];
382  vec12[10] = m_coords[1];
383  vec12[11] = m_coords[2];
384  }
385 
386  /** Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
387  * \sa setFromValues, yaw, pitch, roll
388  */
389  void getYawPitchRoll( double &yaw, double &pitch, double &roll ) const;
390 
391  inline double yaw() const { updateYawPitchRoll(); return m_yaw; } //!< Get the YAW angle (in radians) \sa setFromValues
392  inline double pitch() const { updateYawPitchRoll(); return m_pitch; } //!< Get the PITCH angle (in radians) \sa setFromValues
393  inline double roll() const { updateYawPitchRoll(); return m_roll; } //!< Get the ROLL angle (in radians) \sa setFromValues
394 
395  /** Returns a 1x6 vector with [x y z yaw pitch roll] */
396  void getAsVector(mrpt::math::CVectorDouble &v) const;
397  /// \overload
398  void getAsVector(mrpt::math::CArrayDouble<6> &v) const;
399 
400  /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
401  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right) \f]
402  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
403  * \param out_dq_dr If provided, the 4x3 Jacobian of the transformation will be computed and stored here. It's the Jacobian of the transformation from (yaw pitch roll) to (qr qx qy qz).
404  */
405  void getAsQuaternion(
408  ) const;
409 
410  inline const double &operator[](unsigned int i) const
411  {
412  updateYawPitchRoll();
413  switch(i)
414  {
415  case 0:return m_coords[0];
416  case 1:return m_coords[1];
417  case 2:return m_coords[2];
418  case 3:return m_yaw;
419  case 4:return m_pitch;
420  case 5:return m_roll;
421  default:
422  throw std::runtime_error("CPose3D::operator[]: Index of bounds.");
423  }
424  }
425  // CPose3D CANNOT have a write [] operator, since it'd leave the object in an inconsistent state (outdated rotation matrix).
426  // Use setFromValues() instead.
427  // inline double &operator[](unsigned int i)
428 
429  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
430  * \sa fromString
431  */
432  void asString(std::string &s) const { using mrpt::utils::RAD2DEG; updateYawPitchRoll(); s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],RAD2DEG(m_yaw),RAD2DEG(m_pitch),RAD2DEG(m_roll)); }
433  inline std::string asString() const { std::string s; asString(s); return s; }
434 
435  /** Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. )
436  * \sa asString
437  * \exception std::exception On invalid format
438  */
439  void fromString(const std::string &s) {
440  using mrpt::utils::DEG2RAD;
442  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
443  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
444  this->setFromValues(m.get_unsafe(0,0),m.get_unsafe(0,1),m.get_unsafe(0,2),DEG2RAD(m.get_unsafe(0,3)),DEG2RAD(m.get_unsafe(0,4)),DEG2RAD(m.get_unsafe(0,5)));
445  }
446 
447  /** Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested). */
448  bool isHorizontal( const double tolerance=0) const;
449 
450  /** The euclidean distance between two poses taken as two 6-length vectors (angles in radians). */
451  double distanceEuclidean6D( const CPose3D &o ) const;
452 
453  /** @} */ // modif. components
454 
455 
456 
457  /** @name Lie Algebra methods
458  @{ */
459 
460  /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
461  * \param pseudo_exponential If set to true, XYZ are copied from the first three elements in the vector instead of using the proper Lie Algebra formulas (this is actually the common practice in robotics literature). */
462  static CPose3D exp(const mrpt::math::CArrayNumeric<double,6> & vect, bool pseudo_exponential = false);
463 
464  /** \overload */
465  static void exp(const mrpt::math::CArrayNumeric<double,6> & vect, CPose3D &out_pose, bool pseudo_exponential = false);
466 
467  /** Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix). */
468  static mrpt::math::CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric<double,3> & vect);
469 
470 
471  /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra.
472  * \sa ln_jacob
473  */
474  void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
475 
476  /// \overload
477  inline mrpt::math::CArrayDouble<6> ln() const { mrpt::math::CArrayDouble<6> ret; ln(ret); return ret; }
478 
479  /** Jacobian of the logarithm of the 3x4 matrix defined by this pose.
480  * \sa ln
481  */
482  void ln_jacob(mrpt::math::CMatrixFixedNumeric<double,6,12> &J) const;
483 
484  /** Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rotation matrix R.
485  * \sa ln, ln_jacob
486  */
488 
489  /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra */
490  mrpt::math::CArrayDouble<3> ln_rotation() const;
491 
492  /** The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
493  * \note Eq. 10.3.5 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf */
494  static void jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
495 
496  /** The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.
497  * \note Eq. 10.3.7 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf */
498  static void jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
499 
500  /** @} */
501 
502  void setToNaN() MRPT_OVERRIDE;
503 
504  typedef CPose3D type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
505  enum { is_3D_val = 1 };
506  static inline bool is_3D() { return is_3D_val!=0; }
507  enum { rotation_dimensions = 3 };
508  enum { is_PDF_val = 0 };
509  static inline bool is_PDF() { return is_PDF_val!=0; }
510 
511  inline const type_value & getPoseMean() const { return *this; }
512  inline type_value & getPoseMean() { return *this; }
513 
514  /** @name STL-like methods and typedefs
515  @{ */
516  typedef double value_type; //!< The type of the elements
517  typedef double& reference;
518  typedef const double& const_reference;
519  typedef std::size_t size_type;
521 
522 
523  // size is constant
524  enum { static_size = 6 };
525  static inline size_type size() { return static_size; }
526  static inline bool empty() { return false; }
527  static inline size_type max_size() { return static_size; }
528  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3D to %u.",static_cast<unsigned>(n))); }
529  /** @} */
530 
531  }; // End of class def.
532  DEFINE_SERIALIZABLE_POST( CPose3D )
533 
534 
535  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3D& p);
536 
537  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
538  CPose3D BASE_IMPEXP operator -(const CPose3D &p);
539 
540  bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2);
541  bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2);
542 
543 
544  } // End of namespace
545 } // End of namespace
546 
547 #endif
#define ASSERT_EQUAL_(__A, __B)
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:645
double y
X,Y coordinates.
GLdouble GLdouble z
Definition: glext.h:3734
std::size_t size_type
Definition: CPose3D.h:519
double DEG2RAD(const double x)
Degrees to radians.
const type_value & getPoseMean() const
Definition: CPose3D.h:511
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:330
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It&#39;s not safe to set t...
Definition: CPose3D.h:83
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Definition: CPose2D.cpp:359
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
Definition: CPose3D.h:85
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
#define ASSERT_BELOW_(__A, __B)
GLenum GLsizei n
Definition: glext.h:4618
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:347
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:35
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
CPose3D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
Definition: CPose3D.h:146
double z
X,Y,Z coordinates.
GLdouble s
Definition: glext.h:3602
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
Definition: CPose3D.h:92
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:181
static bool is_PDF()
Definition: CPose3D.h:509
std::ptrdiff_t difference_type
Definition: CPose3D.h:520
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CArrayDouble< 3 > &xyz)
Definition: CPose3D.h:121
const double & const_reference
Definition: CPose3D.h:518
double value_type
The type of the elements.
Definition: CPose3D.h:516
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:152
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:439
type_value & getPoseMean()
Definition: CPose3D.h:512
double RAD2DEG(const double x)
Radians to degrees.
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:81
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:595
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
const GLubyte * c
Definition: glext.h:5590
#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:138
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
CPose2D BASE_IMPEXP 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:307
static void resize(const size_t n)
Definition: CPose3D.h:528
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
Definition: CPose3D.h:294
GLubyte g
Definition: glext.h:5575
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:25
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLubyte GLubyte b
Definition: glext.h:5575
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:225
void inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const
Definition: CPose3D.h:253
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
_W64 int ptrdiff_t
Definition: glew.h:133
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:360
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:235
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:432
#define RAD2DEG
const GLdouble * v
Definition: glext.h:3603
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
overload for 2D points
Definition: CPose3D.h:258
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...
#define ASSERT_ABOVEEQ_(__A, __B)
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
size_t size(const MATRIXLIKE &m, const int dim)
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:112
static bool is_3D()
Definition: CPose3D.h:506
GLenum GLint GLint y
Definition: glext.h:3516
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
Definition: CPose3D.h:178
#define DEFINE_SERIALIZABLE_POST(class_name)
double & reference
Definition: CPose3D.h:517
mrpt::math::CArrayDouble< 6 > ln() const
Definition: CPose3D.h:477
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:41
GLenum GLint x
Definition: glext.h:3516
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
Lightweight 3D point.
Lightweight 2D point.
static bool empty()
Definition: CPose3D.h:526
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:376
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
static size_type max_size()
Definition: CPose3D.h:527
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
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:166
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:229
std::string asString() const
Definition: CPose3D.h:433
const double & operator[](unsigned int i) const
Definition: CPose3D.h:410
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:106
static size_type size()
Definition: CPose3D.h:525



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019