Main MRPT website > C++ reference for MRPT 1.5.6
CQuaternion.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-2017, 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 
10 #ifndef CQuaternion_H
11 #define CQuaternion_H
12 
13 #include <mrpt/math/CMatrixTemplateNumeric.h>
14 #include <mrpt/math/CArrayNumeric.h>
15 #include <mrpt/utils/bits.h>
16 
17 namespace mrpt
18 {
19  namespace math
20  {
21  // For use with a constructor of CQuaternion
22  enum TConstructorFlags_Quaternions
23  {
24  UNINITIALIZED_QUATERNION = 0
25  };
26 
27  /** A quaternion, which can represent a 3D rotation as pair \f$ (r,\mathbf{u}) \f$, with a real part "r" and a 3D vector \f$ \mathbf{u} = (x,y,z) \f$, or alternatively, q = r + ix + jy + kz.
28  *
29  * The elements of the quaternion can be accessed by either:
30  * - r(), x(), y(), z(), or
31  * - the operator [] with indices running from 0 (=r) to 3 (=z).
32  *
33  * Users will usually employ the typedef "CQuaternionDouble" instead of this template.
34  *
35  * For more information about quaternions, see:
36  * - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
37  * - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
38  *
39  * \ingroup mrpt_base_grp
40  * \sa mrpt::poses::CPose3D
41  */
42  template <class T>
43  class CQuaternion : public CArrayNumeric<T,4>
44  {
45  typedef CArrayNumeric<T,4> Base;
46  public:
47  /* @{ Constructors
48  */
49 
50  /** Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical). */
51  inline CQuaternion(TConstructorFlags_Quaternions ) { }
52 
53  /** Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation. */
54  inline CQuaternion()
55  {
56  (*this)[0] = 1;
57  (*this)[1] = 0;
58  (*this)[2] = 0;
59  (*this)[3] = 0;
60  }
61 
62  /** Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz. */
63  inline CQuaternion(const T r,const T x,const T y,const T z)
64  {
65  (*this)[0] = r;
66  (*this)[1] = x;
67  (*this)[2] = y;
68  (*this)[3] = z;
69  ASSERTMSG_(std::abs(normSqr()-1.0)<1e-3, mrpt::format("Initialization data for quaternion is not normalized: %f %f %f %f -> sqrNorm=%f",r,x,y,z,normSqr()) );
70  }
71 
72  /* @}
73  */
74 
75 
76  inline T r()const {return (*this)[0];} //!< Return r coordinate of the quaternion
77  inline T x()const {return (*this)[1];} //!< Return x coordinate of the quaternion
78  inline T y()const {return (*this)[2];} //!< Return y coordinate of the quaternion
79  inline T z()const {return (*this)[3];} //!< Return z coordinate of the quaternion
80  inline void r(const T r) {(*this)[0]=r;} //!< Set r coordinate of the quaternion
81  inline void x(const T x) {(*this)[1]=x;} //!< Set x coordinate of the quaternion
82  inline void y(const T y) {(*this)[2]=y;} //!< Set y coordinate of the quaternion
83  inline void z(const T z) {(*this)[3]=z;} //!< Set z coordinate of the quaternion
84 
85  /** Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector \f$ \mathbf{v} \f$:
86  * If \f$ \mathbf{v}=0 \f$, then the quaternion is \f$ \mathbf{q} = [1 ~ 0 ~ 0 ~ 0]^\top \f$, otherwise:
87  * \f[ \mathbf{q} = \left[ \begin{array}{c}
88  * \cos(\frac{\theta}{2}) \\
89  * v_x \frac{\sin(\frac{\theta}{2})}{\theta} \\
90  * v_y \frac{\sin(\frac{\theta}{2})}{\theta} \\
91  * v_z \frac{\sin(\frac{\theta}{2})}{\theta}
92  * \end{array} \right] \f]
93  * where \f$ \theta = |\mathbf{v}| = \sqrt{v_x^2+v_y^2+v_z^2} \f$.
94  * \sa "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)", James Diebel.
95  */
96  template <class ARRAYLIKE3>
97  void fromRodriguesVector(const ARRAYLIKE3 &v)
98  {
99  if (v.size()!=3) THROW_EXCEPTION("Vector v must have a length=3");
100 
101  const T x = v[0];
102  const T y = v[1];
103  const T z = v[2];
104  const T angle = std::sqrt(x*x+y*y+z*z);
105  if (angle<1e-7)
106  {
107  (*this)[0] = 1;
108  (*this)[1] = static_cast<T>(0.5)*x;
109  (*this)[2] = static_cast<T>(0.5)*y;
110  (*this)[3] = static_cast<T>(0.5)*z;
111  }
112  else
113  {
114  const T s = (::sin(angle/2))/angle;
115  const T c = ::cos(angle/2);
116  (*this)[0] = c;
117  (*this)[1] = x * s;
118  (*this)[2] = y * s;
119  (*this)[3] = z * s;
120  }
121  }
122 
123 
124  /** @name Lie Algebra methods
125  @{ */
126 
127 
128  /** Logarithm of the 3x3 matrix defined by this pose, generating the corresponding vector in the SO(3) Lie Algebra,
129  * which coincides with the so-called "rotation vector" (I don't have space here for the proof ;-).
130  * \param[out] out_ln The target vector, which can be: std::vector<>, or mrpt::math::CVectorDouble or any row or column Eigen::Matrix<>.
131  * \sa exp, mrpt::poses::SE_traits */
132  template <class ARRAYLIKE3>
133  inline void ln(ARRAYLIKE3 &out_ln) const
134  {
135  if (out_ln.size()!=3) out_ln.resize(3);
136  this->ln_noresize(out_ln);
137  }
138  /** overload that returns by value */
139  template <class ARRAYLIKE3>
140  inline ARRAYLIKE3 ln() const
141  {
142  ARRAYLIKE3 out_ln;
143  this->ln(out_ln);
144  return out_ln;
145  }
146  /** Like ln() but does not try to resize the output vector. */
147  template <class ARRAYLIKE3>
148  void ln_noresize(ARRAYLIKE3 &out_ln) const
149  {
150  using mrpt::math::square;
151  const T xyz_norm = std::sqrt(square(x())+square(y())+square(z()));
152  const T K = (xyz_norm<1e-7) ? 2 : 2*::acos(r())/xyz_norm;
153  out_ln[0] = K*x();
154  out_ln[1] = K*y();
155  out_ln[2] = K*z();
156  }
157 
158  /** Exponential map from the SO(3) Lie Algebra to unit quaternions.
159  * \sa ln, mrpt::poses::SE_traits */
160  template <class ARRAYLIKE3>
161  inline static CQuaternion<T> exp(const ARRAYLIKE3 & v)
162  {
163  CQuaternion<T> q(UNINITIALIZED_QUATERNION);
164  q.fromRodriguesVector(v);
165  return q;
166  }
167  /** \overload */
168  template <class ARRAYLIKE3>
169  inline static void exp(const ARRAYLIKE3 & v, CQuaternion<T> & out_quat)
170  {
171  out_quat.fromRodriguesVector(v);
172  }
173 
174  /** @} */ // end of Lie algebra
175 
176 
177  /** Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2
178  * After the operation, "this" will represent the composed rotations of q1 and q2 (q2 applied "after" q1).
179  */
180  inline void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
181  {
182  // First: compute result, then save in this object. In this way we avoid problems when q1 or q2 == *this !!
183  const T new_r = q1.r()*q2.r() - q1.x()*q2.x() - q1.y()*q2.y() - q1.z()*q2.z();
184  const T new_x = q1.r()*q2.x() + q2.r()*q1.x() + q1.y()*q2.z() - q2.y()*q1.z();
185  const T new_y = q1.r()*q2.y() + q2.r()*q1.y() + q1.z()*q2.x() - q2.z()*q1.x();
186  const T new_z = q1.r()*q2.z() + q2.r()*q1.z() + q1.x()*q2.y() - q2.x()*q1.y();
187  this->r(new_r); this->x(new_x); this->y(new_y); this->z(new_z);
188  this->normalize();
189  }
190 
191  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion
192  */
193  void rotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
194  {
195  const double t2 = r()*x(); const double t3 = r()*y(); const double t4 = r()*z(); const double t5 =-x()*x(); const double t6 = x()*y();
196  const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
197  gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
198  gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
199  gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
200  }
201 
202  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion
203  */
204  void inverseRotatePoint(const double lx,const double ly,const double lz, double &gx,double &gy,double &gz ) const
205  {
206  const double t2 =-r()*x(); const double t3 =-r()*y(); const double t4 =-r()*z(); const double t5 =-x()*x(); const double t6 = x()*y();
207  const double t7 = x()*z(); const double t8 =-y()*y(); const double t9 = y()*z(); const double t10=-z()*z();
208  gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
209  gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
210  gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
211  }
212 
213  /** Return the squared norm of the quaternion */
214  inline double normSqr() const {
215  using mrpt::math::square;
216  return square(r()) + square(x()) + square(y()) + square(z());
217  }
218 
219  /** Normalize this quaternion, so its norm becomes the unitity.
220  */
221  inline void normalize()
222  {
223  const T qq = 1.0/std::sqrt( normSqr() );
224  for (unsigned int i=0;i<4;i++)
225  (*this)[i] *= qq;
226  }
227 
228  /** Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
229  * The output matrix can be a dynamic or fixed size (4x4) matrix.
230  */
231  template <class MATRIXLIKE>
232  void normalizationJacobian(MATRIXLIKE &J) const
233  {
234  const T n = 1.0/std::pow(normSqr(),T(1.5));
235  J.setSize(4,4);
236  J.get_unsafe(0,0)=x()*x()+y()*y()+z()*z();
237  J.get_unsafe(0,1)=-r()*x();
238  J.get_unsafe(0,2)=-r()*y();
239  J.get_unsafe(0,3)=-r()*z();
240 
241  J.get_unsafe(1,0)=-x()*r();
242  J.get_unsafe(1,1)=r()*r()+y()*y()+z()*z();
243  J.get_unsafe(1,2)=-x()*y();
244  J.get_unsafe(1,3)=-x()*z();
245 
246  J.get_unsafe(2,0)=-y()*r();
247  J.get_unsafe(2,1)=-y()*x();
248  J.get_unsafe(2,2)=r()*r()+x()*x()+z()*z();
249  J.get_unsafe(2,3)=-y()*z();
250 
251  J.get_unsafe(3,0)=-z()*r();
252  J.get_unsafe(3,1)=-z()*x();
253  J.get_unsafe(3,2)=-z()*y();
254  J.get_unsafe(3,3)=r()*r()+x()*x()+y()*y();
255  J *=n;
256  }
257 
258  /** Compute the Jacobian of the rotation composition operation \f$ p = f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$ \frac{\partial f}{\partial q_{this} } \f$.
259  * The output matrix can be a dynamic or fixed size (4x4) matrix.
260  */
261  template <class MATRIXLIKE>
262  inline void rotationJacobian(MATRIXLIKE &J) const
263  {
264  J.setSize(4,4);
265  J.get_unsafe(0,0)=r(); J.get_unsafe(0,1)=-x(); J.get_unsafe(0,2)=-y(); J.get_unsafe(0,3)=-z();
266  J.get_unsafe(1,0)=x(); J.get_unsafe(1,1)= r(); J.get_unsafe(1,2)=-z(); J.get_unsafe(1,3)= y();
267  J.get_unsafe(2,0)=y(); J.get_unsafe(2,1)= z(); J.get_unsafe(2,2)= r(); J.get_unsafe(2,3)=-x();
268  J.get_unsafe(3,0)=z(); J.get_unsafe(3,1)=-y(); J.get_unsafe(3,2)= x(); J.get_unsafe(3,3)= r();
269  }
270 
271  /** Calculate the 3x3 rotation matrix associated to this quaternion: \f[ \mathbf{R} = \left(
272  * \begin{array}{ccc}
273  * q_r^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y - q_r q_z) & 2(q_z q_x+q_r q_y) \\
274  * 2(q_x q_y+q_r q_z) & q_r^2-q_x^2+q_y^2-q_z^2 & 2(q_y q_z-q_r q_x) \\
275  * 2(q_z q_x-q_r q_y) & 2(q_y q_z+q_r q_x) & q_r^2- q_x^2 - q_y^2 + q_z^2
276  * \end{array}
277  * \right)\f]
278  *
279  */
280  template <class MATRIXLIKE>
281  inline void rotationMatrix(MATRIXLIKE &M) const
282  {
283  M.setSize(3,3);
284  rotationMatrixNoResize(M);
285  }
286 
287  /** Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quaternion (does not resize the matrix, for that, see rotationMatrix). */
288  template <class MATRIXLIKE>
289  inline void rotationMatrixNoResize(MATRIXLIKE &M) const
290  {
291  M.get_unsafe(0,0)=r()*r()+x()*x()-y()*y()-z()*z(); M.get_unsafe(0,1)=2*(x()*y() -r()*z()); M.get_unsafe(0,2)=2*(z()*x()+r()*y());
292  M.get_unsafe(1,0)=2*(x()*y()+r()*z()); M.get_unsafe(1,1)=r()*r()-x()*x()+y()*y()-z()*z(); M.get_unsafe(1,2)=2*(y()*z()-r()*x());
293  M.get_unsafe(2,0)=2*(z()*x()-r()*y()); M.get_unsafe(2,1)=2*(y()*z()+r()*x()); M.get_unsafe(2,2)=r()*r()-x()*x()-y()*y()+z()*z();
294  }
295 
296 
297  /** Return the conjugate quaternion */
298  inline void conj(CQuaternion &q_out) const
299  {
300  q_out.r( r() );
301  q_out.x(-x() );
302  q_out.y(-y() );
303  q_out.z(-z() );
304  }
305 
306  /** Return the conjugate quaternion */
307  inline CQuaternion conj() const
308  {
309  CQuaternion q_aux;
310  conj(q_aux);
311  return q_aux;
312  }
313 
314  /** Return the yaw, pitch & roll angles associated to quaternion
315  * \sa For the equations, see The MRPT Book, or see http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
316  * \sa rpy_and_jacobian
317  */
318  inline void rpy(T &roll, T &pitch, T &yaw) const
319  {
320  rpy_and_jacobian(roll,pitch,yaw,static_cast<mrpt::math::CMatrixDouble*>(NULL));
321  }
322 
323  /** Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of the transformation.
324  * Note that both the angles and the Jacobian have one set of normal equations, plus other special formulas for the degenerated cases of |pitch|=90 degrees.
325  * \sa For the equations, see The MRPT Book, or http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
326  * \sa rpy
327  */
328  template <class MATRIXLIKE>
329  void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq = NULL, bool resize_out_dr_dq_to3x4 = true ) const
330  {
331  using mrpt::math::square;
332  using std::sqrt;
333 
334  if (out_dr_dq && resize_out_dr_dq_to3x4)
335  out_dr_dq->setSize(3,4);
336  const T discr = r()*y()-x()*z();
337  if (fabs(discr)>0.49999)
338  { // pitch = 90 deg
339  pitch = 0.5*M_PI;
340  yaw =-2*atan2(x(),r());
341  roll = 0;
342  if (out_dr_dq) {
343  out_dr_dq->zeros();
344  out_dr_dq->get_unsafe(0,0) = +2/x();
345  out_dr_dq->get_unsafe(0,2) = -2*r()/(x()*x());
346  }
347  }
348  else if (discr<-0.49999)
349  { // pitch =-90 deg
350  pitch = -0.5*M_PI;
351  yaw = 2*atan2(x(),r());
352  roll = 0;
353  if (out_dr_dq) {
354  out_dr_dq->zeros();
355  out_dr_dq->get_unsafe(0,0) = -2/x();
356  out_dr_dq->get_unsafe(0,2) = +2*r()/(x()*x());
357  }
358  }
359  else
360  { // Non-degenerate case:
361  yaw = ::atan2( 2*(r()*z()+x()*y()), 1-2*(y()*y()+z()*z()) );
362  pitch = ::asin ( 2*discr );
363  roll = ::atan2( 2*(r()*x()+y()*z()), 1-2*(x()*x()+y()*y()) );
364  if (out_dr_dq) {
365  // Auxiliary terms:
366  const double val1=(2*x()*x() + 2*y()*y() - 1);
367  const double val12=square(val1);
368  const double val2=(2*r()*x() + 2*y()*z());
369  const double val22=square(val2);
370  const double xy2 = 2*x()*y();
371  const double rz2 = 2*r()*z();
372  const double ry2 = 2*r()*y();
373  const double val3 = (2*y()*y() + 2*z()*z() - 1);
374  const double val4 = ((square(rz2 + xy2)/square(val3) + 1)*val3);
375  const double val5 = (4*(rz2 + xy2))/square(val3);
376  const double val6 = 1.0/(square(rz2 + xy2)/square(val3) + 1);
377  const double val7 = 2.0/ sqrt(1 - square(ry2 - 2*x()*z()));
378  const double val8 = (val22/val12 + 1);
379  const double val9 = -2.0/val8;
380  // row 1:
381  out_dr_dq->get_unsafe(0,0) = -2*z()/val4;
382  out_dr_dq->get_unsafe(0,1) = -2*y()/val4;
383  out_dr_dq->get_unsafe(0,2) = -(2*x()/val3 - y()*val5)*val6 ;
384  out_dr_dq->get_unsafe(0,3) = -(2*r()/val3 - z()*val5)*val6;
385  // row 2:
386  out_dr_dq->get_unsafe(1,0) = y()*val7 ;
387  out_dr_dq->get_unsafe(1,1) = -z()*val7 ;
388  out_dr_dq->get_unsafe(1,2) = r()*val7 ;
389  out_dr_dq->get_unsafe(1,3) = -x()*val7 ;
390  // row 3:
391  out_dr_dq->get_unsafe(2,0) = val9*x()/val1 ;
392  out_dr_dq->get_unsafe(2,1) = val9*(r()/val1 - (2*x()*val2)/val12) ;
393  out_dr_dq->get_unsafe(2,2) = val9*(z()/val1 - (2*y()*val2)/val12) ;
394  out_dr_dq->get_unsafe(2,3) = val9*y()/val1 ;
395  }
396  }
397  }
398 
399  inline CQuaternion operator * (const T &factor)
400  {
401  CQuaternion q = *this;
402  q*=factor;
403  return q;
404  }
405 
406  }; // end class
407 
408  typedef CQuaternion<double> CQuaternionDouble; //!< A quaternion of data type "double"
409  typedef CQuaternion<float> CQuaternionFloat; //!< A quaternion of data type "float"
410 
411  } // end namespace
412 
413 } // end namespace mrpt
414 
415 #endif
mrpt::math::CQuaternion::inverseRotatePoint
void inverseRotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion...
Definition: CQuaternion.h:203
mrpt::math::CQuaternion::exp
static CQuaternion< T > exp(const ARRAYLIKE3 &v)
Exponential map from the SO(3) Lie Algebra to unit quaternions.
Definition: CQuaternion.h:160
mrpt::math::CQuaternion::rotationMatrix
void rotationMatrix(MATRIXLIKE &M) const
Calculate the 3x3 rotation matrix associated to this quaternion: .
Definition: CQuaternion.h:280
mrpt::math::CQuaternion::normalize
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:220
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::math::TConstructorFlags_Quaternions
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
n
GLenum GLsizei n
Definition: glext.h:4618
mrpt::math::CQuaternion::rotationMatrixNoResize
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
Definition: CQuaternion.h:288
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:408
M_PI
#define M_PI
Definition: bits.h:78
s
GLdouble s
Definition: glext.h:3602
CArrayNumeric.h
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::math::CQuaternion::normSqr
double normSqr() const
Return the squared norm of the quaternion.
Definition: CQuaternion.h:213
mrpt::math::CQuaternion::r
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
mrpt::math::CQuaternion::operator*
CQuaternion operator*(const T &factor)
Definition: CQuaternion.h:398
mrpt::math::CQuaternion::crossProduct
void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...
Definition: CQuaternion.h:179
c
const GLubyte * c
Definition: glext.h:5590
mrpt::math::CQuaternion::rotationJacobian
void rotationJacobian(MATRIXLIKE &J) const
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...
Definition: CQuaternion.h:261
mrpt::math::CQuaternion::rotatePoint
void rotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.
Definition: CQuaternion.h:192
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::math::CQuaternion::ln_noresize
void ln_noresize(ARRAYLIKE3 &out_ln) const
Like ln() but does not try to resize the output vector.
Definition: CQuaternion.h:147
mrpt::math::CQuaternion::Base
CArrayNumeric< T, 4 > Base
Definition: CQuaternion.h:45
mrpt::math::CQuaternion::ln
ARRAYLIKE3 ln() const
overload that returns by value
Definition: CQuaternion.h:139
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
mrpt::mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: maps/CColouredPointsMap.h:52
mrpt::math::CQuaternion::CQuaternion
CQuaternion(const T r, const T x, const T y, const T z)
Construct a quaternion from its parameters &#39;r&#39;, &#39;x&#39;, &#39;y&#39;, &#39;z&#39;, with q = r + ix + jy + kz...
Definition: CQuaternion.h:63
CMatrixTemplateNumeric.h
y
GLenum GLint GLint y
Definition: glext.h:3516
x
GLenum GLint x
Definition: glext.h:3516
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
mrpt::math::CQuaternion::rpy_and_jacobian
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=NULL, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
Definition: CQuaternion.h:328
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:277
mrpt::math::CQuaternion::rpy
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:317
mrpt::math::CQuaternion::conj
CQuaternion conj() const
Return the conjugate quaternion.
Definition: CQuaternion.h:306
mrpt::math::CQuaternion::CQuaternion
CQuaternion()
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.
Definition: CQuaternion.h:54
mrpt::math::CQuaternion::CQuaternion
CQuaternion(TConstructorFlags_Quaternions)
Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quat...
Definition: CQuaternion.h:51
mrpt::math::CQuaternionFloat
CQuaternion< float > CQuaternionFloat
A quaternion of data type "float".
Definition: CQuaternion.h:409
mrpt::math::CQuaternion::fromRodriguesVector
void fromRodriguesVector(const ARRAYLIKE3 &v)
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If ...
Definition: CQuaternion.h:96


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019