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 
264  /** Read only [] operator */
265  inline const double& operator[](unsigned int i) const
266  {
267  switch (i)
268  {
269  case 0:
270  return m_coords[0];
271  case 1:
272  return m_coords[1];
273  case 2:
274  return m_coords[2];
275  case 3:
276  return m_quat[0];
277  case 4:
278  return m_quat[1];
279  case 5:
280  return m_quat[2];
281  case 6:
282  return m_quat[3];
283  default:
284  throw std::runtime_error(
285  "CPose3DQuat::operator[]: Index of bounds.");
286  }
287  }
288  /** Read/write [] operator */
289  inline double& operator[](unsigned int i)
290  {
291  switch (i)
292  {
293  case 0:
294  return m_coords[0];
295  case 1:
296  return m_coords[1];
297  case 2:
298  return m_coords[2];
299  case 3:
300  return m_quat[0];
301  case 4:
302  return m_quat[1];
303  case 5:
304  return m_quat[2];
305  case 6:
306  return m_quat[3];
307  default:
308  throw std::runtime_error(
309  "CPose3DQuat::operator[]: Index of bounds.");
310  }
311  }
312 
313  /** Computes the spherical coordinates of a 3D point as seen from the 6D
314  * pose specified by this object.
315  * For the coordinate system see the top of this page.
316  * If the matrix pointers are not nullptr, the Jacobians will be also
317  * computed for the range-yaw-pitch variables wrt the passed 3D point and
318  * this 7D pose.
319  */
321  const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
322  double& out_pitch,
323  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint = nullptr,
324  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose =
325  nullptr) const;
326 
327  public:
328  /** Used to emulate CPosePDF types, for example, in
329  * mrpt::graphs::CNetworkOfPoses */
331  enum
332  {
334  };
335  static constexpr bool is_3D() { return is_3D_val != 0; }
336  enum
337  {
339  };
340  enum
341  {
343  };
344  static constexpr bool is_PDF() { return is_PDF_val != 0; }
345  inline const type_value& getPoseMean() const { return *this; }
346  inline type_value& getPoseMean() { return *this; }
347  /** @name STL-like methods and typedefs
348  @{ */
349  /** The type of the elements */
350  using value_type = double;
351  using reference = double&;
352  using const_reference = const double&;
353  using size_type = std::size_t;
355 
356  // size is constant
357  enum
358  {
360  };
361  static constexpr size_type size() { return static_size; }
362  static constexpr bool empty() { return false; }
363  static constexpr size_type max_size() { return static_size; }
364  static inline void resize(const size_t n)
365  {
366  if (n != static_size)
367  throw std::logic_error(format(
368  "Try to change the size of CPose3DQuat to %u.",
369  static_cast<unsigned>(n)));
370  }
371 
372  inline void assign(const size_t N, const double val)
373  {
374  if (N != 7)
375  throw std::runtime_error(
376  "CPose3DQuat::assign: Try to resize to length!=7.");
377  m_coords.fill(val);
378  m_quat.fill(val);
379  }
380 
381  struct iterator
382  : public std::iterator<std::random_access_iterator_tag, value_type>
383  {
384  private:
385  using iterator_base =
386  std::iterator<std::random_access_iterator_tag, value_type>;
387  /** A reference to the source of this iterator */
388  CPose3DQuat* m_obj{nullptr};
389  /** The iterator points to this element. */
390  size_t m_cur_idx{0};
391  /** The type of the matrix elements */
392  using T = value_type;
393 
394  inline void check_limits(bool allow_end = false) const
395  {
396 #ifdef _DEBUG
397  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
398  if (m_cur_idx > (allow_end ? 7u : 6u))
399  THROW_EXCEPTION("Index out of range in iterator.");
400 #else
401  MRPT_UNUSED_PARAM(allow_end);
402 #endif
403  }
404 
405  public:
406  inline bool operator<(const iterator& it2) const
407  {
408  return m_cur_idx < it2.m_cur_idx;
409  }
410  inline bool operator>(const iterator& it2) const
411  {
412  return m_cur_idx > it2.m_cur_idx;
413  }
414  inline iterator() = default;
415  inline iterator(CPose3DQuat& obj, size_t start_idx)
416  : m_obj(&obj), m_cur_idx(start_idx)
417  {
418  check_limits(true); /*Dont report as error an iterator to end()*/
419  }
421  {
422  check_limits();
423  return (*m_obj)[m_cur_idx];
424  }
426  {
427  check_limits();
428  ++m_cur_idx;
429  return *this;
430  }
431  inline iterator operator++(int)
432  {
433  iterator it = *this;
434  ++*this;
435  return it;
436  }
438  {
439  --m_cur_idx;
440  check_limits();
441  return *this;
442  }
443  inline iterator operator--(int)
444  {
445  iterator it = *this;
446  --*this;
447  return it;
448  }
449  inline iterator& operator+=(iterator_base::difference_type off)
450  {
451  m_cur_idx += off;
452  check_limits(true);
453  return *this;
454  }
455  inline iterator operator+(iterator_base::difference_type off) const
456  {
457  iterator it = *this;
458  it += off;
459  return it;
460  }
461  inline iterator& operator-=(iterator_base::difference_type off)
462  {
463  return (*this) += (-off);
464  }
465  inline iterator operator-(iterator_base::difference_type off) const
466  {
467  iterator it = *this;
468  it -= off;
469  return it;
470  }
471  inline iterator_base::difference_type operator-(
472  const iterator& it) const
473  {
474  return m_cur_idx - it.m_cur_idx;
475  }
477  iterator_base::difference_type off) const
478  {
479  return (*m_obj)[m_cur_idx + off];
480  }
481  inline bool operator==(const iterator& it) const
482  {
483  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
484  }
485  inline bool operator!=(const iterator& it) const
486  {
487  return !(operator==(it));
488  }
489  }; // end iterator
490 
492  : public std::iterator<std::random_access_iterator_tag, value_type>
493  {
494  private:
495  using iterator_base =
496  std::iterator<std::random_access_iterator_tag, value_type>;
497  /** A reference to the source of this iterator */
498  const CPose3DQuat* m_obj{nullptr};
499  /** The iterator points to this element. */
500  size_t m_cur_idx{0};
501  /** The type of the matrix elements */
502  using T = value_type;
503 
504  inline void check_limits(bool allow_end = false) const
505  {
506 #ifdef _DEBUG
507  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
508  if (m_cur_idx > (allow_end ? 7u : 6u))
509  THROW_EXCEPTION("Index out of range in iterator.");
510 #else
511  MRPT_UNUSED_PARAM(allow_end);
512 #endif
513  }
514 
515  public:
516  inline bool operator<(const const_iterator& it2) const
517  {
518  return m_cur_idx < it2.m_cur_idx;
519  }
520  inline bool operator>(const const_iterator& it2) const
521  {
522  return m_cur_idx > it2.m_cur_idx;
523  }
524  inline const_iterator() = default;
525  inline const_iterator(const CPose3DQuat& obj, size_t start_idx)
526  : m_obj(&obj), m_cur_idx(start_idx)
527  {
528  check_limits(true); /*Dont report as error an iterator to end()*/
529  }
531  {
532  check_limits();
533  return (*m_obj)[m_cur_idx];
534  }
536  {
537  check_limits();
538  ++m_cur_idx;
539  return *this;
540  }
542  {
543  const_iterator it = *this;
544  ++*this;
545  return it;
546  }
548  {
549  --m_cur_idx;
550  check_limits();
551  return *this;
552  }
554  {
555  const_iterator it = *this;
556  --*this;
557  return it;
558  }
559  inline const_iterator& operator+=(iterator_base::difference_type off)
560  {
561  m_cur_idx += off;
562  check_limits(true);
563  return *this;
564  }
566  iterator_base::difference_type off) const
567  {
568  const_iterator it = *this;
569  it += off;
570  return it;
571  }
572  inline const_iterator& operator-=(iterator_base::difference_type off)
573  {
574  return (*this) += (-off);
575  }
577  iterator_base::difference_type off) const
578  {
579  const_iterator it = *this;
580  it -= off;
581  return it;
582  }
583  inline iterator_base::difference_type operator-(
584  const const_iterator& it) const
585  {
586  return m_cur_idx - it.m_cur_idx;
587  }
589  iterator_base::difference_type off) const
590  {
591  return (*m_obj)[m_cur_idx + off];
592  }
593  inline bool operator==(const const_iterator& it) const
594  {
595  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
596  }
597  inline bool operator!=(const const_iterator& it) const
598  {
599  return !(operator==(it));
600  }
601  }; // end const_iterator
602 
603  using reverse_iterator = std::reverse_iterator<iterator>;
604  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
605  inline iterator begin() { return iterator(*this, 0); }
606  inline iterator end() { return iterator(*this, static_size); }
607  inline const_iterator begin() const { return const_iterator(*this, 0); }
608  inline const_iterator end() const
609  {
610  return const_iterator(*this, static_size);
611  }
612  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
614  {
615  return const_reverse_iterator(end());
616  }
617  inline reverse_iterator rend() { return reverse_iterator(begin()); }
619  {
620  return const_reverse_iterator(begin());
621  }
622 
623  void swap(CPose3DQuat& o)
624  {
625  std::swap(o.m_coords, m_coords);
626  o.m_quat.swap(m_quat);
627  }
628 
629  /** @} */
630 
631  void setToNaN() override;
632 
633 }; // End of class def.
634 
635 std::ostream& operator<<(std::ostream& o, const CPose3DQuat& p);
636 
637 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
638  * than a pose with all its arguments multiplied by "-1") */
639 CPose3DQuat operator-(const CPose3DQuat& p);
640 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
641  * inverseComposePoint */
642 CPoint3D operator-(const CPoint3D& G, const CPose3DQuat& p);
643 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
644  * inverseComposePoint */
646  const mrpt::math::TPoint3D& G, const CPose3DQuat& p);
647 
648 bool operator==(const CPose3DQuat& p1, const CPose3DQuat& p2);
649 bool operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2);
650 
651 } // namespace mrpt::poses
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:388
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:390
const_iterator end() const
Definition: CPose3DQuat.h:608
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:496
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:372
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:420
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:604
reverse_iterator rbegin()
Definition: CPose3DQuat.h:612
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:471
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
static constexpr bool empty()
Definition: CPose3DQuat.h:362
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:597
const double G
void fill(const Scalar &val)
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:415
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:613
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:406
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:352
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:476
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:504
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:386
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:410
reverse_iterator rend()
Definition: CPose3DQuat.h:617
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:530
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:449
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:583
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:485
const_iterator begin() const
Definition: CPose3DQuat.h:607
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:572
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:502
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:565
static constexpr size_type max_size()
Definition: CPose3DQuat.h:363
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:392
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:576
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:498
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:345
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:481
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:289
double value_type
The type of the elements.
Definition: CPose3DQuat.h:350
#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:465
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:346
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.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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:516
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:364
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:84
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:520
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:455
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:461
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:618
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:559
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:344
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:394
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:593
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:54
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:265
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:354
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:500
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:525
CPose3DQuat & operator+=(const CPose3DQuat &b)
Make .
Definition: CPose3DQuat.h:203
static constexpr bool is_3D()
Definition: CPose3DQuat.h:335
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:603
std::string asString() const
Definition: CPose3DQuat.h:246
static constexpr size_type size()
Definition: CPose3DQuat.h:361
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:588
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:623
#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: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019