MRPT  2.0.2
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-2020, 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  static CPose3DQuat FromString(const std::string& s)
265  {
266  CPose3DQuat o;
267  o.fromString(s);
268  return o;
269  }
270 
271  /** Read only [] operator */
272  inline 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 = double;
360  using size_type = std::size_t;
361  using difference_type = std::ptrdiff_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([[maybe_unused]] 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 #endif
408  }
409 
410  public:
411  inline bool operator<(const iterator& it2) const
412  {
413  return m_cur_idx < it2.m_cur_idx;
414  }
415  inline bool operator>(const iterator& it2) const
416  {
417  return m_cur_idx > it2.m_cur_idx;
418  }
419  inline iterator() = default;
420  inline iterator(CPose3DQuat& obj, size_t start_idx)
421  : m_obj(&obj), m_cur_idx(start_idx)
422  {
423  check_limits(true); /*Dont report as error an iterator to end()*/
424  }
426  {
427  check_limits();
428  return (*m_obj)[m_cur_idx];
429  }
431  {
432  check_limits();
433  ++m_cur_idx;
434  return *this;
435  }
436  inline iterator operator++(int)
437  {
438  iterator it = *this;
439  ++*this;
440  return it;
441  }
443  {
444  --m_cur_idx;
445  check_limits();
446  return *this;
447  }
448  inline iterator operator--(int)
449  {
450  iterator it = *this;
451  --*this;
452  return it;
453  }
454  inline iterator& operator+=(iterator_base::difference_type off)
455  {
456  m_cur_idx += off;
457  check_limits(true);
458  return *this;
459  }
460  inline iterator operator+(iterator_base::difference_type off) const
461  {
462  iterator it = *this;
463  it += off;
464  return it;
465  }
466  inline iterator& operator-=(iterator_base::difference_type off)
467  {
468  return (*this) += (-off);
469  }
470  inline iterator operator-(iterator_base::difference_type off) const
471  {
472  iterator it = *this;
473  it -= off;
474  return it;
475  }
476  inline iterator_base::difference_type operator-(
477  const iterator& it) const
478  {
479  return m_cur_idx - it.m_cur_idx;
480  }
482  iterator_base::difference_type off) const
483  {
484  return (*m_obj)[m_cur_idx + off];
485  }
486  inline bool operator==(const iterator& it) const
487  {
488  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
489  }
490  inline bool operator!=(const iterator& it) const
491  {
492  return !(operator==(it));
493  }
494  }; // end iterator
495 
497  : public std::iterator<std::random_access_iterator_tag, value_type>
498  {
499  private:
500  using iterator_base =
501  std::iterator<std::random_access_iterator_tag, value_type>;
502  /** A reference to the source of this iterator */
503  const CPose3DQuat* m_obj{nullptr};
504  /** The iterator points to this element. */
505  size_t m_cur_idx{0};
506  /** The type of the matrix elements */
507  using T = value_type;
508 
509  inline void check_limits([[maybe_unused]] bool allow_end = false) const
510  {
511 #ifdef _DEBUG
512  ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
513  if (m_cur_idx > (allow_end ? 7u : 6u))
514  THROW_EXCEPTION("Index out of range in iterator.");
515 #endif
516  }
517 
518  public:
519  inline bool operator<(const const_iterator& it2) const
520  {
521  return m_cur_idx < it2.m_cur_idx;
522  }
523  inline bool operator>(const const_iterator& it2) const
524  {
525  return m_cur_idx > it2.m_cur_idx;
526  }
527  inline const_iterator() = default;
528  inline const_iterator(const CPose3DQuat& obj, size_t start_idx)
529  : m_obj(&obj), m_cur_idx(start_idx)
530  {
531  check_limits(true); /*Dont report as error an iterator to end()*/
532  }
534  {
535  check_limits();
536  return (*m_obj)[m_cur_idx];
537  }
539  {
540  check_limits();
541  ++m_cur_idx;
542  return *this;
543  }
545  {
546  const_iterator it = *this;
547  ++*this;
548  return it;
549  }
551  {
552  --m_cur_idx;
553  check_limits();
554  return *this;
555  }
557  {
558  const_iterator it = *this;
559  --*this;
560  return it;
561  }
562  inline const_iterator& operator+=(iterator_base::difference_type off)
563  {
564  m_cur_idx += off;
565  check_limits(true);
566  return *this;
567  }
569  iterator_base::difference_type off) const
570  {
571  const_iterator it = *this;
572  it += off;
573  return it;
574  }
575  inline const_iterator& operator-=(iterator_base::difference_type off)
576  {
577  return (*this) += (-off);
578  }
580  iterator_base::difference_type off) const
581  {
582  const_iterator it = *this;
583  it -= off;
584  return it;
585  }
586  inline iterator_base::difference_type operator-(
587  const const_iterator& it) const
588  {
589  return m_cur_idx - it.m_cur_idx;
590  }
592  iterator_base::difference_type off) const
593  {
594  return (*m_obj)[m_cur_idx + off];
595  }
596  inline bool operator==(const const_iterator& it) const
597  {
598  return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
599  }
600  inline bool operator!=(const const_iterator& it) const
601  {
602  return !(operator==(it));
603  }
604  }; // end const_iterator
605 
606  using reverse_iterator = std::reverse_iterator<iterator>;
607  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
608  inline iterator begin() { return iterator(*this, 0); }
609  inline iterator end() { return iterator(*this, static_size); }
610  inline const_iterator begin() const { return const_iterator(*this, 0); }
611  inline const_iterator end() const
612  {
613  return const_iterator(*this, static_size);
614  }
615  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
617  {
618  return const_reverse_iterator(end());
619  }
620  inline reverse_iterator rend() { return reverse_iterator(begin()); }
622  {
623  return const_reverse_iterator(begin());
624  }
625 
626  void swap(CPose3DQuat& o)
627  {
628  std::swap(o.m_coords, m_coords);
629  o.m_quat.swap(m_quat);
630  }
631 
632  /** @} */
633 
634  void setToNaN() override;
635 
636 }; // End of class def.
637 
638 std::ostream& operator<<(std::ostream& o, const CPose3DQuat& p);
639 
640 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
641  * than a pose with all its arguments multiplied by "-1") */
642 CPose3DQuat operator-(const CPose3DQuat& p);
643 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
644  * inverseComposePoint */
645 CPoint3D operator-(const CPoint3D& G, const CPose3DQuat& p);
646 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa
647  * inverseComposePoint */
649  const mrpt::math::TPoint3D& G, const CPose3DQuat& p);
650 
651 bool operator==(const CPose3DQuat& p1, const CPose3DQuat& p2);
652 bool operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2);
653 
654 } // namespace mrpt::poses
void check_limits([[maybe_unused]] bool allow_end=false) const
Definition: CPose3DQuat.h:401
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:611
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:501
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:425
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:607
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
reverse_iterator rbegin()
Definition: CPose3DQuat.h:615
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:476
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
static constexpr bool empty()
Definition: CPose3DQuat.h:369
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:600
const double G
void fill(const Scalar &val)
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:420
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:616
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
bool operator<(const iterator &it2) const
Definition: CPose3DQuat.h:411
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
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:481
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:540
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...
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.
void check_limits([[maybe_unused]] bool allow_end=false) const
Definition: CPose3DQuat.h:509
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool operator>(const iterator &it2) const
Definition: CPose3DQuat.h:415
reverse_iterator rend()
Definition: CPose3DQuat.h:620
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:533
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:454
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:586
static CPose3DQuat FromString(const std::string &s)
Definition: CPose3DQuat.h:264
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:490
const_iterator begin() const
Definition: CPose3DQuat.h:610
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:575
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:507
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:568
static constexpr size_type max_size()
Definition: CPose3DQuat.h:370
double operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:272
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:579
double x
Translation in x,y,z.
Definition: TPose3DQuat.h:27
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:503
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
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:486
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:470
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...
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
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:519
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:523
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:460
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:466
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:621
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:562
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
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:596
#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
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:361
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void swap(CMatrixFixed &o)
Definition: CMatrixFixed.h:197
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:505
CPose3DQuat operator+(const CPose3DQuat &p) const
Return the composed pose .
Definition: CPose3DQuat.h:210
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:528
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:606
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:591
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:626
CPose3DQuat & operator-=(const CPose3DQuat &b)
Make .
Definition: CPose3DQuat.h:218



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020