Main MRPT website > C++ reference for MRPT 1.9.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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef 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 /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion
24  * (qr,qx,qy,qz).
25  *
26  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint,
27  * or refer
28  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry
29  * tutorial</a> in the wiki.
30  *
31  * To access the translation use x(), y() and z(). To access the rotation, use
32  * CPose3DQuat::quat().
33  *
34  * This class also behaves like a STL container, since it has begin(), end(),
35  * iterators, and can be accessed with the [] operator
36  * with indices running from 0 to 6 to access the [x y z qr qx qy qz] as if
37  * they were a vector. Thus, a CPose3DQuat can be used
38  * as a 7-vector anywhere the MRPT math functions expect any kind of vector.
39  *
40  * This class and CPose3D are very similar, and they can be converted to the
41  * each other automatically via transformation constructors.
42  *
43  * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion),
44  * mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic
45  * version of this class, mrpt::math::CQuaternion, CPoseOrPoint
46  * \ingroup poses_grp
47  */
48 class CPose3DQuat : public CPose<CPose3DQuat>,
50 {
52 
53  public:
54  /** The translation vector [x,y,z] */
56  /** The quaternion. */
58 
59  public:
60  /** Read/Write access to the quaternion representing the 3D rotation. */
62  /** Read-only access to the quaternion representing the 3D rotation. */
63  inline const mrpt::math::CQuaternionDouble& quat() const { return m_quat; }
64  /** Read/Write access to the translation vector in R^3. */
66  /** Read-only access to the translation vector in R^3. */
67  inline const mrpt::math::CArrayDouble<3>& xyz() const { return m_coords; }
68  /** Default constructor, initialize translation to zeros and quaternion to
69  * no rotation. */
70  inline CPose3DQuat() : m_quat()
71  {
72  m_coords[0] = m_coords[1] = m_coords[2] = 0.;
73  }
74 
75  /** Constructor which left all the quaternion members un-initialized, for
76  * use when speed is critical; Use UNINITIALIZED_POSE as argument to this
77  * constructor. */
80  {
81  }
82  /** \overload */
85  {
86  }
87 
88  /** Constructor with initilization of the pose - the quaternion is
89  * normalized to make sure it's unitary */
90  inline CPose3DQuat(
91  const double x, const double y, const double z,
93  : m_quat(q)
94  {
95  m_coords[0] = x;
96  m_coords[1] = y;
97  m_coords[2] = z;
98  m_quat.normalize();
99  }
100 
101  /** Constructor from a CPose3D */
102  explicit CPose3DQuat(const CPose3D& p);
103 
104  /** Constructor from lightweight object. */
106  : m_quat(p.qr, p.qx, p.qy, p.qz)
107  {
108  x() = p.x;
109  y() = p.y;
110  z() = p.z;
111  }
112 
114 
115  /** Constructor from a 4x4 homogeneous transformation matrix.
116  */
117  explicit CPose3DQuat(const mrpt::math::CMatrixDouble44& M);
118 
119  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
120  * point(translation) or pose (translation+orientation).
121  * \sa getInverseHomogeneousMatrix
122  */
124  /** Returns a 1x7 vector with [x y z qr qx qy qz] */
126  /// \overload
128  {
129  v[0] = m_coords[0];
130  v[1] = m_coords[1];
131  v[2] = m_coords[2];
132  v[3] = m_quat[0];
133  v[4] = m_quat[1];
134  v[5] = m_quat[2];
135  v[6] = m_quat[3];
136  }
137 
138  /** Makes \f$ this = A \oplus B \f$ this method is slightly more efficient
139  * than "this= A + B;" since it avoids the temporary object.
140  * \note A or B can be "this" without problems.
141  * \sa inverseComposeFrom, composePoint
142  */
143  void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B);
144 
145  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
146  * than "this= A - B;" since it avoids the temporary object.
147  * \note A or B can be "this" without problems.
148  * \sa composeFrom, composePoint
149  */
150  void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B);
151 
152  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
153  * \sa composeFrom, inverseComposePoint
154  */
155  void composePoint(
156  const double lx, const double ly, const double lz, double& gx,
157  double& gy, double& gz,
158  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
159  nullptr,
160  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacobian_df_dpose =
161  nullptr) const;
162 
163  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
164  * \sa composePoint, composeFrom
165  */
166  void inverseComposePoint(
167  const double gx, const double gy, const double gz, double& lx,
168  double& ly, double& lz,
169  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
170  nullptr,
171  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacobian_df_dpose =
172  nullptr) const;
173 
174  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
175  * POINT1 and POINT1 can be anything supporing [0],[1],[2].
176  * \sa composePoint */
177  template <class POINT1, class POINT2>
178  inline void composePoint(const POINT1& L, POINT2& G) const
179  {
180  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
181  }
182 
183  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
184  * inverseComposePoint */
185  template <class POINT1, class POINT2>
186  inline void inverseComposePoint(const POINT1& G, POINT2& L) const
187  {
188  inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
189  }
190 
191  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa
192  * composePoint */
193  inline CPoint3D operator+(const CPoint3D& L) const
194  {
195  CPoint3D G;
196  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
197  return G;
198  }
199 
200  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa
201  * composePoint */
203  {
205  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
206  return G;
207  }
208 
209  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by
210  * the scalar). */
211  virtual void operator*=(const double s);
212 
213  /** Make \f$ this = this \oplus b \f$ */
215  {
216  composeFrom(*this, b);
217  return *this;
218  }
219 
220  /** Return the composed pose \f$ ret = this \oplus p \f$ */
221  inline CPose3DQuat operator+(const CPose3DQuat& p) const
222  {
223  CPose3DQuat ret;
224  ret.composeFrom(*this, p);
225  return ret;
226  }
227 
228  /** Make \f$ this = this \ominus b \f$ */
230  {
231  inverseComposeFrom(*this, b);
232  return *this;
233  }
234 
235  /** Return the composed pose \f$ ret = this \ominus p \f$ */
236  inline CPose3DQuat operator-(const CPose3DQuat& p) const
237  {
238  CPose3DQuat ret;
239  ret.inverseComposeFrom(*this, p);
240  return ret;
241  }
242 
243  /** Convert this pose into its inverse, saving the result in itself. \sa
244  * operator- */
245  void inverse();
246 
247  /** Returns a human-readable textual representation of the object (eg: "[x y
248  * z qr qx qy qz]", angles in degrees.)
249  * \sa fromString
250  */
251  void asString(std::string& s) const
252  {
253  s = mrpt::format(
254  "[%f %f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2],
255  m_quat[0], m_quat[1], m_quat[2], m_quat[3]);
256  }
257  inline std::string asString() const
258  {
259  std::string s;
260  asString(s);
261  return s;
262  }
263 
264  /** Set the current object value from a string generated by 'asString' (eg:
265  * "[0.02 1.04 -0.8 1 0 0 0]" )
266  * \sa asString
267  * \exception std::exception On invalid format
268  */
269  void fromString(const std::string& s)
270  {
272  if (!m.fromMatlabStringFormat(s))
273  THROW_EXCEPTION("Malformed expression in ::fromString");
274  ASSERTMSG_(m.rows() == 1 && m.cols() == 7, "Expected vector length=7");
275  m_coords[0] = m.get_unsafe(0, 0);
276  m_coords[1] = m.get_unsafe(0, 1);
277  m_coords[2] = m.get_unsafe(0, 2);
278  m_quat[0] = m.get_unsafe(0, 3);
279  m_quat[1] = m.get_unsafe(0, 4);
280  m_quat[2] = m.get_unsafe(0, 5);
281  m_quat[3] = m.get_unsafe(0, 6);
282  }
283  /** Same as fromString, but without requiring the square brackets in the
284  * string */
285  void fromStringRaw(const std::string& s);
286 
287  /** Read only [] operator */
288  inline const double& operator[](unsigned int i) const
289  {
290  switch (i)
291  {
292  case 0:
293  return m_coords[0];
294  case 1:
295  return m_coords[1];
296  case 2:
297  return m_coords[2];
298  case 3:
299  return m_quat[0];
300  case 4:
301  return m_quat[1];
302  case 5:
303  return m_quat[2];
304  case 6:
305  return m_quat[3];
306  default:
307  throw std::runtime_error(
308  "CPose3DQuat::operator[]: Index of bounds.");
309  }
310  }
311  /** Read/write [] operator */
312  inline double& operator[](unsigned int i)
313  {
314  switch (i)
315  {
316  case 0:
317  return m_coords[0];
318  case 1:
319  return m_coords[1];
320  case 2:
321  return m_coords[2];
322  case 3:
323  return m_quat[0];
324  case 4:
325  return m_quat[1];
326  case 5:
327  return m_quat[2];
328  case 6:
329  return m_quat[3];
330  default:
331  throw std::runtime_error(
332  "CPose3DQuat::operator[]: Index of bounds.");
333  }
334  }
335 
336  /** Computes the spherical coordinates of a 3D point as seen from the 6D
337  * pose specified by this object.
338  * For the coordinate system see the top of this page.
339  * If the matrix pointers are not nullptr, the Jacobians will be also
340  * computed for the range-yaw-pitch variables wrt the passed 3D point and
341  * this 7D pose.
342  */
344  const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
345  double& out_pitch,
346  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacob_dryp_dpoint =
347  nullptr,
348  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacob_dryp_dpose =
349  nullptr) const;
350 
351  public:
352  /** Used to emulate CPosePDF types, for example, in
353  * mrpt::graphs::CNetworkOfPoses */
355  enum
356  {
358  };
359  static inline bool is_3D() { return is_3D_val != 0; }
360  enum
361  {
363  };
364  enum
365  {
367  };
368  static inline bool is_PDF() { return is_PDF_val != 0; }
369  inline const type_value& getPoseMean() const { return *this; }
370  inline type_value& getPoseMean() { return *this; }
371  /** @name STL-like methods and typedefs
372  @{ */
373  /** The type of the elements */
374  using value_type = double;
375  using reference = double&;
376  using const_reference = const double&;
377  using size_type = std::size_t;
379 
380  // size is constant
381  enum
382  {
384  };
385  static inline size_type size() { return static_size; }
386  static inline bool empty() { return false; }
387  static inline size_type max_size() { return static_size; }
388  static inline void resize(const size_t n)
389  {
390  if (n != static_size)
391  throw std::logic_error(format(
392  "Try to change the size of CPose3DQuat to %u.",
393  static_cast<unsigned>(n)));
394  }
395 
396  inline void assign(const size_t N, const double val)
397  {
398  if (N != 7)
399  throw std::runtime_error(
400  "CPose3DQuat::assign: Try to resize to length!=7.");
401  m_coords.fill(val);
402  m_quat.fill(val);
403  }
404 
405  struct iterator
406  : public std::iterator<std::random_access_iterator_tag, value_type>
407  {
408  private:
409  using iterator_base =
410  std::iterator<std::random_access_iterator_tag, value_type>;
411  /** A reference to the source of this iterator */
413  /** The iterator points to this element. */
414  size_t m_cur_idx;
415  /** The type of the matrix elements */
416  using T = value_type;
417 
418  inline void check_limits(bool allow_end = false) const
419  {
420 #ifdef _DEBUG
421  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
422  if (m_cur_idx > (allow_end ? 7u : 6u))
423  THROW_EXCEPTION("Index out of range in iterator.");
424 #else
425  MRPT_UNUSED_PARAM(allow_end);
426 #endif
427  }
428 
429  public:
430  inline bool operator<(const iterator& it2) const
431  {
432  return m_cur_idx < it2.m_cur_idx;
433  }
434  inline bool operator>(const iterator& it2) const
435  {
436  return m_cur_idx > it2.m_cur_idx;
437  }
438  inline iterator() : m_obj(nullptr), m_cur_idx(0) {}
439  inline iterator(CPose3DQuat& obj, size_t start_idx)
440  : m_obj(&obj), m_cur_idx(start_idx)
441  {
442  check_limits(true); /*Dont report as error an iterator to end()*/
443  }
445  {
446  check_limits();
447  return (*m_obj)[m_cur_idx];
448  }
450  {
451  check_limits();
452  ++m_cur_idx;
453  return *this;
454  }
455  inline iterator operator++(int)
456  {
457  iterator it = *this;
458  ++*this;
459  return it;
460  }
462  {
463  --m_cur_idx;
464  check_limits();
465  return *this;
466  }
467  inline iterator operator--(int)
468  {
469  iterator it = *this;
470  --*this;
471  return it;
472  }
473  inline iterator& operator+=(iterator_base::difference_type off)
474  {
475  m_cur_idx += off;
476  check_limits(true);
477  return *this;
478  }
479  inline iterator operator+(iterator_base::difference_type off) const
480  {
481  iterator it = *this;
482  it += off;
483  return it;
484  }
485  inline iterator& operator-=(iterator_base::difference_type off)
486  {
487  return (*this) += (-off);
488  }
489  inline iterator operator-(iterator_base::difference_type off) const
490  {
491  iterator it = *this;
492  it -= off;
493  return it;
494  }
495  inline iterator_base::difference_type operator-(
496  const iterator& it) const
497  {
498  return m_cur_idx - it.m_cur_idx;
499  }
501  iterator_base::difference_type off) const
502  {
503  return (*m_obj)[m_cur_idx + off];
504  }
505  inline bool operator==(const iterator& it) const
506  {
507  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
508  }
509  inline bool operator!=(const iterator& it) const
510  {
511  return !(operator==(it));
512  }
513  }; // end iterator
514 
516  : public std::iterator<std::random_access_iterator_tag, value_type>
517  {
518  private:
519  using iterator_base =
520  std::iterator<std::random_access_iterator_tag, value_type>;
521  /** A reference to the source of this iterator */
523  /** The iterator points to this element. */
524  size_t m_cur_idx;
525  /** The type of the matrix elements */
526  using T = value_type;
527 
528  inline void check_limits(bool allow_end = false) const
529  {
530 #ifdef _DEBUG
531  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
532  if (m_cur_idx > (allow_end ? 7u : 6u))
533  THROW_EXCEPTION("Index out of range in iterator.");
534 #else
535  MRPT_UNUSED_PARAM(allow_end);
536 #endif
537  }
538 
539  public:
540  inline bool operator<(const const_iterator& it2) const
541  {
542  return m_cur_idx < it2.m_cur_idx;
543  }
544  inline bool operator>(const const_iterator& it2) const
545  {
546  return m_cur_idx > it2.m_cur_idx;
547  }
548  inline const_iterator() : m_obj(nullptr), m_cur_idx(0) {}
549  inline const_iterator(const CPose3DQuat& obj, size_t start_idx)
550  : m_obj(&obj), m_cur_idx(start_idx)
551  {
552  check_limits(true); /*Dont report as error an iterator to end()*/
553  }
555  {
556  check_limits();
557  return (*m_obj)[m_cur_idx];
558  }
560  {
561  check_limits();
562  ++m_cur_idx;
563  return *this;
564  }
566  {
567  const_iterator it = *this;
568  ++*this;
569  return it;
570  }
572  {
573  --m_cur_idx;
574  check_limits();
575  return *this;
576  }
578  {
579  const_iterator it = *this;
580  --*this;
581  return it;
582  }
583  inline const_iterator& operator+=(iterator_base::difference_type off)
584  {
585  m_cur_idx += off;
586  check_limits(true);
587  return *this;
588  }
590  iterator_base::difference_type off) const
591  {
592  const_iterator it = *this;
593  it += off;
594  return it;
595  }
596  inline const_iterator& operator-=(iterator_base::difference_type off)
597  {
598  return (*this) += (-off);
599  }
601  iterator_base::difference_type off) const
602  {
603  const_iterator it = *this;
604  it -= off;
605  return it;
606  }
607  inline iterator_base::difference_type operator-(
608  const const_iterator& it) const
609  {
610  return m_cur_idx - it.m_cur_idx;
611  }
613  iterator_base::difference_type off) const
614  {
615  return (*m_obj)[m_cur_idx + off];
616  }
617  inline bool operator==(const const_iterator& it) const
618  {
619  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
620  }
621  inline bool operator!=(const const_iterator& it) const
622  {
623  return !(operator==(it));
624  }
625  }; // end const_iterator
626 
627  using reverse_iterator = std::reverse_iterator<iterator>;
628  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
629  inline iterator begin() { return iterator(*this, 0); }
630  inline iterator end() { return iterator(*this, static_size); }
631  inline const_iterator begin() const { return const_iterator(*this, 0); }
632  inline const_iterator end() const
633  {
634  return const_iterator(*this, static_size);
635  }
636  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
638  {
639  return const_reverse_iterator(end());
640  }
641  inline reverse_iterator rend() { return reverse_iterator(begin()); }
643  {
644  return const_reverse_iterator(begin());
645  }
646 
647  void swap(CPose3DQuat& o)
648  {
649  std::swap(o.m_coords, m_coords);
650  o.m_quat.swap(m_quat);
651  }
652 
653  /** @} */
654  //! See ops_containers.h
656  // DECLARE_MRPT_CONTAINER_TYPES
657 
658  void setToNaN() override;
659 
660 }; // End of class def.
661 
662 std::ostream& operator<<(std::ostream& o, const CPose3DQuat& p);
663 
664 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
665  * than a pose with all its arguments multiplied by "-1") */
667 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
668  * inverseComposePoint */
669 CPoint3D operator-(const CPoint3D& G, const CPose3DQuat& p);
670 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
671  * inverseComposePoint */
673  const mrpt::math::TPoint3D& G, const CPose3DQuat& p);
674 
675 bool operator==(const CPose3DQuat& p1, const CPose3DQuat& p2);
676 bool operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2);
677 
678 } // namespace poses
679 } // namespace mrpt
680 
681 #endif
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:412
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x7 vector with [x y z qr qx qy qz].
Definition: CPose3DQuat.cpp:61
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:61
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
static size_type max_size()
Definition: CPose3DQuat.h:387
Scalar * iterator
Definition: eigen_plugins.h:26
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:414
const_iterator end() const
Definition: CPose3DQuat.h:632
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:272
GLdouble GLdouble z
Definition: glext.h:3872
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:520
CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)
Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...
Definition: CPose3DQuat.h:78
void assign(const size_t N, const double val)
Definition: CPose3DQuat.h:396
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
Definition: CPose3DQuat.h:65
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:444
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:628
reverse_iterator rbegin()
Definition: CPose3DQuat.h:636
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:495
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:621
const double G
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:439
static size_type size()
Definition: CPose3DQuat.h:385
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:637
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
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:269
GLenum GLsizei n
Definition: glext.h:5074
bool operator<(const iterator &it2) const
Definition: CPose3DQuat.h:430
mrpt::math::TPoint3D operator+(const mrpt::math::TPoint3D &L) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:202
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:105
const double & const_reference
Definition: CPose3DQuat.h:376
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:500
mrpt::math::TPose3DQuat asTPose() const
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacob_dryp_dpose=nullptr) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:528
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
Definition: CPose3DQuat.h:67
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:410
void setToNaN() override
Set all data fields to quiet NaN.
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
bool operator>(const iterator &it2) const
Definition: CPose3DQuat.h:434
reverse_iterator rend()
Definition: CPose3DQuat.h:641
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:554
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:473
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:607
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:509
A numeric matrix of compile-time fixed size.
const_iterator begin() const
Definition: CPose3DQuat.h:631
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
Definition: CPose3DQuat.h:127
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:596
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:526
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:77
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:589
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:63
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:174
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:416
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Definition: CPose2D.cpp:315
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:600
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:522
void inverse()
Convert this pose into its inverse, saving the result in itself.
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:27
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:369
int val
Definition: mrpt_jpeglib.h:955
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:505
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:312
double value_type
The type of the elements.
Definition: CPose3DQuat.h:374
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
virtual void operator*=(const double s)
Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:489
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:33
type_value & getPoseMean()
Definition: CPose3DQuat.h:370
_W64 int ptrdiff_t
Definition: glew.h:137
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:55
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:186
CPose3DQuat(TConstructorFlags_Poses)
Definition: CPose3DQuat.h:83
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:178
const GLdouble * v
Definition: glext.h:3678
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...
bool operator<(const const_iterator &it2) const
Definition: CPose3DQuat.h:540
static void resize(const size_t n)
Definition: CPose3DQuat.h:388
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
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:90
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3DQuat.cpp:49
bool operator>(const const_iterator &it2) const
Definition: CPose3DQuat.h:544
CPoint3D operator+(const CPoint3D &L) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:193
CPose3DQuat operator-(const CPose3DQuat &p) const
Return the composed pose .
Definition: CPose3DQuat.h:236
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:479
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:485
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:642
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:583
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
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:251
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:70
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:96
GLenum GLint GLint y
Definition: glext.h:3538
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:418
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:617
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:57
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:288
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:378
GLenum GLint x
Definition: glext.h:3538
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:46
Lightweight 3D point.
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:524
CPose3DQuat operator+(const CPose3DQuat &p) const
Return the composed pose .
Definition: CPose3DQuat.h:221
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:549
CPose3DQuat & operator+=(const CPose3DQuat &b)
Make .
Definition: CPose3DQuat.h:214
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:627
std::string asString() const
Definition: CPose3DQuat.h:257
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:612
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:647
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:140
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
CPose3DQuat & operator-=(const CPose3DQuat &b)
Make .
Definition: CPose3DQuat.h:229



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019