Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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>
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 {
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. */
67  /** \overload */
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
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  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
137  inline CPoint3D operator -( const CPoint3D &G) const { CPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
138 
139  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
140  inline mrpt::math::TPoint3D operator -( const mrpt::math::TPoint3D &G) const { mrpt::math::TPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
141 
142  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
143  */
144  virtual void operator *=(const double s);
145 
146  /** Make \f$ this = this \oplus b \f$ */
147  inline CPose3DQuat& operator += (const CPose3DQuat& b)
148  {
149  composeFrom(*this,b);
150  return *this;
151  }
152 
153  /** Return the composed pose \f$ ret = this \oplus p \f$ */
154  inline CPose3DQuat operator + (const CPose3DQuat& p) const
155  {
156  CPose3DQuat ret;
157  ret.composeFrom(*this,p);
158  return ret;
159  }
160 
161  /** Make \f$ this = this \ominus b \f$ */
162  inline CPose3DQuat& operator -= (const CPose3DQuat& b)
163  {
164  inverseComposeFrom(*this,b);
165  return *this;
166  }
167 
168  /** Return the composed pose \f$ ret = this \ominus p \f$ */
169  inline CPose3DQuat operator - (const CPose3DQuat& p) const
170  {
171  CPose3DQuat ret;
172  ret.inverseComposeFrom(*this,p);
173  return ret;
174  }
175 
176  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
177  void inverse();
178 
179  /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)
180  * \sa fromString
181  */
182  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]); }
183  inline std::string asString() const { std::string s; asString(s); return s; }
184 
185  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )
186  * \sa asString
187  * \exception std::exception On invalid format
188  */
189  void fromString(const std::string &s) {
191  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
192  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
193  m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
194  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);
195  }
196 
197  /** Read only [] operator */
198  inline const double &operator[](unsigned int i) const
199  {
200  switch(i)
201  {
202  case 0:return m_coords[0];
203  case 1:return m_coords[1];
204  case 2:return m_coords[2];
205  case 3:return m_quat[0];
206  case 4:return m_quat[1];
207  case 5:return m_quat[2];
208  case 6:return m_quat[3];
209  default:
210  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
211  }
212  }
213  /** Read/write [] operator */
214  inline double &operator[](unsigned int i)
215  {
216  switch(i)
217  {
218  case 0:return m_coords[0];
219  case 1:return m_coords[1];
220  case 2:return m_coords[2];
221  case 3:return m_quat[0];
222  case 4:return m_quat[1];
223  case 5:return m_quat[2];
224  case 6:return m_quat[3];
225  default:
226  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
227  }
228  }
229 
230  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
231  * For the coordinate system see the top of this page.
232  * 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.
233  */
234  void sphericalCoordinates(
235  const mrpt::math::TPoint3D &point,
236  double &out_range,
237  double &out_yaw,
238  double &out_pitch,
239  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
240  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
241  ) const;
242 
243  public:
244  typedef CPose3DQuat type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
245  enum { is_3D_val = 1 };
246  static inline bool is_3D() { return is_3D_val!=0; }
247  enum { rotation_dimensions = 3 };
248  enum { is_PDF_val = 1 };
249  static inline bool is_PDF() { return is_PDF_val!=0; }
250 
251  inline const type_value & getPoseMean() const { return *this; }
252  inline type_value & getPoseMean() { return *this; }
253 
254  /** @name STL-like methods and typedefs
255  @{ */
256  typedef double value_type; //!< The type of the elements
257  typedef double& reference;
258  typedef const double& const_reference;
259  typedef std::size_t size_type;
260  typedef std::ptrdiff_t difference_type;
261 
262  // size is constant
263  enum { static_size = 7 };
264  static inline size_type size() { return static_size; }
265  static inline bool empty() { return false; }
266  static inline size_type max_size() { return static_size; }
267  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))); }
268 
269  inline void assign(const size_t N, const double val)
270  {
271  if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
272  m_coords.fill(val);
273  m_quat.fill(val);
274  }
275 
276  struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
277  {
278  private:
279  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
280  CPose3DQuat *m_obj; //!< A reference to the source of this iterator
281  size_t m_cur_idx; //!< The iterator points to this element.
282  typedef value_type T; //!< The type of the matrix elements
283 
284  inline void check_limits(bool allow_end = false) const
285  {
286  #ifdef _DEBUG
287  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
288  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
289  #else
290  MRPT_UNUSED_PARAM(allow_end);
291  #endif
292  }
293  public:
294  inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
295  inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
296  inline iterator() : m_obj(NULL),m_cur_idx(0) { }
297  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()*/ }
298  inline CPose3DQuat::reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
299  inline iterator &operator++() {
300  check_limits();
301  ++m_cur_idx;
302  return *this;
303  }
304  inline iterator operator++(int) {
305  iterator it=*this;
306  ++*this;
307  return it;
308  }
309  inline iterator &operator--() {
310  --m_cur_idx;
311  check_limits();
312  return *this;
313  }
314  inline iterator operator--(int) {
315  iterator it=*this;
316  --*this;
317  return it;
318  }
319  inline iterator &operator+=(iterator_base::difference_type off) {
320  m_cur_idx+=off;
321  check_limits(true);
322  return *this;
323  }
324  inline iterator operator+(iterator_base::difference_type off) const {
325  iterator it=*this;
326  it+=off;
327  return it;
328  }
329  inline iterator &operator-=(iterator_base::difference_type off) {
330  return (*this)+=(-off);
331  }
332  inline iterator operator-(iterator_base::difference_type off) const {
333  iterator it=*this;
334  it-=off;
335  return it;
336  }
337  inline iterator_base::difference_type operator-(const iterator &it) const { return m_cur_idx - it.m_cur_idx; }
338  inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
339  inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
340  inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
341  }; // end iterator
342 
343  struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
344  {
345  private:
346  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
347  const CPose3DQuat *m_obj; //!< A reference to the source of this iterator
348  size_t m_cur_idx; //!< The iterator points to this element.
349  typedef value_type T; //!< The type of the matrix elements
350 
351  inline void check_limits(bool allow_end = false) const
352  {
353  #ifdef _DEBUG
354  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
355  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
356  #else
357  MRPT_UNUSED_PARAM(allow_end);
358  #endif
359  }
360  public:
361  inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
362  inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
363  inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
364  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()*/ }
365  inline CPose3DQuat::const_reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
367  check_limits();
368  ++m_cur_idx;
369  return *this;
370  }
372  const_iterator it=*this;
373  ++*this;
374  return it;
375  }
377  --m_cur_idx;
378  check_limits();
379  return *this;
380  }
382  const_iterator it=*this;
383  --*this;
384  return it;
385  }
386  inline const_iterator &operator+=(iterator_base::difference_type off) {
387  m_cur_idx+=off;
388  check_limits(true);
389  return *this;
390  }
391  inline const_iterator operator+(iterator_base::difference_type off) const {
392  const_iterator it=*this;
393  it+=off;
394  return it;
395  }
396  inline const_iterator &operator-=(iterator_base::difference_type off) {
397  return (*this)+=(-off);
398  }
399  inline const_iterator operator-(iterator_base::difference_type off) const {
400  const_iterator it=*this;
401  it-=off;
402  return it;
403  }
404  inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
405  inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
406  inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
407  inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
408  }; // end const_iterator
409 
410  typedef std::reverse_iterator<iterator> reverse_iterator;
411  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
412  inline iterator begin() { return iterator(*this,0); }
413  inline iterator end() { return iterator(*this,static_size); }
414  inline const_iterator begin() const { return const_iterator(*this,0); }
415  inline const_iterator end() const { return const_iterator(*this,static_size); }
416  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
417  inline const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
418  inline reverse_iterator rend() { return reverse_iterator(begin()); }
419  inline const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
420 
421 
422  void swap (CPose3DQuat& o)
423  {
424  std::swap(o.m_coords, m_coords);
425  o.m_quat.swap(m_quat);
426  }
427 
428  /** @} */
429  //! See ops_containers.h
430  typedef CPose3DQuat mrpt_autotype;
431  //DECLARE_MRPT_CONTAINER_TYPES
432 
433  void setToNaN();
434 
435  }; // End of class def.
437 
438  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DQuat& p);
439 
440  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with all its arguments multiplied by "-1") */
441  CPose3DQuat BASE_IMPEXP operator -(const CPose3DQuat &p);
442 
443 
444 
445  } // End of namespace
446 } // End of namespace
447 
448 #endif
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:280
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:282
const double & const_reference
Definition: CPose3DQuat.h:258
static size_type max_size()
Definition: CPose3DQuat.h:266
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:281
const_iterator end() const
Definition: CPose3DQuat.h:415
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
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:269
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
Definition: CPose3DQuat.h:57
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:298
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...
reverse_iterator rbegin()
Definition: CPose3DQuat.h:416
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:337
TConstructorFlags_Quaternions
Definition: CQuaternion.h:21
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:407
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:297
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
static size_type size()
Definition: CPose3DQuat.h:264
#define THROW_EXCEPTION(msg)
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:417
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:189
Scalar * iterator
Definition: eigen_plugins.h:23
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:77
size_t size(const MATRIXLIKE &m, int dim)
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:279
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
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:338
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:407
bool operator>(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:291
STL namespace.
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:351
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
Definition: CPose3DQuat.h:59
const Scalar * const_iterator
Definition: eigen_plugins.h:24
reverse_iterator rend()
Definition: CPose3DQuat.h:418
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:365
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:319
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:404
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:340
A numeric matrix of compile-time fixed size.
const_iterator begin() const
Definition: CPose3DQuat.h:414
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
Definition: CPose3DQuat.h:91
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:396
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:391
class BASE_IMPEXP CSerializable
Definition: CStream.h:23
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:54
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.
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.
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:399
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:347
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:25
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:251
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
double value_type
The type of the elements.
Definition: CPose3DQuat.h:256
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:339
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:214
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.
iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:332
class BASE_IMPEXP CPose3DQuat
Definition: CPose3D.h:23
A class used to store a 3D point.
Definition: CPoint3D.h:32
type_value & getPoseMean()
Definition: CPose3DQuat.h:252
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
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:128
CPose3DQuat(TConstructorFlags_Poses)
Definition: CPose3DQuat.h:68
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:125
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...
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...
CPose3DQuat mrpt_autotype
See ops_containers.h.
Definition: CPose3DQuat.h:430
static void resize(const size_t n)
Definition: CPose3DQuat.h:267
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&#39;s unitary...
Definition: CPose3DQuat.h:71
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:346
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:260
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:324
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:329
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:419
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:386
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:182
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:63
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
Definition: CPoint.h:116
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:284
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:406
#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:410
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:198
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:42
Lightweight 3D point.
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...
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose3DQuat.h:244
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:411
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:348
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:349
#define ASSERTMSG_(f, __ERROR_MSG)
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:364
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
Definition: CPose3DQuat.h:183
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:405
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:422



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:46:27 UTC 2016