Main MRPT website > C++ reference
MRPT logo
CPose3DQuat.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPose3DQuat_H
29 #define CPose3DQuat_H
30 
31 #include <mrpt/poses/CPose.h>
32 #include <mrpt/math/CMatrixFixedNumeric.h>
33 #include <mrpt/math/CQuaternion.h>
34 #include <mrpt/poses/CPoint3D.h>
35 #include <mrpt/math/lightweight_geom_data.h>
36 
37 namespace mrpt
38 {
39 namespace poses
40 {
41  using namespace mrpt::math;
42 
43  class CPose3D;
44 
45  DEFINE_SERIALIZABLE_PRE( CPose3DQuat )
46 
47  /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
48  *
49  * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
50  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> in the wiki.
51  *
52  * To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().
53  *
54  * This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator
55  * with indices running from 0 to 6 to access the [x y z qr qx qy qz] as if they were a vector. Thus, a CPose3DQuat can be used
56  * as a 7-vector anywhere the MRPT math functions expect any kind of vector.
57  *
58  * This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.
59  *
60  * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion), mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic version of this class, mrpt::math::CQuaternion, CPoseOrPoint
61  * \ingroup poses_grp
62  */
63  class BASE_IMPEXP CPose3DQuat : public CPose<CPose3DQuat>, public mrpt::utils::CSerializable
64  {
65  // This must be added to any CSerializable derived class:
66  DEFINE_SERIALIZABLE( CPose3DQuat )
67 
68  public:
69  CArrayDouble<3> m_coords; //!< The translation vector [x,y,z]
70  mrpt::math::CQuaternionDouble m_quat; //!< The quaternion.
71 
72  public:
73  /** Read/Write access to the quaternion representing the 3D rotation. */
74  inline mrpt::math::CQuaternionDouble & quat() { return m_quat; }
75  /** Read-only access to the quaternion representing the 3D rotation. */
76  inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
77 
78  /** Read/Write access to the translation vector in R^3. */
79  inline mrpt::math::CArrayDouble<3> & xyz() { return m_coords; }
80  /** Read-only access to the translation vector in R^3. */
81  inline const mrpt::math::CArrayDouble<3> & xyz() const { return m_coords; }
82 
83 
84  /** Default constructor, initialize translation to zeros and quaternion to no rotation. */
85  inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
86 
87  /** Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor. */
88  inline CPose3DQuat(TConstructorFlags_Quaternions constructor_dummy_param) : m_quat(UNINITIALIZED_QUATERNION) { }
89 
90  /** Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary */
91  inline CPose3DQuat(const double x,const double y,const double z,const mrpt::math::CQuaternionDouble &q ) : m_quat(q) { m_coords[0]=x; m_coords[1]=y; m_coords[2]=z; m_quat.normalize(); }
92 
93  /** Constructor from a CPose3D */
94  explicit CPose3DQuat(const CPose3D &p);
95 
96  /** Constructor from lightweight object. */
97  CPose3DQuat(const mrpt::math::TPose3DQuat &p) : m_quat(p.qr,p.qx,p.qy,p.qz) { x()=p.x; y()=p.y; z()=p.z; }
98 
99  /** Constructor from a 4x4 homogeneous transformation matrix.
100  */
101  explicit CPose3DQuat(const CMatrixDouble44 &M);
102 
103  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
104  * \sa getInverseHomogeneousMatrix
105  */
106  void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const;
107 
108  /** Returns a 1x7 vector with [x y z qr qx qy qz] */
109  void getAsVector(vector_double &v) const;
110  /// \overload
111  void getAsVector(mrpt::math::CArrayDouble<7> &v) const {
112  v[0] = m_coords[0]; v[1] = m_coords[1]; v[2] = m_coords[2];
113  v[3] = m_quat[0]; v[4] = m_quat[1]; v[5] = m_quat[2]; v[6] = m_quat[3];
114  }
115 
116  /** Makes \f$ this = A \oplus B \f$ this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
117  * \note A or B can be "this" without problems.
118  * \sa inverseComposeFrom, composePoint
119  */
120  void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
121 
122  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
123  * \note A or B can be "this" without problems.
124  * \sa composeFrom, composePoint
125  */
126  void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
127 
128  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
129  * \sa composeFrom, inverseComposePoint
130  */
131  void composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
132  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
133  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
134 
135  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
136  * \sa composePoint, composeFrom
137  */
138  void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
139  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
140  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
141 
142  /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
143  * POINT1 and POINT1 can be anything supporing [0],[1],[2].
144  * \sa composePoint */
145  template <class POINT1,class POINT2> inline void composePoint( const POINT1 &L, POINT2 &G) const { composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); }
146 
147  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
148  template <class POINT1,class POINT2> inline void inverseComposePoint( const POINT1 &G, POINT2 &L) const { inverseComposePoint(G[0],G[1],G[2],L[0],L[1],L[2]); }
149 
150  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */
151  inline CPoint3D operator +( const CPoint3D &L) const { CPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
152 
153  /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */
154  inline TPoint3D operator +( const TPoint3D &L) const { TPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
155 
156  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
157  inline CPoint3D operator -( const CPoint3D &G) const { CPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
158 
159  /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
160  inline TPoint3D operator -( const TPoint3D &G) const { TPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
161 
162  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
163  */
164  virtual void operator *=(const double s);
165 
166  /** Make \f$ this = this \oplus b \f$ */
167  inline CPose3DQuat& operator += (const CPose3DQuat& b)
168  {
169  composeFrom(*this,b);
170  return *this;
171  }
172 
173  /** Return the composed pose \f$ ret = this \oplus p \f$ */
174  inline CPose3DQuat operator + (const CPose3DQuat& p) const
175  {
176  CPose3DQuat ret;
177  ret.composeFrom(*this,p);
178  return ret;
179  }
180 
181  /** Make \f$ this = this \ominus b \f$ */
182  inline CPose3DQuat& operator -= (const CPose3DQuat& b)
183  {
184  inverseComposeFrom(*this,b);
185  return *this;
186  }
187 
188  /** Return the composed pose \f$ ret = this \ominus p \f$ */
189  inline CPose3DQuat operator - (const CPose3DQuat& p) const
190  {
191  CPose3DQuat ret;
192  ret.inverseComposeFrom(*this,p);
193  return ret;
194  }
195 
196  /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)
197  * \sa fromString
198  */
199  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_quat[0],m_quat[1],m_quat[2],m_quat[3]); }
200  inline std::string asString() const { std::string s; asString(s); return s; }
201 
202  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )
203  * \sa asString
204  * \exception std::exception On invalid format
205  */
206  void fromString(const std::string &s) {
207  CMatrixDouble m;
208  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
209  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
210  m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
211  m_quat[0] = m.get_unsafe(0,3); m_quat[1] = m.get_unsafe(0,4); m_quat[2] = m.get_unsafe(0,5); m_quat[3] = m.get_unsafe(0,6);
212  }
213 
214  /** Read only [] operator */
215  inline const double &operator[](unsigned int i) const
216  {
217  switch(i)
218  {
219  case 0:return m_coords[0];
220  case 1:return m_coords[1];
221  case 2:return m_coords[2];
222  case 3:return m_quat[0];
223  case 4:return m_quat[1];
224  case 5:return m_quat[2];
225  case 6:return m_quat[3];
226  default:
227  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
228  }
229  }
230  /** Read/write [] operator */
231  inline double &operator[](unsigned int i)
232  {
233  switch(i)
234  {
235  case 0:return m_coords[0];
236  case 1:return m_coords[1];
237  case 2:return m_coords[2];
238  case 3:return m_quat[0];
239  case 4:return m_quat[1];
240  case 5:return m_quat[2];
241  case 6:return m_quat[3];
242  default:
243  throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
244  }
245  }
246 
247  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
248  * For the coordinate system see the top of this page.
249  * If the matrix pointers are not NULL, the Jacobians will be also computed for the range-yaw-pitch variables wrt the passed 3D point and this 7D pose.
250  */
251  void sphericalCoordinates(
252  const TPoint3D &point,
253  double &out_range,
254  double &out_yaw,
255  double &out_pitch,
256  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
257  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
258  ) const;
259 
260  public:
261  typedef CPose3DQuat type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
262  enum { is_3D_val = 1 };
263  static inline bool is_3D() { return is_3D_val!=0; }
264  enum { rotation_dimensions = 3 };
265  enum { is_PDF_val = 1 };
266  static inline bool is_PDF() { return is_PDF_val!=0; }
267 
268  inline const type_value & getPoseMean() const { return *this; }
269  inline type_value & getPoseMean() { return *this; }
270 
271  /** @name STL-like methods and typedefs
272  @{ */
273  typedef double value_type; //!< The type of the elements
274  typedef double& reference;
275  typedef const double& const_reference;
276  typedef std::size_t size_type;
277  typedef std::ptrdiff_t difference_type;
278 
279  // size is constant
280  enum { static_size = 7 };
281  static inline size_type size() { return static_size; }
282  static inline bool empty() { return false; }
283  static inline size_type max_size() { return static_size; }
284  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DQuat to %u.",static_cast<unsigned>(n))); }
285 
286  inline void assign(const size_t N, const double val)
287  {
288  if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
289  m_coords.fill(val);
290  m_quat.fill(val);
291  }
292 
293  struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
294  {
295  private:
296  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
297  CPose3DQuat *m_obj; //!< A reference to the source of this iterator
298  size_t m_cur_idx; //!< The iterator points to this element.
299  typedef value_type T; //!< The type of the matrix elements
300 
301  inline void check_limits(bool allow_end = false) const
302  {
303  #ifdef _DEBUG
304  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
305  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
306  #endif
307  }
308  public:
309  inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
310  inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
311  inline iterator() : m_obj(NULL),m_cur_idx(0) { }
312  inline iterator(CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
313  inline CPose3DQuat::reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
314  inline iterator &operator++() {
315  check_limits();
316  ++m_cur_idx;
317  return *this;
318  }
319  inline iterator operator++(int) {
320  iterator it=*this;
321  ++*this;
322  return it;
323  }
324  inline iterator &operator--() {
325  --m_cur_idx;
326  check_limits();
327  return *this;
328  }
329  inline iterator operator--(int) {
330  iterator it=*this;
331  --*this;
332  return it;
333  }
334  inline iterator &operator+=(iterator_base::difference_type off) {
335  m_cur_idx+=off;
336  check_limits(true);
337  return *this;
338  }
339  inline iterator operator+(iterator_base::difference_type off) const {
340  iterator it=*this;
341  it+=off;
342  return it;
343  }
344  inline iterator &operator-=(iterator_base::difference_type off) {
345  return (*this)+=(-off);
346  }
347  inline iterator operator-(iterator_base::difference_type off) const {
348  iterator it=*this;
349  it-=off;
350  return it;
351  }
352  inline iterator_base::difference_type operator-(const iterator &it) const { return m_cur_idx - it.m_cur_idx; }
353  inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
354  inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
355  inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
356  }; // end iterator
357 
358  struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
359  {
360  private:
361  typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
362  const CPose3DQuat *m_obj; //!< A reference to the source of this iterator
363  size_t m_cur_idx; //!< The iterator points to this element.
364  typedef value_type T; //!< The type of the matrix elements
365 
366  inline void check_limits(bool allow_end = false) const
367  {
368  #ifdef _DEBUG
369  ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
370  if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
371  #endif
372  }
373  public:
374  inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
375  inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
376  inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
377  inline const_iterator(const CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
378  inline CPose3DQuat::const_reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
379  inline const_iterator &operator++() {
380  check_limits();
381  ++m_cur_idx;
382  return *this;
383  }
384  inline const_iterator operator++(int) {
385  const_iterator it=*this;
386  ++*this;
387  return it;
388  }
389  inline const_iterator &operator--() {
390  --m_cur_idx;
391  check_limits();
392  return *this;
393  }
394  inline const_iterator operator--(int) {
395  const_iterator it=*this;
396  --*this;
397  return it;
398  }
399  inline const_iterator &operator+=(iterator_base::difference_type off) {
400  m_cur_idx+=off;
401  check_limits(true);
402  return *this;
403  }
404  inline const_iterator operator+(iterator_base::difference_type off) const {
405  const_iterator it=*this;
406  it+=off;
407  return it;
408  }
409  inline const_iterator &operator-=(iterator_base::difference_type off) {
410  return (*this)+=(-off);
411  }
412  inline const_iterator operator-(iterator_base::difference_type off) const {
413  const_iterator it=*this;
414  it-=off;
415  return it;
416  }
417  inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
418  inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
419  inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
420  inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
421  }; // end const_iterator
422 
423  typedef std::reverse_iterator<iterator> reverse_iterator;
424  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
425  inline iterator begin() { return iterator(*this,0); }
426  inline iterator end() { return iterator(*this,static_size); }
427  inline const_iterator begin() const { return const_iterator(*this,0); }
428  inline const_iterator end() const { return const_iterator(*this,static_size); }
429  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
430  inline const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
431  inline reverse_iterator rend() { return reverse_iterator(begin()); }
432  inline const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
433 
434 
435  void swap (CPose3DQuat& o)
436  {
437  std::swap(o.m_coords, m_coords);
438  o.m_quat.swap(m_quat);
439  }
440 
441  /** @} */
442  //! See ops_containers.h
443  typedef CPose3DQuat mrpt_autotype;
444  //DECLARE_MRPT_CONTAINER_TYPES
445 
446  }; // End of class def.
447 
448  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DQuat& p);
449 
450 
451  } // End of namespace
452 } // End of namespace
453 
454 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013