Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
15
16// Add for declaration of mexplus::from template specialization
18
19namespace mrpt
20{
21namespace poses
22{
23 class CPose3DQuat;
24 class CPose3DRotVec;
25
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. */
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. */
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 CPose3D(const CPose2D &);
127
128 /** Constructor from a CPoint3D object.
129 */
131
132 /** Constructor from lightweight object.
133 */
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. */
141
142 /** Constructor from a CPose3DRotVec. */
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. */
199
200 /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
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. */
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 = ggx; gy = ggy; gz = 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 */
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 */
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] */
397 /// \overload
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 */
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) {
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 * \note Method from TooN (C) Tom Drummond (GNU GPL) */
463 static CPose3D exp(const mrpt::math::CArrayNumeric<double,6> & vect, bool pseudo_exponential = false);
464
465 /** \overload */
466 static void exp(const mrpt::math::CArrayNumeric<double,6> & vect, CPose3D &out_pose, bool pseudo_exponential = false);
467
468 /** Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
469 * \note Method from TooN (C) Tom Drummond (GNU GPL) */
471
472
473 /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra.
474 * \note Method from TooN (C) Tom Drummond (GNU GPL)
475 * \sa ln_jacob
476 */
477 void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
478
479 /// \overload
480 inline mrpt::math::CArrayDouble<6> ln() const { mrpt::math::CArrayDouble<6> ret; ln(ret); return ret; }
481
482 /** Jacobian of the logarithm of the 3x4 matrix defined by this pose.
483 * \note Method from TooN (C) Tom Drummond (GNU GPL)
484 * \sa ln
485 */
487
488 /** Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rotation matrix R.
489 * \sa ln, ln_jacob
490 */
492
493 /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra.
494 * \note Method from TooN (C) Tom Drummond (GNU GPL) */
496
497 /** The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
498 * \note Eq. 10.3.5 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf */
499 static void jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
500
501 /** The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.
502 * \note Eq. 10.3.7 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf */
503 static void jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
504
505 /** @} */
506
508
509 typedef CPose3D type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
510 enum { is_3D_val = 1 };
511 static inline bool is_3D() { return is_3D_val!=0; }
512 enum { rotation_dimensions = 3 };
513 enum { is_PDF_val = 0 };
514 static inline bool is_PDF() { return is_PDF_val!=0; }
515
516 inline const type_value & getPoseMean() const { return *this; }
517 inline type_value & getPoseMean() { return *this; }
518
519 /** @name STL-like methods and typedefs
520 @{ */
521 typedef double value_type; //!< The type of the elements
522 typedef double& reference;
523 typedef const double& const_reference;
524 typedef std::size_t size_type;
525 typedef std::ptrdiff_t difference_type;
526
527
528 // size is constant
529 enum { static_size = 6 };
530 static inline size_type size() { return static_size; }
531 static inline bool empty() { return false; }
532 static inline size_type max_size() { return static_size; }
533 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))); }
534 /** @} */
535
536 }; // End of class def.
537 DEFINE_SERIALIZABLE_POST( CPose3D )
538
539
540 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3D& p);
541
542 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
543 CPose3D BASE_IMPEXP operator -(const CPose3D &p);
544
545 bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2);
546 bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2);
547
548
549 } // End of namespace
550} // End of namespace
551
552#endif
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class.
#define DEFINE_SERIALIZABLE_POST(class_name)
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
#define RAD2DEG
Definition: bits.h:87
#define DEG2RAD
Definition: bits.h:86
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:26
A numeric matrix of compile-time fixed size.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:43
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
Definition: CQuaternion.h:288
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
A class used to store a 2D point.
Definition: CPoint2D.h:37
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
CPose3D(const CPose3DQuat &)
Constructor from a CPose3DQuat.
std::size_t size_type
Definition: CPose3D.h:524
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
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...
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
static bool empty()
Definition: CPose3D.h:531
CPose3D(const CPose2D &)
Constructor from a CPose2D object.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
CPose3D(const math::CMatrixDouble44 &m)
Constructor from a 4x4 homogeneous matrix:
CPose3D(const mrpt::math::TPose3D &)
Constructor from lightweight object.
void getAsVector(mrpt::math::CArrayDouble< 6 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
void ln(mrpt::math::CArrayDouble< 6 > &out_ln) const
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the...
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
std::string asString() const
Definition: CPose3D.h:433
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
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
mrpt::math::CArrayDouble< 6 > ln() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3D.h:480
static size_type size()
Definition: CPose3D.h:530
void inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3D.h:253
std::ptrdiff_t difference_type
Definition: CPose3D.h:525
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
CPose3D(const math::CMatrixDouble &m)
Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than...
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
Definition: CPose3D.h:258
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
CPose3D(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
Constructor with initilization of the pose; (remember that angles are always given in radians!...
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards),...
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
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
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
static void exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, CPose3D &out_pose, bool pseudo_exponential=false)
This is an overloaded member function, provided for convenience. It differs from the above function o...
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
static bool is_3D()
Definition: CPose3D.h:511
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).
const double & const_reference
Definition: CPose3D.h:523
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).
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.
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...
CPose3D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.
Definition: CPose3D.h:146
static void resize(const size_t n)
Definition: CPose3D.h:533
const type_value & getPoseMean() const
Definition: CPose3D.h:516
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
Definition: CPose3D.h:294
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...
CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z)
Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement.
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
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
Definition: CPose3D.h:85
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 ln_jacob(mrpt::math::CMatrixFixedNumeric< double, 6, 12 > &J) const
Jacobian of the logarithm of the 3x4 matrix defined by this pose.
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CArrayDouble< 3 > &xyz)
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3D.h:121
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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...
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...
double & reference
Definition: CPose3D.h:522
double value_type
The type of the elements.
Definition: CPose3D.h:521
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]",...
Definition: CPose3D.h:439
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
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.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:181
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x6 vector with [x y z yaw pitch roll].
const double & operator[](unsigned int i) const
Definition: CPose3D.h:410
CPose3D(const CPose3DRotVec &p)
Constructor from a CPose3DRotVec.
static bool is_PDF()
Definition: CPose3D.h:514
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT
Definition: CPose3D.h:92
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
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
type_value & getPoseMean()
Definition: CPose3D.h:517
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
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const
Computes the 3D point L such as .
CPose3D(const CPoint3D &)
Constructor from a CPoint3D object.
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.
CPose3D()
Default constructor, with all the coordinates set to zero.
static size_type max_size()
Definition: CPose3D.h:532
void inverse()
Convert this pose into its inverse, saving the result in itself.
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It's not safe to set t...
Definition: CPose3D.h:83
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3D.h:178
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
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:42
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:42
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:26
@ static_size
Definition: eigen_plugins.h:17
#define ASSERT_EQUAL_(__A, __B)
Definition: mrpt_macros.h:264
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
#define ASSERT_BELOW_(__A, __B)
Definition: mrpt_macros.h:266
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: mrpt_macros.h:269
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
class BASE_IMPEXP CPose3DRotVec
Definition: CPose3DRotVec.h:21
class BASE_IMPEXP CPose3DQuat
Definition: CPose3DQuat.h:23
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...
@ UNINITIALIZED_POSE
Definition: CPoseOrPoint.h:35
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.
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
double RAD2DEG(const double x)
Radians to degrees.
Definition: bits.h:75
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
Lightweight 2D point.
double y
X,Y coordinates.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Sat Jan 21 06:46:15 UTC 2023