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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020