Main MRPT website > C++ reference for MRPT 1.9.9
CPose3DRotVec.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 CPOSE3DROTVEC_H
10 #define CPOSE3DROTVEC_H
11 
12 #include <mrpt/poses/CPose.h>
14 #include <mrpt/math/CQuaternion.h>
15 #include <mrpt/poses/poses_frwds.h>
16 
17 namespace mrpt
18 {
19 namespace poses
20 {
21 /** A 3D pose, with a 3D translation and a rotation in 3D parameterized in
22  * rotation-vector form (equivalent to axis-angle).
23  * The 6D transformation in SE(3) stored in this class is kept in two
24  * separate containers: a 3-array for the rotation vector, and a 3-array for
25  * the translation.
26  *
27  * \code
28  * CPose3DRotVec pose;
29  * pose.m_rotvec[{0:2}]=... // Rotation vector
30  * pose.m_coords[{0:2}]=... // Translation
31  * \endcode
32  *
33  * For a complete descriptionan of Points/Poses, see mrpt::poses::CPoseOrPoint,
34  * or refer
35  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry
36  * tutorial</a> online.
37  *
38  * There are Lie algebra methods: \a exp and \a ln (see the methods for
39  * documentation).
40  *
41  * \ingroup poses_grp
42  * \sa CPose3DRotVec, CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
43  */
44 class CPose3DRotVec : public CPose<CPose3DRotVec>,
46 {
48 
49  public:
50  /** The translation vector [x,y,z] */
52  /** The rotation vector [vx,vy,vz] */
54 
55  /** @name Constructors
56  @{ */
57 
58  /** Default constructor, with all the coordinates set to zero. */
59  inline CPose3DRotVec()
60  {
61  m_coords[0] = m_coords[1] = m_coords[2] = 0;
62  m_rotvec[0] = m_rotvec[1] = m_rotvec[2] = 0;
63  }
64 
65  /** Fast constructor that leaves all the data uninitialized - call with
66  * UNINITIALIZED_POSE as argument */
67  inline CPose3DRotVec(TConstructorFlags_Poses constructor_dummy_param)
68  : m_coords(), m_rotvec()
69  {
70  MRPT_UNUSED_PARAM(constructor_dummy_param);
71  }
72 
73  /** Constructor with initilization of the pose */
74  inline CPose3DRotVec(
75  const double vx, const double vy, const double vz, const double x,
76  const double y, const double z)
77  {
78  m_coords[0] = x;
79  m_coords[1] = y;
80  m_coords[2] = z;
81  m_rotvec[0] = vx;
82  m_rotvec[1] = vy;
83  m_rotvec[2] = vz;
84  }
85 
86  /** Constructor with initilization of the pose from a vector [w1 w2 w3 x y
87  * z] */
89  {
90  m_rotvec[0] = v[0];
91  m_rotvec[1] = v[1];
92  m_rotvec[2] = v[2];
93  m_coords[0] = v[3];
94  m_coords[1] = v[4];
95  m_coords[2] = v[5];
96  }
97 
98  /** Copy constructor */
99  inline CPose3DRotVec(const CPose3DRotVec& o)
100  {
101  this->m_rotvec = o.m_rotvec;
102  this->m_coords = o.m_coords;
103  }
104 
105  /** Constructor from a 4x4 homogeneous matrix: */
106  explicit CPose3DRotVec(const math::CMatrixDouble44& m);
107 
108  /** Constructor from a CPose3D object.*/
109  explicit CPose3DRotVec(const CPose3D& m);
110 
111  /** Constructor from a quaternion (which only represents the 3D rotation
112  * part) and a 3D displacement. */
114  const mrpt::math::CQuaternionDouble& q, const double x, const double y,
115  const double z);
116 
117  /** Constructor from an array with these 6 elements: [w1 w2 w3 x y z]
118  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
119  * the 3D translation of the pose
120  * \sa setFrom12Vector, getAs12Vector
121  */
122  inline explicit CPose3DRotVec(const double* vec6)
123  {
124  m_coords[0] = vec6[3];
125  m_coords[1] = vec6[4];
126  m_coords[2] = vec6[5];
127  m_rotvec[0] = vec6[0];
128  m_rotvec[1] = vec6[1];
129  m_rotvec[2] = vec6[2];
130  }
131 
132  /** @} */ // end Constructors
133 
134  /** @name Access 3x3 rotation and 4x4 homogeneous matrices
135  @{ */
136 
137  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
138  * point(translation) or pose (translation+orientation).
139  * \sa getInverseHomogeneousMatrix, getRotationMatrix
140  */
142  {
143  out_HM.block<3, 3>(0, 0) = getRotationMatrix();
144  out_HM.set_unsafe(0, 3, m_coords[0]);
145  out_HM.set_unsafe(1, 3, m_coords[1]);
146  out_HM.set_unsafe(2, 3, m_coords[2]);
147  out_HM.set_unsafe(3, 0, 0);
148  out_HM.set_unsafe(3, 1, 0);
149  out_HM.set_unsafe(3, 2, 0);
150  out_HM.set_unsafe(3, 3, 1);
151  }
152 
153  /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
155  //! \overload
157  {
159  getRotationMatrix(ROT);
160  return ROT;
161  }
162 
163  /** @} */ // end rot and HM
164 
165  /** @name Pose-pose and pose-point compositions and operators
166  @{ */
167 
168  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
169  inline CPose3DRotVec operator+(const CPose3DRotVec& b) const
170  {
172  ret.composeFrom(*this, b);
173  return ret;
174  }
175 
176  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
177  CPoint3D operator+(const CPoint3D& b) const;
178 
179  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
180  CPoint3D operator+(const CPoint2D& b) const;
181 
184  {
186 
187  this->m_rotvec[0] = aux[0];
188  this->m_rotvec[1] = aux[1];
189  this->m_rotvec[2] = aux[2];
190 
191  this->m_coords[0] = m.get_unsafe(0, 3);
192  this->m_coords[1] = m.get_unsafe(1, 3);
193  this->m_coords[2] = m.get_unsafe(2, 3);
194  }
195 
196  void setFromXYZAndAngles(
197  const double x, const double y, const double z, const double yaw = 0,
198  const double pitch = 0, const double roll = 0);
199 
200  /** Computes the spherical coordinates of a 3D point as seen from the 6D
201  * pose specified by this object. For the coordinate system see
202  * mrpt::poses::CPose3D */
204  const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
205  double& out_pitch) const;
206 
207  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
208  * \f$ with G and L being 3D points and P this 6D pose.
209  * If pointers are provided, the corresponding Jacobians are returned.
210  * See <a
211  * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
212  * >this report</a> for mathematical details.
213  */
214  void composePoint(
215  double lx, double ly, double lz, double& gx, double& gy, double& gz,
216  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
217  nullptr,
218  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose =
219  nullptr) const;
220 
221  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
222  * \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
224  * be the same variable
225  */
226  inline void composePoint(
227  const mrpt::math::TPoint3D local_point,
228  mrpt::math::TPoint3D& global_point) const
229  {
230  composePoint(
231  local_point.x, local_point.y, local_point.z, global_point.x,
232  global_point.y, global_point.z);
233  }
234 
235  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
236  * If pointers are provided, the corresponding Jacobians are returned.
237  * See <a
238  * href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty"
239  * >this report</a> for mathematical details.
240  * \sa composePoint, composeFrom
241  */
242  void inverseComposePoint(
243  const double gx, const double gy, const double gz, double& lx,
244  double& ly, double& lz,
245  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
246  nullptr,
247  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose =
248  nullptr) const;
249 
250  /** Makes "this = A (+) B"; this method is slightly more efficient than
251  * "this= A + B;" since it avoids the temporary object.
252  * \note A or B can be "this" without problems.
253  */
254  void composeFrom(
255  const CPose3DRotVec& A, const CPose3DRotVec& B,
257  out_jacobian_drvtC_drvtA = nullptr,
259  out_jacobian_drvtC_drvtB = nullptr);
260 
261  /** Convert this RVT into a quaternion + XYZ
262  */
263  void toQuatXYZ(CPose3DQuat& q) const;
264 
265  /** Make \f$ this = this \oplus b \f$ (\a b can be "this" without problems)
266  */
268  {
269  composeFrom(*this, b);
270  return *this;
271  }
272 
273  /** Copy operator */
275  {
276  this->m_rotvec = o.m_rotvec;
277  this->m_coords = o.m_coords;
278  return *this;
279  }
280 
281  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
282  * than "this= A - B;" since it avoids the temporary object.
283  * \note A or B can be "this" without problems.
284  * \sa composeFrom, composePoint
285  */
286  void inverseComposeFrom(const CPose3DRotVec& A, const CPose3DRotVec& B);
287 
288  /** Compute \f$ RET = this \oplus b \f$ */
289  inline CPose3DRotVec operator-(const CPose3DRotVec& b) const
290  {
292  ret.inverseComposeFrom(*this, b);
293  return ret;
294  }
295 
296  /** Convert this pose into its inverse, saving the result in itself. \sa
297  * operator- */
298  void inverse();
299 
300  /** Compute the inverse of this pose and return the result. */
301  CPose3DRotVec getInverse() const;
302 
303  /** makes: this = p (+) this */
305  {
306  composeFrom(p, CPose3DRotVec(*this));
307  }
308 
309  /** @} */ // compositions
310 
311  /** @name Access and modify contents
312  @{ */
313 
314  inline double rx() const { return m_rotvec[0]; }
315  inline double ry() const { return m_rotvec[1]; }
316  inline double rz() const { return m_rotvec[2]; }
317  inline double& rx() { return m_rotvec[0]; }
318  inline double& ry() { return m_rotvec[1]; }
319  inline double& rz() { return m_rotvec[2]; }
320  /** Scalar sum of all 6 components: This is diferent from poses composition,
321  * which is implemented as "+" operators. */
322  inline void addComponents(const CPose3DRotVec& p)
323  {
324  m_coords += p.m_coords;
325  m_rotvec += p.m_rotvec;
326  }
327 
328  /** Scalar multiplication of x,y,z,vx,vy,vz. */
329  inline void operator*=(const double s)
330  {
331  m_coords *= s;
332  m_rotvec *= s;
333  }
334 
335  /** Create a vector with 3 components according to the input transformation
336  * matrix (only the rotation will be taken into account)
337  */
339  const math::CMatrixDouble44& m);
340 
341  /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles
342  * (radians) - This method recomputes the internal rotation matrix.
343  * \sa getYawPitchRoll, setYawPitchRoll
344  */
346  const double x0, const double y0, const double z0, const double vx,
347  const double vy, const double vz)
348  {
349  m_coords[0] = x0;
350  m_coords[1] = y0;
351  m_coords[2] = z0;
352  m_rotvec[0] = vx;
353  m_rotvec[1] = vy;
354  m_rotvec[2] = vz;
355  }
356 
357  /** Set pose from an array with these 6 elements: [x y z vx vy vz]
358  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the
359  * pose
360  * \sa getAs6Vector
361  */
362  template <class ARRAYORVECTOR>
363  inline void setFrom6Vector(const ARRAYORVECTOR& vec6)
364  {
365  m_rotvec[0] = vec6[3];
366  m_rotvec[1] = vec6[4];
367  m_rotvec[2] = vec6[5];
368  m_coords[0] = vec6[0];
369  m_coords[1] = vec6[1];
370  m_coords[2] = vec6[2];
371  }
372 
373  /** Gets pose as an array with these 6 elements: [x y z vx vy vz]
374  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the
375  * pose
376  * The target vector MUST ALREADY have space for 6 elements (i.e. no
377  * .resize() method will be called).
378  * \sa setAs6Vector, getAsVector
379  */
380  template <class ARRAYORVECTOR>
381  inline void getAs6Vector(ARRAYORVECTOR& vec6) const
382  {
383  vec6[0] = m_coords[0];
384  vec6[1] = m_coords[1];
385  vec6[2] = m_coords[2];
386  vec6[3] = m_rotvec[0];
387  vec6[4] = m_rotvec[1];
388  vec6[5] = m_rotvec[2];
389  }
390 
391  /** Like getAs6Vector() but for dynamic size vectors (required by base class
392  * CPoseOrPoint) */
393  template <class ARRAYORVECTOR>
394  inline void getAsVector(ARRAYORVECTOR& v) const
395  {
396  v.resize(6);
397  getAs6Vector(v);
398  }
399 
400  inline const double& operator[](unsigned int i) const
401  {
402  switch (i)
403  {
404  case 0:
405  return m_coords[0];
406  case 1:
407  return m_coords[1];
408  case 2:
409  return m_coords[2];
410  case 3:
411  return m_rotvec[0];
412  case 4:
413  return m_rotvec[1];
414  case 5:
415  return m_rotvec[2];
416  default:
417  throw std::runtime_error(
418  "CPose3DRotVec::operator[]: Index of bounds.");
419  }
420  }
421  inline double& operator[](unsigned int i)
422  {
423  switch (i)
424  {
425  case 0:
426  return m_coords[0];
427  case 1:
428  return m_coords[1];
429  case 2:
430  return m_coords[2];
431  case 3:
432  return m_rotvec[0];
433  case 4:
434  return m_rotvec[1];
435  case 5:
436  return m_rotvec[2];
437  default:
438  throw std::runtime_error(
439  "CPose3DRotVec::operator[]: Index of bounds.");
440  }
441  }
442 
443  /** Returns a human-readable textual representation of the object: "[x y z
444  * rx ry rz]"
445  * \sa fromString
446  */
447  void asString(std::string& s) const
448  {
449  s = mrpt::format(
450  "[%f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2],
451  m_rotvec[0], m_rotvec[1], m_rotvec[2]);
452  }
453  inline std::string asString() const
454  {
455  std::string s;
456  asString(s);
457  return s;
458  }
459 
460  /** Set the current object value from a string generated by 'asString' (eg:
461  * "[x y z yaw pitch roll]", angles in deg. )
462  * \sa asString
463  * \exception std::exception On invalid format
464  */
465  void fromString(const std::string& s)
466  {
468  if (!m.fromMatlabStringFormat(s))
469  THROW_EXCEPTION("Malformed expression in ::fromString");
470  ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
471  for (int i = 0; i < 3; i++) m_coords[i] = m.get_unsafe(0, i);
472  for (int i = 0; i < 3; i++) m_rotvec[i] = m.get_unsafe(0, 3 + i);
473  }
474 
475  /** @} */ // modif. components
476 
477  /** @name Lie Algebra methods
478  @{ */
479 
480  /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new
481  * CPose3DRotVec (static method). */
482  static CPose3DRotVec exp(const mrpt::math::CArrayDouble<6>& vect);
483 
484  /** Take the logarithm of the 3x4 matrix defined by this pose, generating
485  * the corresponding vector in the SE(3) Lie Algebra. */
486  void ln(mrpt::math::CArrayDouble<6>& out_ln) const;
487 
488  /** Take the logarithm of the 3x3 rotation matrix part of this pose,
489  * generating the corresponding vector in the Lie Algebra. */
491 
492  /** @} */
493 
494  /** Used to emulate CPosePDF types, for example, in
495  * mrpt::graphs::CNetworkOfPoses */
497  enum
498  {
500  };
501  static inline bool is_3D() { return is_3D_val != 0; }
502  enum
503  {
505  };
506  enum
507  {
509  };
510  static inline bool is_PDF() { return is_PDF_val != 0; }
511  inline const type_value& getPoseMean() const { return *this; }
512  inline type_value& getPoseMean() { return *this; }
513  void setToNaN() override;
514 
515  /** @name STL-like methods and typedefs
516  @{ */
517  /** The type of the elements */
518  using value_type = double;
519  using reference = double&;
520  using const_reference = const double&;
521  using size_type = std::size_t;
523 
524  // size is constant
525  enum
526  {
528  };
529  static inline size_type size() { return static_size; }
530  static inline bool empty() { return false; }
531  static inline size_type max_size() { return static_size; }
532  static inline void resize(const size_t n)
533  {
534  if (n != static_size)
535  throw std::logic_error(
536  format(
537  "Try to change the size of CPose3DRotVec to %u.",
538  static_cast<unsigned>(n)));
539  }
540  /** @} */
541 
542 }; // End of class def.
543 
544 std::ostream& operator<<(std::ostream& o, const CPose3DRotVec& p);
545 
546 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
547  * than a pose with negative x y z yaw pitch roll) */
548 CPose3DRotVec operator-(const CPose3DRotVec& p);
549 
550 bool operator==(const CPose3DRotVec& p1, const CPose3DRotVec& p2);
551 bool operator!=(const CPose3DRotVec& p1, const CPose3DRotVec& p2);
552 
553 } // End of namespace
554 } // End of namespace
555 
556 #endif
CPose3DRotVec(const double vx, const double vy, const double vz, const double x, const double y, const double z)
Constructor with initilization of the pose.
Definition: CPose3DRotVec.h:74
static CPose3DRotVec exp(const mrpt::math::CArrayDouble< 6 > &vect)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3DRotVec (static method)...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
CPose3DRotVec operator+(const CPose3DRotVec &b) const
The operator is the pose compounding operator.
void changeCoordinatesReference(const CPose3DRotVec &p)
makes: this = p (+) this
const double & operator[](unsigned int i) const
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...
GLdouble GLdouble z
Definition: glext.h:3872
void asString(std::string &s) const
Returns a human-readable textual representation of the object: "[x y z rx ry rz]".
const type_value & getPoseMean() const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void inverse()
Convert this pose into its inverse, saving the result in itself.
void setFromValues(const double x0, const double y0, const double z0, const double vx, const double vy, const double vz)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
CPose3DRotVec getInverse() const
Compute the inverse of this pose and return the result.
CPose3DRotVec operator-(const CPose3DRotVec &b) const
Compute .
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
GLenum GLsizei n
Definition: glext.h:5074
void setToNaN() override
Set all data fields to quiet NaN.
void addComponents(const CPose3DRotVec &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void setFromTransformationMatrix(const mrpt::math::CMatrixDouble44 &m)
double & operator[](unsigned int i)
const double & const_reference
GLdouble s
Definition: glext.h:3676
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) const
Computes the 3D point L such as .
const mrpt::math::CMatrixDouble33 getRotationMatrix() const
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...
static size_type size()
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]"...
A numeric matrix of compile-time fixed size.
void getAs6Vector(ARRAYORVECTOR &vec6) const
Gets pose as an array with these 6 elements: [x y z vx vy vz] where v{xyz} is the rotation vector and...
void setFrom6Vector(const ARRAYORVECTOR &vec6)
Set pose from an array with these 6 elements: [x y z vx vy vz] where v{xyz} is the rotation vector an...
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...
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:174
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) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
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 operator*=(const double s)
Scalar multiplication of x,y,z,vx,vy,vz.
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:27
mrpt::math::CArrayDouble< 3 > rotVecFromRotMat(const math::CMatrixDouble44 &m)
Create a vector with 3 components according to the input transformation matrix (only the rotation wil...
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
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
_W64 int ptrdiff_t
Definition: glew.h:137
const GLdouble * v
Definition: glext.h:3678
static void resize(const size_t n)
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...
CPose3DRotVec(TConstructorFlags_Poses constructor_dummy_param)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
Definition: CPose3DRotVec.h:67
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix part of this pose, generating the corresponding vector ...
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
CPose3DRotVec(const mrpt::math::CArrayDouble< 6 > &v)
Constructor with initilization of the pose from a vector [w1 w2 w3 x y z].
Definition: CPose3DRotVec.h:88
CPose3DRotVec & operator+=(const CPose3DRotVec &b)
Make (b can be "this" without problems)
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:53
void toQuatXYZ(CPose3DQuat &q) const
Convert this RVT into a quaternion + XYZ.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
void inverseComposeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
GLenum GLint GLint y
Definition: glext.h:3538
void composeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtA=nullptr, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtB=nullptr)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
CPose3DRotVec(const CPose3DRotVec &o)
Copy constructor.
Definition: CPose3DRotVec.h:99
CPose3DRotVec & operator=(const CPose3DRotVec &o)
Copy operator.
CPose3DRotVec(const double *vec6)
Constructor from an array with these 6 elements: [w1 w2 w3 x y z] where r{ij} are the entries of the ...
static size_type max_size()
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
Lightweight 3D point.
void getAsVector(ARRAYORVECTOR &v) const
Like getAs6Vector() but for dynamic size vectors (required by base class CPoseOrPoint) ...
GLfloat GLfloat p
Definition: glext.h:6305
double value_type
The type of the elements.
CPose3DRotVec()
Default constructor, with all the coordinates set to zero.
Definition: CPose3DRotVec.h:59
std::ptrdiff_t difference_type
std::string asString() const
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DRotVec.h:51



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