Main MRPT website > C++ reference for MRPT 1.5.6
CPose3DQuat.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 |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CPose3DQuat_H
10 #define CPose3DQuat_H
11
12 #include <mrpt/poses/CPose.h>
14 #include <mrpt/math/CQuaternion.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/poses_frwds.h>
18
19 namespace mrpt
20 {
21 namespace poses
22 {
23  DEFINE_SERIALIZABLE_PRE( CPose3DQuat )
24
25  /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
26  *
27  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
28  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> in the wiki.
29  *
30  * To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().
31  *
32  * This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator
33  * with indices running from 0 to 6 to access the [x y z qr qx qy qz] as if they were a vector. Thus, a CPose3DQuat can be used
34  * as a 7-vector anywhere the MRPT math functions expect any kind of vector.
35  *
36  * This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.
37  *
38  * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion), mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic version of this class, mrpt::math::CQuaternion, CPoseOrPoint
39  * \ingroup poses_grp
40  */
41  class BASE_IMPEXP CPose3DQuat : public CPose<CPose3DQuat>, public mrpt::utils::CSerializable
42  {
43  // This must be added to any CSerializable derived class:
44  DEFINE_SERIALIZABLE( CPose3DQuat )
45
46  public:
47  mrpt::math::CArrayDouble<3> m_coords; //!< The translation vector [x,y,z]
48  mrpt::math::CQuaternionDouble m_quat; //!< The quaternion.
49
50  public:
52  inline mrpt::math::CQuaternionDouble & quat() { return m_quat; }
54  inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
55
57  inline mrpt::math::CArrayDouble<3> & xyz() { return m_coords; }
59  inline const mrpt::math::CArrayDouble<3> & xyz() const { return m_coords; }
60
61
62  /** Default constructor, initialize translation to zeros and quaternion to no rotation. */
63  inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
64
65  /** Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor. */
69
70  /** Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary */
71  inline CPose3DQuat(const double x,const double y,const double z,const mrpt::math::CQuaternionDouble &q ) : m_quat(q) { m_coords[0]=x; m_coords[1]=y; m_coords[2]=z; m_quat.normalize(); }
72
73  /** Constructor from a CPose3D */
74  explicit CPose3DQuat(const CPose3D &p);
75
76  /** Constructor from lightweight object. */
77  CPose3DQuat(const mrpt::math::TPose3DQuat &p) : m_quat(p.qr,p.qx,p.qy,p.qz) { x()=p.x; y()=p.y; z()=p.z; }
78
79  /** Constructor from a 4x4 homogeneous transformation matrix.
80  */
81  explicit CPose3DQuat(const mrpt::math::CMatrixDouble44 &M);
82
83  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
84  * \sa getInverseHomogeneousMatrix
85  */
86  void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 & out_HM ) const;
87
88  /** Returns a 1x7 vector with [x y z qr qx qy qz] */
89  void getAsVector(mrpt::math::CVectorDouble &v) const;
92  v[0] = m_coords[0]; v[1] = m_coords[1]; v[2] = m_coords[2];
93  v[3] = m_quat[0]; v[4] = m_quat[1]; v[5] = m_quat[2]; v[6] = m_quat[3];
94  }
95
96  /** Makes \f\$ this = A \oplus B \f\$ this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
97  * \note A or B can be "this" without problems.
98  * \sa inverseComposeFrom, composePoint
99  */
100  void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
101
102  /** Makes \f\$ this = A \ominus B \f\$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
103  * \note A or B can be "this" without problems.
104  * \sa composeFrom, composePoint
105  */
106  void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
107
108  /** Computes the 3D point G such as \f\$ G = this \oplus L \f\$.
109  * \sa composeFrom, inverseComposePoint
110  */
111  void composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
112  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
113  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
114
115  /** Computes the 3D point L such as \f\$ L = G \ominus this \f\$.
116  * \sa composePoint, composeFrom
117  */
118  void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
119  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
120  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
121
122  /** Computes the 3D point G such as \f\$ G = this \oplus L \f\$.
123  * POINT1 and POINT1 can be anything supporing [0],[1],[2].
124  * \sa composePoint */
125  template <class POINT1,class POINT2> inline void composePoint( const POINT1 &L, POINT2 &G) const { composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); }
126
127  /** Computes the 3D point L such as \f\$ L = G \ominus this \f\$. \sa inverseComposePoint */
128  template <class POINT1,class POINT2> inline void inverseComposePoint( const POINT1 &G, POINT2 &L) const { inverseComposePoint(G[0],G[1],G[2],L[0],L[1],L[2]); }
129
130  /** Computes the 3D point G such as \f\$ G = this \oplus L \f\$. \sa composePoint */
131  inline CPoint3D operator +( const CPoint3D &L) const { CPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
132
133  /** Computes the 3D point G such as \f\$ G = this \oplus L \f\$. \sa composePoint */
134  inline mrpt::math::TPoint3D operator +( const mrpt::math::TPoint3D &L) const { mrpt::math::TPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
135
136  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar). */
137  virtual void operator *=(const double s);
138
139  /** Make \f\$ this = this \oplus b \f\$ */
140  inline CPose3DQuat& operator += (const CPose3DQuat& b)
141  {
142  composeFrom(*this,b);
143  return *this;
144  }
145
146  /** Return the composed pose \f\$ ret = this \oplus p \f\$ */
147  inline CPose3DQuat operator + (const CPose3DQuat& p) const
148  {
149  CPose3DQuat ret;
150  ret.composeFrom(*this,p);
151  return ret;
152  }
153
154  /** Make \f\$ this = this \ominus b \f\$ */
155  inline CPose3DQuat& operator -= (const CPose3DQuat& b)
156  {
157  inverseComposeFrom(*this,b);
158  return *this;
159  }
160
161  /** Return the composed pose \f\$ ret = this \ominus p \f\$ */
162  inline CPose3DQuat operator - (const CPose3DQuat& p) const
163  {
164  CPose3DQuat ret;
165  ret.inverseComposeFrom(*this,p);
166  return ret;
167  }
168
169  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
170  void inverse();
171
172  /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)
173  * \sa fromString
174  */
175  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_quat[0],m_quat[1],m_quat[2],m_quat[3]); }
176  inline std::string asString() const { std::string s; asString(s); return s; }
177
178  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )
179  * \sa asString
180  * \exception std::exception On invalid format
181  */
182  void fromString(const std::string &s) {
184  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
185  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
186  m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
187  m_quat[0] = m.get_unsafe(0,3); m_quat[1] = m.get_unsafe(0,4); m_quat[2] = m.get_unsafe(0,5); m_quat[3] = m.get_unsafe(0,6);
188  }
189
190  /** Read only [] operator */
191  inline const double &operator[](unsigned int i) const
192  {
193  switch(i)
194  {
195  case 0:return m_coords[0];
196  case 1:return m_coords[1];
197  case 2:return m_coords[2];
198  case 3:return m_quat[0];
199  case 4:return m_quat[1];
200  case 5:return m_quat[2];
201  case 6:return m_quat[3];
202  default:
203  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
204  }
205  }
206  /** Read/write [] operator */
207  inline double &operator[](unsigned int i)
208  {
209  switch(i)
210  {
211  case 0:return m_coords[0];
212  case 1:return m_coords[1];
213  case 2:return m_coords[2];
214  case 3:return m_quat[0];
215  case 4:return m_quat[1];
216  case 5:return m_quat[2];
217  case 6:return m_quat[3];
218  default:
219  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
220  }
221  }
222
223  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
224  * For the coordinate system see the top of this page.
225  * If the matrix pointers are not NULL, the Jacobians will be also computed for the range-yaw-pitch variables wrt the passed 3D point and this 7D pose.
226  */
227  void sphericalCoordinates(
228  const mrpt::math::TPoint3D &point,
229  double &out_range,
230  double &out_yaw,
231  double &out_pitch,
232  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
233  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
234  ) const;
235
236  public:
237  typedef CPose3DQuat type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
238  enum { is_3D_val = 1 };
239  static inline bool is_3D() { return is_3D_val!=0; }
240  enum { rotation_dimensions = 3 };
241  enum { is_PDF_val = 1 };
242  static inline bool is_PDF() { return is_PDF_val!=0; }
243
244  inline const type_value & getPoseMean() const { return *this; }
245  inline type_value & getPoseMean() { return *this; }
246
247  /** @name STL-like methods and typedefs
248  @{ */
249  typedef double value_type; //!< The type of the elements
250  typedef double& reference;
251  typedef const double& const_reference;
252  typedef std::size_t size_type;
254
255  // size is constant
256  enum { static_size = 7 };
257  static inline size_type size() { return static_size; }
258  static inline bool empty() { return false; }
259  static inline size_type max_size() { return static_size; }
260  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DQuat to %u.",static_cast<unsigned>(n))); }
261
262  inline void assign(const size_t N, const double val)
263  {
264  if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
265  m_coords.fill(val);
266  m_quat.fill(val);
267  }
268
269  struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
270  {
271  private:
272  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
273  CPose3DQuat *m_obj; //!< A reference to the source of this iterator
274  size_t m_cur_idx; //!< The iterator points to this element.
275  typedef value_type T; //!< The type of the matrix elements
276
277  inline void check_limits(bool allow_end = false) const
278  {
279  #ifdef _DEBUG
280  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
281  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
282  #else
283  MRPT_UNUSED_PARAM(allow_end);
284  #endif
285  }
286  public:
287  inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
288  inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
289  inline iterator() : m_obj(NULL),m_cur_idx(0) { }
290  inline iterator(CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
291  inline CPose3DQuat::reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
292  inline iterator &operator++() {
293  check_limits();
294  ++m_cur_idx;
295  return *this;
296  }
297  inline iterator operator++(int) {
298  iterator it=*this;
299  ++*this;
300  return it;
301  }
302  inline iterator &operator--() {
303  --m_cur_idx;
304  check_limits();
305  return *this;
306  }
307  inline iterator operator--(int) {
308  iterator it=*this;
309  --*this;
310  return it;
311  }
312  inline iterator &operator+=(iterator_base::difference_type off) {
313  m_cur_idx+=off;
314  check_limits(true);
315  return *this;
316  }
317  inline iterator operator+(iterator_base::difference_type off) const {
318  iterator it=*this;
319  it+=off;
320  return it;
321  }
322  inline iterator &operator-=(iterator_base::difference_type off) {
323  return (*this)+=(-off);
324  }
325  inline iterator operator-(iterator_base::difference_type off) const {
326  iterator it=*this;
327  it-=off;
328  return it;
329  }
330  inline iterator_base::difference_type operator-(const iterator &it) const { return m_cur_idx - it.m_cur_idx; }
331  inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
332  inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
333  inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
334  }; // end iterator
335
336  struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
337  {
338  private:
339  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
340  const CPose3DQuat *m_obj; //!< A reference to the source of this iterator
341  size_t m_cur_idx; //!< The iterator points to this element.
342  typedef value_type T; //!< The type of the matrix elements
343
344  inline void check_limits(bool allow_end = false) const
345  {
346  #ifdef _DEBUG
347  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
348  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
349  #else
350  MRPT_UNUSED_PARAM(allow_end);
351  #endif
352  }
353  public:
354  inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
355  inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
356  inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
357  inline const_iterator(const CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
358  inline CPose3DQuat::const_reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
360  check_limits();
361  ++m_cur_idx;
362  return *this;
363  }
365  const_iterator it=*this;
366  ++*this;
367  return it;
368  }
370  --m_cur_idx;
371  check_limits();
372  return *this;
373  }
375  const_iterator it=*this;
376  --*this;
377  return it;
378  }
379  inline const_iterator &operator+=(iterator_base::difference_type off) {
380  m_cur_idx+=off;
381  check_limits(true);
382  return *this;
383  }
384  inline const_iterator operator+(iterator_base::difference_type off) const {
385  const_iterator it=*this;
386  it+=off;
387  return it;
388  }
389  inline const_iterator &operator-=(iterator_base::difference_type off) {
390  return (*this)+=(-off);
391  }
392  inline const_iterator operator-(iterator_base::difference_type off) const {
393  const_iterator it=*this;
394  it-=off;
395  return it;
396  }
397  inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
398  inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
399  inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
400  inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
401  }; // end const_iterator
402
403  typedef std::reverse_iterator<iterator> reverse_iterator;
404  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
405  inline iterator begin() { return iterator(*this,0); }
406  inline iterator end() { return iterator(*this,static_size); }
407  inline const_iterator begin() const { return const_iterator(*this,0); }
408  inline const_iterator end() const { return const_iterator(*this,static_size); }
409  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
410  inline const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
411  inline reverse_iterator rend() { return reverse_iterator(begin()); }
412  inline const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
413
414
415  void swap (CPose3DQuat& o)
416  {
417  std::swap(o.m_coords, m_coords);
418  o.m_quat.swap(m_quat);
419  }
420
421  /** @} */
422  //! See ops_containers.h
423  typedef CPose3DQuat mrpt_autotype;
424  //DECLARE_MRPT_CONTAINER_TYPES
425
426  void setToNaN() MRPT_OVERRIDE;
427
428  }; // End of class def.
429  DEFINE_SERIALIZABLE_POST( CPose3DQuat )
430
431  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DQuat& p);
432
433  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with all its arguments multiplied by "-1") */
434  CPose3DQuat BASE_IMPEXP operator -(const CPose3DQuat &p);
435  /** Computes the 3D point L such as \f\$ L = G \ominus this \f\$. \sa inverseComposePoint */
436  CPoint3D BASE_IMPEXP operator -(const CPoint3D &G, const CPose3DQuat &p);
437  /** Computes the 3D point L such as \f\$ L = G \ominus this \f\$. \sa inverseComposePoint */
438  mrpt::math::TPoint3D BASE_IMPEXP operator -(const mrpt::math::TPoint3D &G,const CPose3DQuat &p);
439
440  bool BASE_IMPEXP operator==(const CPose3DQuat &p1,const CPose3DQuat &p2);
441  bool BASE_IMPEXP operator!=(const CPose3DQuat &p1,const CPose3DQuat &p2);
442
443
444  } // End of namespace
445 } // End of namespace
446
447 #endif
const GLdouble * v
Definition: glew.h:1296
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:273
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:275
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:291
const double & const_reference
Definition: CPose3DQuat.h:251
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:410
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
static size_type max_size()
Definition: CPose3DQuat.h:259
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:274
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:331
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:244
std::string asString() const
Definition: CPose3DQuat.h:176
CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)
Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...
Definition: CPose3DQuat.h:66
void assign(const size_t N, const double val)
Definition: CPose3DQuat.h:262
mrpt::math::CArrayDouble< 3 > & xyz()
Definition: CPose3DQuat.h:57
reverse_iterator rbegin()
Definition: CPose3DQuat.h:409
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:392
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:400
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
const_iterator begin() const
Definition: CPose3DQuat.h:407
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
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:290
static size_type size()
Definition: CPose3DQuat.h:257
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:384
#define THROW_EXCEPTION(msg)
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
Definition: CPose3DQuat.h:182
Scalar * iterator
Definition: eigen_plugins.h:23
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:77
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:272
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:408
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
Definition: CPose3DQuat.h:91
reverse_iterator rend()
Definition: CPose3DQuat.h:411
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:399
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:312
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:344
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:333
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:317
GLdouble s
Definition: glew.h:1295
A numeric matrix of compile-time fixed size.
GLuint GLuint end
Definition: glew.h:1167
GLsizei n
Definition: glew.h:5051
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:389
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
const mrpt::math::CQuaternionDouble & quat() const
Definition: CPose3DQuat.h:54
void composeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A + B;" since it avoids the temporary objec...
Definition: CPose3DQuat.cpp:75
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
double z
Translation in x,y,z.
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
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:397
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:340
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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
GLhandleARB obj
Definition: glew.h:3276
double value_type
The type of the elements.
Definition: CPose3DQuat.h:249
double & operator[](unsigned int i)
Definition: CPose3DQuat.h:207
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]"...
Definition: CPose3DQuat.h:175
const double & operator[](unsigned int i) const
Definition: CPose3DQuat.h:191
bool operator>(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:289
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
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
A class used to store a 3D point.
Definition: CPoint3D.h:32
type_value & getPoseMean()
Definition: CPose3DQuat.h:245
_W64 int ptrdiff_t
Definition: glew.h:133
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:47
CPose3DQuat(TConstructorFlags_Poses)
Definition: CPose3DQuat.h:68
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLdouble GLdouble z
Definition: glew.h:1464
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:277
GLsizei const GLcharARB ** string
Definition: glew.h:3293
size_t size(const MATRIXLIKE &m, const int dim)
CPose3DQuat mrpt_autotype
See ops_containers.h.
Definition: CPose3DQuat.h:423
static void resize(const size_t n)
Definition: CPose3DQuat.h:260
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
CPose3DQuat(const double x, const double y, const double z, const mrpt::math::CQuaternionDouble &q)
Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary...
Definition: CPose3DQuat.h:71
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:339
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:253
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:322
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:379
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:398
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:63
void inverseComposeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose3DQuat.cpp:92
const_iterator end() const
Definition: CPose3DQuat.h:408
const GLdouble * m
Definition: glew.h:5094
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
Definition: CPoint.h:116
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:330
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:332
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:128
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:412
#define DEFINE_SERIALIZABLE_POST(class_name)
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:48
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:403
GLuint GLfloat * val
Definition: glew.h:7785
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
Lightweight 3D point.
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose3DQuat.h:237
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:404
iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:325
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:341
const mrpt::math::CArrayDouble< 3 > & xyz() const
Definition: CPose3DQuat.h:59
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:342
#define ASSERTMSG_(f, __ERROR_MSG)
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:357
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:358
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:415
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:125

 Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018