MRPT  1.9.9
CPose3DQuat.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/math/CQuaternion.h>
13 #include <mrpt/math/TPose3DQuat.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose.h>
16 #include <mrpt/poses/poses_frwds.h>
17 
18 namespace mrpt::poses
19 {
20 /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion
21  * (qr,qx,qy,qz).
22  *
23  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint,
24  * or refer
25  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry
26  * tutorial</a> in the wiki.
27  *
28  * To access the translation use x(), y() and z(). To access the rotation, use
29  * CPose3DQuat::quat().
30  *
31  * This class also behaves like a STL container, since it has begin(), end(),
32  * 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
34  * they were a vector. Thus, a CPose3DQuat can be used
35  * as a 7-vector anywhere the MRPT math functions expect any kind of vector.
36  *
37  * This class and CPose3D are very similar, and they can be converted to the
38  * each other automatically via transformation constructors.
39  *
40  * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion),
41  * mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic
42  * version of this class, mrpt::math::CQuaternion, CPoseOrPoint
43  * \ingroup poses_grp
44  */
45 class CPose3DQuat : public CPose<CPose3DQuat, 7>,
47 {
50  public:
51  /** The translation vector [x,y,z] */
53  /** The quaternion. */
55 
56  public:
57  /** Read/Write access to the quaternion representing the 3D rotation. */
58  inline mrpt::math::CQuaternionDouble& quat() { return m_quat; }
59  /** Read-only access to the quaternion representing the 3D rotation. */
60  inline const mrpt::math::CQuaternionDouble& quat() const { return m_quat; }
61  /** Read/Write access to the translation vector in R^3. */
63  /** Read-only access to the translation vector in R^3. */
64  inline const mrpt::math::CVectorFixedDouble<3>& xyz() const
65  {
66  return m_coords;
67  }
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 7x1 vector with [x y z qr qx qy qz]' */
125  void asVector(vector_t& v) const;
126 
127  /** Makes \f$ this = A \oplus B \f$ this method is slightly more efficient
128  * than "this= A + B;" since it avoids the temporary object.
129  * \note A or B can be "this" without problems.
130  * \sa inverseComposeFrom, composePoint
131  */
132  void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B);
133 
134  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
135  * than "this= A - B;" since it avoids the temporary object.
136  * \note A or B can be "this" without problems.
137  * \sa composeFrom, composePoint
138  */
139  void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B);
140 
141  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
142  * \sa composeFrom, inverseComposePoint
143  */
144  void composePoint(
145  const double lx, const double ly, const double lz, double& gx,
146  double& gy, double& gz,
147  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint =
148  nullptr,
149  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose =
150  nullptr) const;
151 
152  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
153  * \sa composePoint, composeFrom
154  */
155  void inverseComposePoint(
156  const double gx, const double gy, const double gz, double& lx,
157  double& ly, double& lz,
158  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint =
159  nullptr,
160  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose =
161  nullptr) const;
162 
163  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
164  * POINT1 and POINT1 can be anything supporing [0],[1],[2].
165  * \sa composePoint */
166  template <class POINT1, class POINT2>
167  inline void composePoint(const POINT1& L, POINT2& G) const
168  {
169  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
170  }
171 
172  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
173  * inverseComposePoint */
174  template <class POINT1, class POINT2>
175  inline void inverseComposePoint(const POINT1& G, POINT2& L) const
176  {
177  inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
178  }
179 
180  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa
181  * composePoint */
182  inline CPoint3D operator+(const CPoint3D& L) const
183  {
184  CPoint3D G;
185  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
186  return G;
187  }
188 
189  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa
190  * composePoint */
192  {
194  composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
195  return G;
196  }
197 
198  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by
199  * the scalar). */
200  virtual void operator*=(const double s);
201 
202  /** Make \f$ this = this \oplus b \f$ */
204  {
205  composeFrom(*this, b);
206  return *this;
207  }
208 
209  /** Return the composed pose \f$ ret = this \oplus p \f$ */
210  inline CPose3DQuat operator+(const CPose3DQuat& p) const
211  {
212  CPose3DQuat ret;
213  ret.composeFrom(*this, p);
214  return ret;
215  }
216 
217  /** Make \f$ this = this \ominus b \f$ */
219  {
220  inverseComposeFrom(*this, b);
221  return *this;
222  }
223 
224  /** Return the composed pose \f$ ret = this \ominus p \f$ */
225  inline CPose3DQuat operator-(const CPose3DQuat& p) const
226  {
227  CPose3DQuat ret;
228  ret.inverseComposeFrom(*this, p);
229  return ret;
230  }
231 
232  /** Convert this pose into its inverse, saving the result in itself. \sa
233  * operator- */
234  void inverse();
235 
236  /** Returns a human-readable textual representation of the object (eg: "[x y
237  * z qr qx qy qz]", angles in degrees.)
238  * \sa fromString
239  */
240  void asString(std::string& s) const
241  {
242  s = mrpt::format(
243  "[%f %f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2],
244  m_quat[0], m_quat[1], m_quat[2], m_quat[3]);
245  }
246  inline std::string asString() const
247  {
248  std::string s;
249  asString(s);
250  return s;
251  }
252 
253  /** Set the current object value from a string generated by 'asString' (eg:
254  * "[0.02 1.04 -0.8 1 0 0 0]" )
255  * \sa asString
256  * \exception std::exception On invalid format
257  */
258  void fromString(const std::string& s);
259 
260  /** Same as fromString, but without requiring the square brackets in the
261  * string */
262  void fromStringRaw(const std::string& s);
263 
265  {
266  CPose3DQuat o;
267  o.fromString(s);
268  return o;
269  }
270 
271  /** Read only [] operator */
272  inline const double& operator[](unsigned int i) const
273  {
274  switch (i)
275  {
276  case 0:
277  return m_coords[0];
278  case 1:
279  return m_coords[1];
280  case 2:
281  return m_coords[2];
282  case 3:
283  return m_quat[0];
284  case 4:
285  return m_quat[1];
286  case 5:
287  return m_quat[2];
288  case 6:
289  return m_quat[3];
290  default:
291  throw std::runtime_error(
292  "CPose3DQuat::operator[]: Index of bounds.");
293  }
294  }
295  /** Read/write [] operator */
296  inline double& operator[](unsigned int i)
297  {
298  switch (i)
299  {
300  case 0:
301  return m_coords[0];
302  case 1:
303  return m_coords[1];
304  case 2:
305  return m_coords[2];
306  case 3:
307  return m_quat[0];
308  case 4:
309  return m_quat[1];
310  case 5:
311  return m_quat[2];
312  case 6:
313  return m_quat[3];
314  default:
315  throw std::runtime_error(
316  "CPose3DQuat::operator[]: Index of bounds.");
317  }
318  }
319 
320  /** Computes the spherical coordinates of a 3D point as seen from the 6D
321  * pose specified by this object.
322  * For the coordinate system see the top of this page.
323  * If the matrix pointers are not nullptr, the Jacobians will be also
324  * computed for the range-yaw-pitch variables wrt the passed 3D point and
325  * this 7D pose.
326  */
328  const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
329  double& out_pitch,
330  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint = nullptr,
331  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose =
332  nullptr) const;
333 
334  public:
335  /** Used to emulate CPosePDF types, for example, in
336  * mrpt::graphs::CNetworkOfPoses */
338  enum
339  {
341  };
342  static constexpr bool is_3D() { return is_3D_val != 0; }
343  enum
344  {
346  };
347  enum
348  {
350  };
351  static constexpr bool is_PDF() { return is_PDF_val != 0; }
352  inline const type_value& getPoseMean() const { return *this; }
353  inline type_value& getPoseMean() { return *this; }
354  /** @name STL-like methods and typedefs
355  @{ */
356  /** The type of the elements */
357  using value_type = double;
358  using reference = double&;
359  using const_reference = const double&;
360  using size_type = std::size_t;
362 
363  // size is constant
364  enum
365  {
367  };
368  static constexpr size_type size() { return static_size; }
369  static constexpr bool empty() { return false; }
370  static constexpr size_type max_size() { return static_size; }
371  static inline void resize(const size_t n)
372  {
373  if (n != static_size)
374  throw std::logic_error(format(
375  "Try to change the size of CPose3DQuat to %u.",
376  static_cast<unsigned>(n)));
377  }
378 
379  inline void assign(const size_t N, const double val)
380  {
381  if (N != 7)
382  throw std::runtime_error(
383  "CPose3DQuat::assign: Try to resize to length!=7.");
384  m_coords.fill(val);
385  m_quat.fill(val);
386  }
387 
388  struct iterator
389  : public std::iterator<std::random_access_iterator_tag, value_type>
390  {
391  private:
392  using iterator_base =
393  std::iterator<std::random_access_iterator_tag, value_type>;
394  /** A reference to the source of this iterator */
395  CPose3DQuat* m_obj{nullptr};
396  /** The iterator points to this element. */
397  size_t m_cur_idx{0};
398  /** The type of the matrix elements */
399  using T = value_type;
400 
401  inline void check_limits(bool allow_end = false) const
402  {
403 #ifdef _DEBUG
404  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
405  if (m_cur_idx > (allow_end ? 7u : 6u))
406  THROW_EXCEPTION("Index out of range in iterator.");
407 #else
408  MRPT_UNUSED_PARAM(allow_end);
409 #endif
410  }
411 
412  public:
413  inline bool operator<(const iterator& it2) const
414  {
415  return m_cur_idx < it2.m_cur_idx;
416  }
417  inline bool operator>(const iterator& it2) const
418  {
419  return m_cur_idx > it2.m_cur_idx;
420  }
421  inline iterator() = default;
422  inline iterator(CPose3DQuat& obj, size_t start_idx)
423  : m_obj(&obj), m_cur_idx(start_idx)
424  {
425  check_limits(true); /*Dont report as error an iterator to end()*/
426  }
428  {
429  check_limits();
430  return (*m_obj)[m_cur_idx];
431  }
433  {
434  check_limits();
435  ++m_cur_idx;
436  return *this;
437  }
438  inline iterator operator++(int)
439  {
440  iterator it = *this;
441  ++*this;
442  return it;
443  }
445  {
446  --m_cur_idx;
447  check_limits();
448  return *this;
449  }
450  inline iterator operator--(int)
451  {
452  iterator it = *this;
453  --*this;
454  return it;
455  }
456  inline iterator& operator+=(iterator_base::difference_type off)
457  {
458  m_cur_idx += off;
459  check_limits(true);
460  return *this;
461  }
462  inline iterator operator+(iterator_base::difference_type off) const
463  {
464  iterator it = *this;
465  it += off;
466  return it;
467  }
468  inline iterator& operator-=(iterator_base::difference_type off)
469  {
470  return (*this) += (-off);
471  }
472  inline iterator operator-(iterator_base::difference_type off) const
473  {
474  iterator it = *this;
475  it -= off;
476  return it;
477  }
478  inline iterator_base::difference_type operator-(
479  const iterator& it) const
480  {
481  return m_cur_idx - it.m_cur_idx;
482  }
484  iterator_base::difference_type off) const
485  {
486  return (*m_obj)[m_cur_idx + off];
487  }
488  inline bool operator==(const iterator& it) const
489  {
490  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
491  }
492  inline bool operator!=(const iterator& it) const
493  {
494  return !(operator==(it));
495  }
496  }; // end iterator
497 
499  : public std::iterator<std::random_access_iterator_tag, value_type>
500  {
501  private:
502  using iterator_base =
503  std::iterator<std::random_access_iterator_tag, value_type>;
504  /** A reference to the source of this iterator */
505  const CPose3DQuat* m_obj{nullptr};
506  /** The iterator points to this element. */
507  size_t m_cur_idx{0};
508  /** The type of the matrix elements */
509  using T = value_type;
510 
511  inline void check_limits(bool allow_end = false) const
512  {
513 #ifdef _DEBUG
514  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
515  if (m_cur_idx > (allow_end ? 7u : 6u))
516  THROW_EXCEPTION("Index out of range in iterator.");
517 #else
518  MRPT_UNUSED_PARAM(allow_end);
519 #endif
520  }
521 
522  public:
523  inline bool operator<(const const_iterator& it2) const
524  {
525  return m_cur_idx < it2.m_cur_idx;
526  }
527  inline bool operator>(const const_iterator& it2) const
528  {
529  return m_cur_idx > it2.m_cur_idx;
530  }
531  inline const_iterator() = default;
532  inline const_iterator(const CPose3DQuat& obj, size_t start_idx)
533  : m_obj(&obj), m_cur_idx(start_idx)
534  {
535  check_limits(true); /*Dont report as error an iterator to end()*/
536  }
538  {
539  check_limits();
540  return (*m_obj)[m_cur_idx];
541  }
543  {
544  check_limits();
545  ++m_cur_idx;
546  return *this;
547  }
549  {
550  const_iterator it = *this;
551  ++*this;
552  return it;
553  }
555  {
556  --m_cur_idx;
557  check_limits();
558  return *this;
559  }
561  {
562  const_iterator it = *this;
563  --*this;
564  return it;
565  }
566  inline const_iterator& operator+=(iterator_base::difference_type off)
567  {
568  m_cur_idx += off;
569  check_limits(true);
570  return *this;
571  }
573  iterator_base::difference_type off) const
574  {
575  const_iterator it = *this;
576  it += off;
577  return it;
578  }
579  inline const_iterator& operator-=(iterator_base::difference_type off)
580  {
581  return (*this) += (-off);
582  }
584  iterator_base::difference_type off) const
585  {
586  const_iterator it = *this;
587  it -= off;
588  return it;
589  }
590  inline iterator_base::difference_type operator-(
591  const const_iterator& it) const
592  {
593  return m_cur_idx - it.m_cur_idx;
594  }
596  iterator_base::difference_type off) const
597  {
598  return (*m_obj)[m_cur_idx + off];
599  }
600  inline bool operator==(const const_iterator& it) const
601  {
602  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
603  }
604  inline bool operator!=(const const_iterator& it) const
605  {
606  return !(operator==(it));
607  }
608  }; // end const_iterator
609 
610  using reverse_iterator = std::reverse_iterator<iterator>;
611  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
612  inline iterator begin() { return iterator(*this, 0); }
613  inline iterator end() { return iterator(*this, static_size); }
614  inline const_iterator begin() const { return const_iterator(*this, 0); }
615  inline const_iterator end() const
616  {
617  return const_iterator(*this, static_size);
618  }
619  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
621  {
622  return const_reverse_iterator(end());
623  }
624  inline reverse_iterator rend() { return reverse_iterator(begin()); }
626  {
627  return const_reverse_iterator(begin());
628  }
629 
630  void swap(CPose3DQuat& o)
631  {
632  std::swap(o.m_coords, m_coords);
633  o.m_quat.swap(m_quat);
634  }
635 
636  /** @} */
637 
638  void setToNaN() override;
639 
640 }; // End of class def.
641 
642 std::ostream& operator<<(std::ostream& o, const CPose3DQuat& p);
643 
644 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
645  * than a pose with all its arguments multiplied by "-1") */
646 CPose3DQuat operator-(const CPose3DQuat& p);
647 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
648  * inverseComposePoint */
649 CPoint3D operator-(const CPoint3D& G, const CPose3DQuat& p);
650 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
651  * inverseComposePoint */
653  const mrpt::math::TPoint3D& G, const CPose3DQuat& p);
654 
655 bool operator==(const CPose3DQuat& p1, const CPose3DQuat& p2);
656 bool operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2);
657 
658 } // namespace mrpt::poses
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:395
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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...
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:397
const_iterator end() const
Definition: CPose3DQuat.h:615
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:275
GLdouble GLdouble z
Definition: glext.h:3879
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:503
void asVector(vector_t &v) const
Returns a 7x1 vector with [x y z qr qx qy qz]&#39;.
Definition: CPose3DQuat.cpp:64
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:379
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:427
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:611
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
reverse_iterator rbegin()
Definition: CPose3DQuat.h:619
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:478
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
static constexpr bool empty()
Definition: CPose3DQuat.h:369
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:604
const double G
void fill(const Scalar &val)
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:422
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:620
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
GLenum GLsizei n
Definition: glext.h:5136
bool operator<(const iterator &it2) const
Definition: CPose3DQuat.h:413
mrpt::math::TPoint3D operator+(const mrpt::math::TPoint3D &L) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:191
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:105
const double & const_reference
Definition: CPose3DQuat.h:359
mrpt::math::CVectorFixedDouble< DIM > vector_t
Fixed-size vector of the correct size to hold all the coordinates of the point/pose.
Definition: CPoseOrPoint.h:137
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:483
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:505
mrpt::math::TPose3DQuat asTPose() const
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixed< 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:511
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:393
void setToNaN() override
Set all data fields to quiet NaN.
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
bool operator>(const iterator &it2) const
Definition: CPose3DQuat.h:417
reverse_iterator rend()
Definition: CPose3DQuat.h:624
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:537
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:456
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
const mrpt::math::CVectorFixedDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
Definition: CPose3DQuat.h:64
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:590
static CPose3DQuat FromString(const std::string &s)
Definition: CPose3DQuat.h:264
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:492
const_iterator begin() const
Definition: CPose3DQuat.h:614
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:579
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:509
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:79
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:572
static constexpr size_type max_size()
Definition: CPose3DQuat.h:370
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:60
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:399
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:356
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:583
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:505
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:24
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:352
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:488
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:296
double value_type
The type of the elements.
Definition: CPose3DQuat.h:357
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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:472
GLsizei const GLchar ** string
Definition: glext.h:4116
A class used to store a 3D point.
Definition: CPoint3D.h:31
type_value & getPoseMean()
Definition: CPose3DQuat.h:353
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
_W64 int ptrdiff_t
Definition: glew.h:136
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:175
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:128
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:167
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
bool operator<(const const_iterator &it2) const
Definition: CPose3DQuat.h:523
mrpt::math::CVectorFixedDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
Definition: CPose3DQuat.h:62
static void resize(const size_t n)
Definition: CPose3DQuat.h:371
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:85
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:53
bool operator>(const const_iterator &it2) const
Definition: CPose3DQuat.h:527
CPoint3D operator+(const CPoint3D &L) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:182
CPose3DQuat operator-(const CPose3DQuat &p) const
Return the composed pose .
Definition: CPose3DQuat.h:225
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:462
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:468
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:625
CVectorFixed< double, N > CVectorFixedDouble
Specialization of CVectorFixed for double numbers.
Definition: CVectorFixed.h:32
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:566
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:52
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:240
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:70
static constexpr bool is_PDF()
Definition: CPose3DQuat.h:351
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:98
GLenum GLint GLint y
Definition: glext.h:3542
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:401
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:600
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:54
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:272
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:361
GLenum GLint x
Definition: glext.h:3542
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
Lightweight 3D point.
Definition: TPoint3D.h:90
void swap(CMatrixFixed &o)
Definition: CMatrixFixed.h:197
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:507
CPose3DQuat operator+(const CPose3DQuat &p) const
Return the composed pose .
Definition: CPose3DQuat.h:210
GLfloat GLfloat p
Definition: glext.h:6398
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:532
CPose3DQuat & operator+=(const CPose3DQuat &b)
Make .
Definition: CPose3DQuat.h:203
static constexpr bool is_3D()
Definition: CPose3DQuat.h:342
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:610
std::string asString() const
Definition: CPose3DQuat.h:246
static constexpr size_type size()
Definition: CPose3DQuat.h:368
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:595
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:630
#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:218



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019