Main MRPT website > C++ reference
MRPT logo
CPose3DRotVec.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 CPOSE3DROTVEC_H
29 #define CPOSE3DROTVEC_H
30 
31 #include <mrpt/poses/CPose.h>
32 #include <mrpt/math/CMatrixFixedNumeric.h>
33 #include <mrpt/math/CQuaternion.h>
34 
35 namespace mrpt
36 {
37 namespace poses
38 {
39  using namespace mrpt::math;
40 
41  class CPose3D;
42  class CPose3DQuat;
43 
44  DEFINE_SERIALIZABLE_PRE( CPose3DRotVec )
45 
46  /** A 3D pose (a 3D translation + a rotation in 3D).
47  * The 6D transformation in SE(3) stored in this class is kept in two
48  * separate containers: a 3-array for the translation, and a 3-array for a rotation vector.
49  *
50  * \code
51  * CPose3DRotVec pose;
52  * pose.m_coords[{0:2}]=... // Translation
53  * pose.m_rotvec[{0:2}]=... // Rotation vector
54  * \endcode
55  *
56  * For a complete descriptionan of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
57  * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> online.
58  *
59  * There are Lie algebra methods: \a exp and \a ln (see the methods for documentation).
60  *
61  * \ingroup poses_grp
62  * \sa CPose3DRotVec, CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
63  */
64  class BASE_IMPEXP CPose3DRotVec : public CPose<CPose3DRotVec>, public mrpt::utils::CSerializable
65  {
66  // This must be added to any CSerializable derived class:
67  DEFINE_SERIALIZABLE( CPose3DRotVec )
68 
69  public:
70  CArrayDouble<3> m_coords; //!< The translation vector [x,y,z]
71  CArrayDouble<3> m_rotvec; //!< The rotation vector [vx,vy,vz]
72 
73  /** @name Constructors
74  @{ */
75 
76  /** Default constructor, with all the coordinates set to zero. */
77  inline CPose3DRotVec() {
78  m_coords[0]=m_coords[1]=m_coords[2]=0;
79  m_rotvec[0]=m_rotvec[1]=m_rotvec[2]=0;
80  }
81 
82  /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
83  inline CPose3DRotVec(TConstructorFlags_Poses constructor_dummy_param) : m_coords(),m_rotvec() { }
84 
85  /** Constructor with initilization of the pose */
86  inline CPose3DRotVec(const double x,const double y,const double z,const double vx, const double vy, const double vz) {
87  m_coords[0]= x; m_coords[1]= y; m_coords[2]= z;
88  m_rotvec[0]=vx; m_rotvec[1]=vy; m_rotvec[2]=vz;
89  }
90 
91  /** Constructor with initilization of the pose from a vector [x y z rx ry rz] */
92  inline CPose3DRotVec(const CArrayDouble<6> &v) {
93  m_coords[0]=v[0]; m_coords[1]=v[1]; m_coords[2]=v[2];
94  m_rotvec[0]=v[3]; m_rotvec[1]=v[4]; m_rotvec[2]=v[5];
95  }
96 
97  /** Constructor from a 4x4 homogeneous matrix: */
98  explicit CPose3DRotVec(const math::CMatrixDouble44 &m);
99 
100  /** Constructor from a CPose3D object.*/
101  explicit CPose3DRotVec(const CPose3D &);
102 
103  /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
104  CPose3DRotVec(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );
105 
106  /** Constructor from an array with these 6 elements: [x y z vx vy vz]
107  * where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
108  * \sa setFrom12Vector, getAs12Vector
109  */
110  inline explicit CPose3DRotVec(const double *vec6) {
111  m_coords[0]=vec6[0]; m_coords[1]=vec6[1]; m_coords[2]=vec6[2];
112  m_rotvec[0]=vec6[3]; m_rotvec[1]=vec6[4]; m_rotvec[2]=vec6[5];
113  }
114 
115  /** @} */ // end Constructors
116 
117 
118  /** @name Access 3x3 rotation and 4x4 homogeneous matrices
119  @{ */
120 
121  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
122  * \sa getInverseHomogeneousMatrix, getRotationMatrix
123  */
124  inline void getHomogeneousMatrix(CMatrixDouble44 & out_HM) const {
125  out_HM.block<3,3>(0,0) = getRotationMatrix();
126  out_HM.get_unsafe(0,3)=m_coords[0];
127  out_HM.get_unsafe(1,3)=m_coords[1];
128  out_HM.get_unsafe(2,3)=m_coords[2];
129  out_HM.get_unsafe(3,0)=0; out_HM.get_unsafe(3,1)=0; out_HM.get_unsafe(3,2)=0; out_HM.get_unsafe(3,3)=1;
130  }
131 
132  inline CMatrixDouble44 getHomogeneousMatrixVal() const { CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}
133 
134  /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
135  void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const;
136  //! \overload
137  inline const mrpt::math::CMatrixDouble33 getRotationMatrix() const { mrpt::math::CMatrixDouble33 ROT(UNINITIALIZED_MATRIX); getRotationMatrix(ROT); return ROT; }
138 
139  /** @} */ // end rot and HM
140 
141 
142  /** @name Pose-pose and pose-point compositions and operators
143  @{ */
144 
145  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
146  inline CPose3DRotVec operator + (const CPose3DRotVec& b) const
147  {
148  CPose3DRotVec ret(UNINITIALIZED_POSE);
149  ret.composeFrom(*this,b);
150  return ret;
151  }
152 
153  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
154  CPoint3D operator + (const CPoint3D& b) const;
155 
156  /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
157  CPoint3D operator + (const CPoint2D& b) const;
158 
159  /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. For the coordinate system see mrpt::poses::CPose3D */
160  void sphericalCoordinates(
161  const TPoint3D &point,
162  double &out_range,
163  double &out_yaw,
164  double &out_pitch ) const;
165 
166  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
167  * If pointers are provided, the corresponding Jacobians are returned.
168  * See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
169  */
170  void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
171  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
172  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const;
173 
174  /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
175  * \note local_point is passed by value to allow global and local point to be the same variable
176  */
177  inline void composePoint(const TPoint3D local_point, TPoint3D &global_point) const {
178  composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,global_point.z );
179  }
180 
181  /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
182  * If pointers are provided, the corresponding Jacobians are returned.
183  * See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
184  * \sa composePoint, composeFrom
185  */
186  void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
187  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
188  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL) const;
189 
190  /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
191  * \note A or B can be "this" without problems.
192  */
193  void composeFrom(const CPose3DRotVec& A, const CPose3DRotVec& B );
194 
195  /** Make \f$ this = this \oplus b \f$ (\a b can be "this" without problems) */
196  inline CPose3DRotVec& operator += (const CPose3DRotVec& b)
197  {
198  composeFrom(*this,b);
199  return *this;
200  }
201 
202  /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
203  * \note A or B can be "this" without problems.
204  * \sa composeFrom, composePoint
205  */
206  void inverseComposeFrom(const CPose3DRotVec& A, const CPose3DRotVec& B );
207 
208  /** Compute \f$ RET = this \oplus b \f$ */
209  inline CPose3DRotVec operator - (const CPose3DRotVec& b) const
210  {
211  CPose3DRotVec ret(UNINITIALIZED_POSE);
212  ret.inverseComposeFrom(*this,b);
213  return ret;
214  }
215 
216  /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
217  void inverse();
218 
219  /** makes: this = p (+) this */
220  inline void changeCoordinatesReference( const CPose3DRotVec & p ) { composeFrom(p,CPose3DRotVec(*this)); }
221 
222  /** @} */ // compositions
223 
224 
225  /** @name Access and modify contents
226  @{ */
227 
228  inline double rx() const { return m_rotvec[0]; }
229  inline double ry() const { return m_rotvec[1]; }
230  inline double rz() const { return m_rotvec[2]; }
231 
232  inline double &rx() { return m_rotvec[0]; }
233  inline double &ry() { return m_rotvec[1]; }
234  inline double &rz() { return m_rotvec[2]; }
235 
236  /** Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators. */
237  inline void addComponents(const CPose3DRotVec &p) {
238  m_coords+=p.m_coords;
239  m_rotvec+=p.m_rotvec;
240  }
241 
242  /** Scalar multiplication of x,y,z,vx,vy,vz. */
243  inline void operator *=(const double s) {
244  m_coords*=s;
245  m_rotvec*=s;
246  }
247 
248  /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
249  * \sa getYawPitchRoll, setYawPitchRoll
250  */
251  void setFromValues(
252  const double x0,
253  const double y0,
254  const double z0,
255  const double vx,
256  const double vy,
257  const double vz )
258  {
259  m_coords[0]=x0; m_coords[1]=y0; m_coords[2]=z0;
260  m_rotvec[0]=vx; m_rotvec[1]=vy; m_rotvec[2]=vz;
261  }
262 
263  /** Set pose from an array with these 6 elements: [x y z vx vy vz]
264  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the pose
265  * \sa getAs6Vector
266  */
267  template <class ARRAYORVECTOR>
268  inline void setFrom6Vector(const ARRAYORVECTOR &vec6)
269  {
270  m_rotvec[0]=vec6[3]; m_rotvec[1]=vec6[4]; m_rotvec[2]=vec6[5];
271  m_coords[0]=vec6[0]; m_coords[1]=vec6[1]; m_coords[2]=vec6[2];
272  }
273 
274  /** Gets pose as an array with these 6 elements: [x y z vx vy vz]
275  * where v{xyz} is the rotation vector and {xyz} the 3D translation of the pose
276  * The target vector MUST ALREADY have space for 6 elements (i.e. no .resize() method will be called).
277  * \sa setAs6Vector, getAsVector
278  */
279  template <class ARRAYORVECTOR>
280  inline void getAs6Vector(ARRAYORVECTOR &vec6) const
281  {
282  vec6[0]=m_coords[0]; vec6[1]=m_coords[1]; vec6[2]=m_coords[2];
283  vec6[3]=m_rotvec[0]; vec6[4]=m_rotvec[1]; vec6[5]=m_rotvec[2];
284  }
285 
286  /** Like getAs6Vector() but for dynamic size vectors (required by base class CPoseOrPoint) */
287  template <class ARRAYORVECTOR>
288  inline void getAsVector(ARRAYORVECTOR &v) const { v.resize(6); getAs6Vector(v); }
289 
290  inline const double &operator[](unsigned int i) const
291  {
292  switch(i)
293  {
294  case 0:return m_coords[0];
295  case 1:return m_coords[1];
296  case 2:return m_coords[2];
297  case 3:return m_rotvec[0];
298  case 4:return m_rotvec[1];
299  case 5:return m_rotvec[2];
300  default:
301  throw std::runtime_error("CPose3DRotVec::operator[]: Index of bounds.");
302  }
303  }
304  inline double &operator[](unsigned int i)
305  {
306  switch(i)
307  {
308  case 0:return m_coords[0];
309  case 1:return m_coords[1];
310  case 2:return m_coords[2];
311  case 3:return m_rotvec[0];
312  case 4:return m_rotvec[1];
313  case 5:return m_rotvec[2];
314  default:
315  throw std::runtime_error("CPose3DRotVec::operator[]: Index of bounds.");
316  }
317  }
318 
319  /** Returns a human-readable textual representation of the object: "[x y z rx ry rz]"
320  * \sa fromString
321  */
322  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_rotvec[0],m_rotvec[1],m_rotvec[2]); }
323  inline std::string asString() const { std::string s; asString(s); return s; }
324 
325  /** Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. )
326  * \sa asString
327  * \exception std::exception On invalid format
328  */
329  void fromString(const std::string &s) {
330  CMatrixDouble m;
331  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
332  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
333  for (int i=0;i<3;i++) m_coords[i]=m.get_unsafe(0,i);
334  for (int i=0;i<3;i++) m_rotvec[i]=m.get_unsafe(0,3+i);
335  }
336 
337  /** @} */ // modif. components
338 
339 
340  /** @name Lie Algebra methods
341  @{ */
342 
343  /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3DRotVec (static method). */
344  static CPose3DRotVec exp(const mrpt::math::CArrayDouble<6> & vect);
345 
346  /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra. */
347  void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
348 
349  /** Take the logarithm of the 3x3 rotation matrix part of this pose, generating the corresponding vector in the Lie Algebra. */
350  CArrayDouble<3> ln_rotation() const;
351 
352  /** @} */
353 
354  typedef CPose3DRotVec type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
355  enum { is_3D_val = 1 };
356  static inline bool is_3D() { return is_3D_val!=0; }
357  enum { rotation_dimensions = 3 };
358  enum { is_PDF_val = 0 };
359  static inline bool is_PDF() { return is_PDF_val!=0; }
360 
361  inline const type_value & getPoseMean() const { return *this; }
362  inline type_value & getPoseMean() { return *this; }
363 
364  /** @name STL-like methods and typedefs
365  @{ */
366  typedef double value_type; //!< The type of the elements
367  typedef double& reference;
368  typedef const double& const_reference;
369  typedef std::size_t size_type;
370  typedef std::ptrdiff_t difference_type;
371 
372 
373  // size is constant
374  enum { static_size = 6 };
375  static inline size_type size() { return static_size; }
376  static inline bool empty() { return false; }
377  static inline size_type max_size() { return static_size; }
378  static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DRotVec to %u.",static_cast<unsigned>(n))); }
379  /** @} */
380 
381  }; // End of class def.
382 
383 
384  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DRotVec& p);
385 
386  /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
387  CPose3DRotVec BASE_IMPEXP operator -(const CPose3DRotVec &p);
388 
389  bool BASE_IMPEXP operator==(const CPose3DRotVec &p1,const CPose3DRotVec &p2);
390  bool BASE_IMPEXP operator!=(const CPose3DRotVec &p1,const CPose3DRotVec &p2);
391 
392 
393  } // End of namespace
394 } // End of namespace
395 
396 #endif


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