Main MRPT website > C++ reference for MRPT 1.5.7
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-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 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  DEFINE_SERIALIZABLE_PRE( CPose3DRotVec )
22 
23  /** A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivalent to axis-angle).
24  * The 6D transformation in SE(3) stored in this class is kept in two
25  * separate containers: a 3-array for the rotation vector, and a 3-array for 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, or refer
34  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> online.
35  *
36  * There are Lie algebra methods: \a exp and \a ln (see the methods for documentation).
37  *
38  * \ingroup poses_grp
39  * \sa CPose3DRotVec, CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
40  */
41  class BASE_IMPEXP CPose3DRotVec : public CPose<CPose3DRotVec>, public mrpt::utils::CSerializable
42  {
43  // This must be added to any CSerializable derived class:
45 
46  public:
47  mrpt::math::CArrayDouble<3> m_coords; //!< The translation vector [x,y,z]
48  mrpt::math::CArrayDouble<3> m_rotvec; //!< The rotation vector [vx,vy,vz]
49 
50  /** @name Constructors
51  @{ */
52 
53  /** Default constructor, with all the coordinates set to zero. */
54  inline CPose3DRotVec() {
55  m_coords[0]=m_coords[1]=m_coords[2]=0;
56  m_rotvec[0]=m_rotvec[1]=m_rotvec[2]=0;
57  }
58 
59  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
60  inline CPose3DRotVec(TConstructorFlags_Poses constructor_dummy_param) : m_coords(),m_rotvec() {
61  MRPT_UNUSED_PARAM(constructor_dummy_param);
62  }
63 
64  /** Constructor with initilization of the pose */
65  inline CPose3DRotVec(const double vx, const double vy, const double vz, const double x,const double y,const double z) {
66  m_coords[0]= x; m_coords[1]= y; m_coords[2]= z;
67  m_rotvec[0]=vx; m_rotvec[1]=vy; m_rotvec[2]=vz;
68  }
69 
70  /** Constructor with initilization of the pose from a vector [w1 w2 w3 x y z] */
72  m_rotvec[0]=v[0]; m_rotvec[1]=v[1]; m_rotvec[2]=v[2];
73  m_coords[0]=v[3]; m_coords[1]=v[4]; m_coords[2]=v[5];
74  }
75 
76  /** Copy constructor */
77  inline CPose3DRotVec(const CPose3DRotVec &o)
78  {
79  this->m_rotvec = o.m_rotvec;
80  this->m_coords = o.m_coords;
81  }
82 
83  /** Constructor from a 4x4 homogeneous matrix: */
84  explicit CPose3DRotVec(const math::CMatrixDouble44 &m);
85 
86  /** Constructor from a CPose3D object.*/
87  explicit CPose3DRotVec(const CPose3D &m);
88 
89  /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
90  CPose3DRotVec(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );
91 
92  /** Constructor from an array with these 6 elements: [w1 w2 w3 x y z]
93  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
94  * \sa setFrom12Vector, getAs12Vector
95  */
96  inline explicit CPose3DRotVec(const double *vec6) {
97  m_coords[0]=vec6[3]; m_coords[1]=vec6[4]; m_coords[2]=vec6[5];
98  m_rotvec[0]=vec6[0]; m_rotvec[1]=vec6[1]; m_rotvec[2]=vec6[2];
99  }
100 
101  /** @} */ // end Constructors
102 
103 
104  /** @name Access 3x3 rotation and 4x4 homogeneous matrices
105  @{ */
106 
107  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
108  * \sa getInverseHomogeneousMatrix, getRotationMatrix
109  */
110  inline void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 & out_HM) const {
111  out_HM.block<3,3>(0,0) = getRotationMatrix();
112  out_HM.set_unsafe(0,3,m_coords[0]);
113  out_HM.set_unsafe(1,3,m_coords[1]);
114  out_HM.set_unsafe(2,3,m_coords[2]);
115  out_HM.set_unsafe(3,0,0); out_HM.set_unsafe(3,1,0); out_HM.set_unsafe(3,2,0); out_HM.set_unsafe(3,3,1);
116  }
117 
118  inline mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const { mrpt::math::CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}
119 
120  /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
121  void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const;
122  //! \overload
124 
125  /** @} */ // end rot and HM
126 
127 
128  /** @name Pose-pose and pose-point compositions and operators
129  @{ */
130 
131  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
133  {
135  ret.composeFrom(*this,b);
136  return ret;
137  }
138 
139  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
140  CPoint3D operator + (const CPoint3D& b) const;
141 
142  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
143  CPoint3D operator + (const CPoint2D& b) const;
144 
146  {
147  mrpt::math::CArrayDouble<3> aux = rotVecFromRotMat( m );
148 
149  this->m_rotvec[0] = aux[0];
150  this->m_rotvec[1] = aux[1];
151  this->m_rotvec[2] = aux[2];
152 
153  this->m_coords[0] = m.get_unsafe(0,3);
154  this->m_coords[1] = m.get_unsafe(1,3);
155  this->m_coords[2] = m.get_unsafe(2,3);
156  }
157 
158  void setFromXYZAndAngles( const double x,const double y,const double z,const double yaw=0, const double pitch=0, const double roll=0 );
159 
160  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. For the coordinate system see mrpt::poses::CPose3D */
161  void sphericalCoordinates(
162  const mrpt::math::TPoint3D &point,
163  double &out_range,
164  double &out_yaw,
165  double &out_pitch ) const;
166 
167  /** 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.
168  * If pointers are provided, the corresponding Jacobians are returned.
169  * See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
170  */
171  void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
172  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
173  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const;
174 
175  /** 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.
176  * \note local_point is passed by value to allow global and local point to be the same variable
177  */
178  inline void composePoint(const mrpt::math::TPoint3D local_point, mrpt::math::TPoint3D &global_point) const {
179  composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,global_point.z );
180  }
181 
182  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
183  * If pointers are provided, the corresponding Jacobians are returned.
184  * See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
185  * \sa composePoint, composeFrom
186  */
187  void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
188  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
189  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const;
190 
191  /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
192  * \note A or B can be "this" without problems.
193  */
194  void composeFrom(const CPose3DRotVec& A,
195  const CPose3DRotVec& B,
196  mrpt::math::CMatrixFixedNumeric<double,6,6> *out_jacobian_drvtC_drvtA=NULL,
197  mrpt::math::CMatrixFixedNumeric<double,6,6> *out_jacobian_drvtC_drvtB=NULL);
198 
199 
200  /** Convert this RVT into a quaternion + XYZ
201  */
202  void toQuatXYZ(CPose3DQuat &q) const;
203 
204  /** Make \f$ this = this \oplus b \f$ (\a b can be "this" without problems) */
206  {
207  composeFrom(*this,b);
208  return *this;
209  }
210 
211  /** Copy operator */
212  inline CPose3DRotVec& operator = (const CPose3DRotVec& o)
213  {
214  this->m_rotvec = o.m_rotvec;
215  this->m_coords = o.m_coords;
216  return *this;
217  }
218 
219  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
220  * \note A or B can be "this" without problems.
221  * \sa composeFrom, composePoint
222  */
223  void inverseComposeFrom(const CPose3DRotVec& A, const CPose3DRotVec& B );
224 
225  /** Compute \f$ RET = this \oplus b \f$ */
227  {
229  ret.inverseComposeFrom(*this,b);
230  return ret;
231  }
232 
233  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
234  void inverse();
235 
236  /** Compute the inverse of this pose and return the result. */
237  CPose3DRotVec getInverse() const;
238 
239  /** makes: this = p (+) this */
240  inline void changeCoordinatesReference( const CPose3DRotVec & p ) { composeFrom(p,CPose3DRotVec(*this)); }
241 
242  /** @} */ // compositions
243 
244 
245  /** @name Access and modify contents
246  @{ */
247 
248  inline double rx() const { return m_rotvec[0]; }
249  inline double ry() const { return m_rotvec[1]; }
250  inline double rz() const { return m_rotvec[2]; }
251 
252  inline double &rx() { return m_rotvec[0]; }
253  inline double &ry() { return m_rotvec[1]; }
254  inline double &rz() { return m_rotvec[2]; }
255 
256  /** Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators. */
257  inline void addComponents(const CPose3DRotVec &p) {
258  m_coords+=p.m_coords;
259  m_rotvec+=p.m_rotvec;
260  }
261 
262  /** Scalar multiplication of x,y,z,vx,vy,vz. */
263  inline void operator *=(const double s) {
264  m_coords*=s;
265  m_rotvec*=s;
266  }
267 
268  /** Create a vector with 3 components according to the input transformation matrix (only the rotation will be taken into account)
269  */
270  mrpt::math::CArrayDouble<3> rotVecFromRotMat( const math::CMatrixDouble44 &m );
271 
272  /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
273  * \sa getYawPitchRoll, setYawPitchRoll
274  */
276  const double x0,
277  const double y0,
278  const double z0,
279  const double vx,
280  const double vy,
281  const double vz )
282  {
283  m_coords[0]=x0; m_coords[1]=y0; m_coords[2]=z0;
284  m_rotvec[0]=vx; m_rotvec[1]=vy; m_rotvec[2]=vz;
285  }
286 
287  /** Set pose from an array with these 6 elements: [x y z vx vy vz]
288  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the pose
289  * \sa getAs6Vector
290  */
291  template <class ARRAYORVECTOR>
292  inline void setFrom6Vector(const ARRAYORVECTOR &vec6)
293  {
294  m_rotvec[0]=vec6[3]; m_rotvec[1]=vec6[4]; m_rotvec[2]=vec6[5];
295  m_coords[0]=vec6[0]; m_coords[1]=vec6[1]; m_coords[2]=vec6[2];
296  }
297 
298  /** Gets pose as an array with these 6 elements: [x y z vx vy vz]
299  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the pose
300  * The target vector MUST ALREADY have space for 6 elements (i.e. no .resize() method will be called).
301  * \sa setAs6Vector, getAsVector
302  */
303  template <class ARRAYORVECTOR>
304  inline void getAs6Vector(ARRAYORVECTOR &vec6) const
305  {
306  vec6[0]=m_coords[0]; vec6[1]=m_coords[1]; vec6[2]=m_coords[2];
307  vec6[3]=m_rotvec[0]; vec6[4]=m_rotvec[1]; vec6[5]=m_rotvec[2];
308  }
309 
310  /** Like getAs6Vector() but for dynamic size vectors (required by base class CPoseOrPoint) */
311  template <class ARRAYORVECTOR>
312  inline void getAsVector(ARRAYORVECTOR &v) const { v.resize(6); getAs6Vector(v); }
313 
314  inline const double &operator[](unsigned int i) const
315  {
316  switch(i)
317  {
318  case 0:return m_coords[0];
319  case 1:return m_coords[1];
320  case 2:return m_coords[2];
321  case 3:return m_rotvec[0];
322  case 4:return m_rotvec[1];
323  case 5:return m_rotvec[2];
324  default:
325  throw std::runtime_error("CPose3DRotVec::operator[]: Index of bounds.");
326  }
327  }
328  inline double &operator[](unsigned int i)
329  {
330  switch(i)
331  {
332  case 0:return m_coords[0];
333  case 1:return m_coords[1];
334  case 2:return m_coords[2];
335  case 3:return m_rotvec[0];
336  case 4:return m_rotvec[1];
337  case 5:return m_rotvec[2];
338  default:
339  throw std::runtime_error("CPose3DRotVec::operator[]: Index of bounds.");
340  }
341  }
342 
343  /** Returns a human-readable textual representation of the object: "[x y z rx ry rz]"
344  * \sa fromString
345  */
346  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_rotvec[0],m_rotvec[1],m_rotvec[2]); }
347  inline std::string asString() const { std::string s; asString(s); return s; }
348 
349  /** Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. )
350  * \sa asString
351  * \exception std::exception On invalid format
352  */
353  void fromString(const std::string &s) {
355  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
356  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
357  for (int i=0;i<3;i++) m_coords[i]=m.get_unsafe(0,i);
358  for (int i=0;i<3;i++) m_rotvec[i]=m.get_unsafe(0,3+i);
359  }
360 
361  /** @} */ // modif. components
362 
363 
364  /** @name Lie Algebra methods
365  @{ */
366 
367  /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3DRotVec (static method). */
368  static CPose3DRotVec exp(const mrpt::math::CArrayDouble<6> & vect);
369 
370  /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra. */
371  void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
372 
373  /** Take the logarithm of the 3x3 rotation matrix part of this pose, generating the corresponding vector in the Lie Algebra. */
374  mrpt::math::CArrayDouble<3> ln_rotation() const;
375 
376  /** @} */
377 
378  typedef CPose3DRotVec type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
379  enum { is_3D_val = 1 };
380  static inline bool is_3D() { return is_3D_val!=0; }
381  enum { rotation_dimensions = 3 };
382  enum { is_PDF_val = 0 };
383  static inline bool is_PDF() { return is_PDF_val!=0; }
384 
385  inline const type_value & getPoseMean() const { return *this; }
386  inline type_value & getPoseMean() { return *this; }
387 
388  void setToNaN() MRPT_OVERRIDE;
389 
390  /** @name STL-like methods and typedefs
391  @{ */
392  typedef double value_type; //!< The type of the elements
393  typedef double& reference;
394  typedef const double& const_reference;
395  typedef std::size_t size_type;
397 
398 
399  // size is constant
400  enum { static_size = 6 };
401  static inline size_type size() { return static_size; }
402  static inline bool empty() { return false; }
403  static inline size_type max_size() { return static_size; }
404  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DRotVec to %u.",static_cast<unsigned>(n))); }
405  /** @} */
406 
407  }; // End of class def.
408  DEFINE_SERIALIZABLE_POST( CPose3DRotVec )
409 
410 
411  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DRotVec& p);
412 
413  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
414  CPose3DRotVec BASE_IMPEXP operator -(const CPose3DRotVec &p);
415 
416  bool BASE_IMPEXP operator==(const CPose3DRotVec &p1,const CPose3DRotVec &p2);
417  bool BASE_IMPEXP operator!=(const CPose3DRotVec &p1,const CPose3DRotVec &p2);
418 
419 
420  } // End of namespace
421 } // End of namespace
422 
423 #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:65
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:3734
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
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...
#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::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
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
GLenum GLsizei n
Definition: glext.h:4618
void addComponents(const CPose3DRotVec &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
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)
STL namespace.
double & operator[](unsigned int i)
double z
X,Y,Z coordinates.
GLdouble s
Definition: glext.h:3602
const mrpt::math::CMatrixDouble33 getRotationMatrix() const
static size_type size()
CPose3DRotVec type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
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 composeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtA=NULL, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtB=NULL)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
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
double value_type
The type of the elements.
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
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
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
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
_W64 int ptrdiff_t
Definition: glew.h:133
const GLdouble * v
Definition: glext.h:3603
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:60
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 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:71
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:48
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
const double & const_reference
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:3516
CPose3DRotVec(const CPose3DRotVec &o)
Copy constructor.
Definition: CPose3DRotVec.h:77
#define DEFINE_SERIALIZABLE_POST(class_name)
std::ptrdiff_t difference_type
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 ...
Definition: CPose3DRotVec.h:96
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: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
Lightweight 3D point.
#define ASSERTMSG_(f, __ERROR_MSG)
void getAsVector(ARRAYORVECTOR &v) const
Like getAs6Vector() but for dynamic size vectors (required by base class CPoseOrPoint) ...
GLfloat GLfloat p
Definition: glext.h:5587
CPose3DRotVec()
Default constructor, with all the coordinates set to zero.
Definition: CPose3DRotVec.h:54
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
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:106
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DRotVec.h:47



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019