Main MRPT website > C++ reference for MRPT 1.5.9
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 |
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 CPose3DQuat_H
10 #define CPose3DQuat_H
11 
12 #include <mrpt/poses/CPose.h>
13 #include <mrpt/math/CMatrixFixedNumeric.h>
14 #include <mrpt/math/CQuaternion.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/poses_frwds.h>
17 #include <mrpt/math/lightweight_geom_data.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:
51  /** Read/Write access to the quaternion representing the 3D rotation. */
52  inline mrpt::math::CQuaternionDouble & quat() { return m_quat; }
53  /** Read-only access to the quaternion representing the 3D rotation. */
54  inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
55 
56  /** Read/Write access to the translation vector in R^3. */
57  inline mrpt::math::CArrayDouble<3> & xyz() { return m_coords; }
58  /** Read-only access to the translation vector in R^3. */
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. */
66  inline CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions ) : m_quat(mrpt::math::UNINITIALIZED_QUATERNION) { }
67  /** \overload */
68  inline CPose3DQuat(TConstructorFlags_Poses ) : m_quat(mrpt::math::UNINITIALIZED_QUATERNION) { }
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;
90  /// \overload
91  void getAsVector(mrpt::math::CArrayDouble<7> &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) {
183  mrpt::math::CMatrixDouble m;
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;
253  typedef std::ptrdiff_t difference_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]; }
359  inline const_iterator &operator++() {
360  check_limits();
361  ++m_cur_idx;
362  return *this;
363  }
364  inline const_iterator operator++(int) {
365  const_iterator it=*this;
366  ++*this;
367  return it;
368  }
369  inline const_iterator &operator--() {
370  --m_cur_idx;
371  check_limits();
372  return *this;
373  }
374  inline const_iterator operator--(int) {
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
mrpt::poses::CPose3DQuat::iterator::m_obj
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:273
mrpt::poses::CPose3DQuat::iterator::T
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:275
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
mrpt::poses::CPose3DQuat::const_reference
const double & const_reference
Definition: CPose3DQuat.h:251
mrpt::poses::CPose3DQuat::max_size
static size_type max_size()
Definition: CPose3DQuat.h:259
mrpt::poses::CPose3DQuat::const_iterator::const_iterator
const_iterator()
Definition: CPose3DQuat.h:356
CMatrixFixedNumeric.h
mrpt::poses::CPose3DQuat::iterator::m_cur_idx
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:274
mrpt::poses::CPose3DQuat::end
const_iterator end() const
Definition: CPose3DQuat.h:408
mrpt::math::CQuaternion::normalize
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:220
z
GLdouble GLdouble z
Definition: glext.h:3734
static_size
Definition: eigen_plugins.h:17
mrpt::poses::CPose3DQuat::CPose3DQuat
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
mrpt::poses::CPose3DQuat::assign
void assign(const size_t N, const double val)
Definition: CPose3DQuat.h:262
mrpt::poses::CPose3DQuat::xyz
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
Definition: CPose3DQuat.h:57
mrpt::poses::CPose3DQuat::iterator::operator*
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:291
mrpt::poses::CPose3DQuat::rbegin
reverse_iterator rbegin()
Definition: CPose3DQuat.h:409
MRPT_OVERRIDE
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: maps/CColouredPointsMap.h:58
DEFINE_SERIALIZABLE_PRE
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Definition: CSerializable.h:162
mrpt::poses::CPose3DQuat::iterator::operator-
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:330
mrpt::math::TConstructorFlags_Quaternions
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
mrpt::poses::CPose3DQuat::const_iterator::operator!=
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:400
mrpt::poses::operator+
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
mrpt::poses::CPose3DQuat::iterator::iterator
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:290
mrpt::poses::CPose3DQuat::size
static size_type size()
Definition: CPose3DQuat.h:257
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
mrpt::poses::CPose3DQuat::rbegin
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:410
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
mrpt::poses::CPose3DQuat::fromString
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
Definition: CPose3DQuat.h:182
n
GLenum GLsizei n
Definition: glext.h:4618
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::poses::CPose3DQuat::const_iterator
Definition: CPose3DQuat.h:336
mrpt::poses::CPose3DQuat::CPose3DQuat
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:77
mrpt::poses::CPose3DQuat::iterator::iterator_base
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:272
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::poses::CPose3DQuat::iterator::operator[]
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:331
std
STL namespace.
mrpt::poses::CPose3DQuat::const_iterator::check_limits
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:344
mrpt::poses::CPose3DQuat::xyz
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
Definition: CPose3DQuat.h:59
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
s
GLdouble s
Definition: glext.h:3602
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::poses::CPose3DQuat::rend
reverse_iterator rend()
Definition: CPose3DQuat.h:411
mrpt::poses::CPose3DQuat::const_iterator::operator++
const_iterator operator++(int)
Definition: CPose3DQuat.h:364
mrpt::poses::CPose3DQuat::const_iterator::operator*
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:358
mrpt::poses::CPose3DQuat::iterator::operator+=
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:312
mrpt::poses::CPose3DQuat::const_iterator::operator-
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:397
mrpt::poses::CPose3DQuat::iterator::operator!=
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:333
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
mrpt::poses::CPose3DQuat::begin
const_iterator begin() const
Definition: CPose3DQuat.h:407
mrpt::poses::CPose3DQuat::getAsVector
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
Definition: CPose3DQuat.h:91
mrpt::poses::CPose3DQuat::const_iterator::operator-=
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:389
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::poses::CPose3DQuat::composeFrom
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
mrpt::poses::CPose3DQuat::const_iterator::operator+
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:384
mrpt::poses::CPose3DQuat::quat
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:54
poses_frwds.h
mrpt::math::operator+=
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
BASE_IMPEXP
#define BASE_IMPEXP
Definition: maps/CColouredPointsMap.h:42
mrpt::poses::operator-
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
mrpt::poses::CPose3DQuat::const_iterator::operator-
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:392
end
GLuint GLuint end
Definition: glext.h:3512
mrpt::poses::CPose3DQuat::const_iterator::m_obj
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:340
mrpt::poses::CPose
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:25
mrpt::poses::CPose3DQuat::getPoseMean
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:244
mrpt::pbmap::inverse
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
val
int val
Definition: mrpt_jpeglib.h:953
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
b
GLubyte GLubyte b
Definition: glext.h:5575
mrpt::poses::CPose3DQuat::value_type
double value_type
The type of the elements.
Definition: CPose3DQuat.h:249
mrpt::poses::CPose3DQuat::iterator::operator==
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:332
mrpt::poses::CPose3DQuat::operator[]
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:207
mrpt::utils::operator>
bool operator>(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:289
mrpt::poses::CPose3DQuat
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
CPose.h
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::poses::CPose3DQuat::iterator::operator-
iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:325
CQuaternion.h
string
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::poses::CPose3DQuat::getPoseMean
type_value & getPoseMean()
Definition: CPose3DQuat.h:245
mrpt::poses::CPose3DQuat::const_iterator::operator--
const_iterator operator--(int)
Definition: CPose3DQuat.h:374
ptrdiff_t
_W64 int ptrdiff_t
Definition: glew.h:133
mrpt::math::TPose3DQuat
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: lightweight_geom_data.h:483
mrpt::poses::CPose3DQuat::m_coords
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:47
mrpt::poses::CPose3DQuat::inverseComposePoint
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:128
mrpt::poses::CPose3DQuat::const_iterator::operator++
const_iterator & operator++()
Definition: CPose3DQuat.h:359
mrpt::poses::CPose3DQuat::CPose3DQuat
CPose3DQuat(TConstructorFlags_Poses)
Definition: CPose3DQuat.h:68
mrpt::poses::CPose3DQuat::composePoint
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:125
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:148
mrpt::mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: maps/CColouredPointsMap.h:43
lightweight_geom_data.h
mrpt::poses::CPose3DQuat::mrpt_autotype
CPose3DQuat mrpt_autotype
See ops_containers.h.
Definition: CPose3DQuat.h:423
mrpt::poses::CPose3DQuat::resize
static void resize(const size_t n)
Definition: CPose3DQuat.h:260
mrpt::poses::operator==
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
mrpt::poses::CPose3DQuat::CPose3DQuat
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&#39;s unitary...
Definition: CPose3DQuat.h:71
mrpt::poses::CPose3DQuat::const_iterator::iterator_base
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:339
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::poses::CPose3DQuat::difference_type
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:253
mrpt::poses::CPose3DQuat::iterator::operator+
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:317
mrpt::poses::CPose3DQuat::iterator::operator-=
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:322
mrpt::poses::CPose3DQuat::rend
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:412
mrpt::poses::CPose3DQuat::const_iterator::operator+=
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:379
mrpt::poses::CPose3DQuat::asString
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
mrpt::poses::CPose3DQuat::CPose3DQuat
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:63
mrpt::poses::CPose3DQuat::inverseComposeFrom
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
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::poses::operator<
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
Definition: CPoint.h:116
mrpt::poses::CPose3DQuat::iterator::check_limits
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:277
mrpt::poses::CPose3DQuat::const_iterator::operator==
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:399
DEFINE_SERIALIZABLE_POST
#define DEFINE_SERIALIZABLE_POST(class_name)
Definition: CSerializable.h:166
mrpt::poses::CPose3DQuat::m_quat
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:48
mrpt::poses::CPose3DQuat::reverse_iterator
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:403
mrpt::poses::CPose3DQuat::operator[]
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:191
CPoint3D.h
x
GLenum GLint x
Definition: glext.h:3516
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
mrpt::poses::CPose3DQuat::type_value
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose3DQuat.h:237
mrpt::poses::CPose3DQuat::const_reverse_iterator
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:404
mrpt::poses::CPose3DQuat::const_iterator::m_cur_idx
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:341
mrpt::poses::CPose3DQuat::const_iterator::T
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:342
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:277
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::poses::CPose3DQuat::const_iterator::operator--
const_iterator & operator--()
Definition: CPose3DQuat.h:369
mrpt::poses::CPose3DQuat::const_iterator::const_iterator
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:357
mrpt::math::operator*=
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
mrpt::poses::CPose3DQuat::asString
std::string asString() const
Definition: CPose3DQuat.h:176
mrpt::poses::CPose3DQuat::const_iterator::operator[]
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:398
mrpt::poses::CPose3DQuat::swap
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:415


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020