MRPT  1.9.9
CQuaternion.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 
10 #pragma once
11 
12 #include <mrpt/core/bits_math.h> // square()
13 #include <mrpt/core/exceptions.h>
15 #include <mrpt/math/CVectorFixed.h>
16 
17 namespace mrpt::math
18 {
19 // For use with a constructor of CQuaternion
21 {
23 };
24 
25 /** A quaternion, which can represent a 3D rotation as pair \f$ (r,\mathbf{u})
26  *\f$, with a real part "r" and a 3D vector \f$ \mathbf{u} = (x,y,z) \f$, or
27  *alternatively, q = r + ix + jy + kz.
28  *
29  * The elements of the quaternion can be accessed by either:
30  * - r()(equivalent to w()), x(), y(), z(), or
31  * - the operator [] with indices running from 0 (=r) to 3 (=z).
32  *
33  * Users will usually employ the type `CQuaternionDouble` instead of this
34  *template.
35  *
36  * For more information about quaternions, see:
37  * - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
38  * - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
39  *
40  * \ingroup mrpt_math_grp
41  * \sa mrpt::poses::CPose3D
42  */
43 template <class T>
44 class CQuaternion : public CVectorFixed<T, 4>
45 {
47 
48  public:
49  /* @{ Constructors
50  */
51 
52  /** Can be used with UNINITIALIZED_QUATERNION as argument, does not
53  * initialize the 4 elements of the quaternion (use this constructor when
54  * speed is critical). */
56  /** Default constructor: construct a (1, (0,0,0) ) quaternion representing
57  * no rotation. */
58  inline CQuaternion()
59  {
60  r() = 1;
61  x() = 0;
62  y() = 0;
63  z() = 0;
64  }
65 
66  /** Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q =
67  * r + ix + jy + kz. */
68  inline CQuaternion(const T R, const T X, const T Y, const T Z)
69  {
70  r() = R;
71  x() = X;
72  y() = Y;
73  z() = Z;
75 
76  ASSERTMSG_(
77  std::abs(normSqr() - 1.0) < 1e-3,
79  "Initialization data for quaternion is not normalized: %f %f "
80  "%f %f -> sqrNorm=%f",
81  R, X, Y, Z, normSqr()));
82  }
83 
84  /* @}
85  */
86 
87  /** Adhere to the convention of w>=0 to avoid ambiguity of quaternion double
88  * cover of SO(3) */
89  inline void ensurePositiveRealPart()
90  {
91  // Ensure r()>0
92  if (r() < 0)
93  {
94  r() = -r();
95  x() = -x();
96  y() = -y();
97  z() = -z();
98  }
99  }
100 
101  /** Return r (real part) coordinate of the quaternion */
102  inline T r() const { return (*this)[0]; }
103  /** Return w (real part) coordinate of the quaternion. Alias of r() */
104  inline T w() const { return (*this)[0]; }
105  /** Return x coordinate of the quaternion */
106  inline T x() const { return (*this)[1]; }
107  /** Return y coordinate of the quaternion */
108  inline T y() const { return (*this)[2]; }
109  /** Return z coordinate of the quaternion */
110  inline T z() const { return (*this)[3]; }
111  /** Set r (real part) coordinate of the quaternion */
112  inline void r(const T r) { (*this)[0] = r; }
113  /** Set w (real part) coordinate of the quaternion. Alias of r() */
114  inline void w(const T w) { (*this)[0] = w; }
115  /** Set x coordinate of the quaternion */
116  inline void x(const T x) { (*this)[1] = x; }
117  /** Set y coordinate of the quaternion */
118  inline void y(const T y) { (*this)[2] = y; }
119  /** Set z coordinate of the quaternion */
120  inline void z(const T z) { (*this)[3] = z; }
122  inline T& r() { return (*this)[0]; }
123  inline T& x() { return (*this)[1]; }
124  inline T& y() { return (*this)[2]; }
125  inline T& z() { return (*this)[3]; }
126 
127  /** Set this quaternion to the rotation described by a 3D (Rodrigues)
128  * rotation vector \f$ \mathbf{v} \f$:
129  * If \f$ \mathbf{v}=0 \f$, then the quaternion is \f$ \mathbf{q} = [1 ~
130  * 0 ~ 0 ~ 0]^\top \f$, otherwise:
131  * \f[ \mathbf{q} = \left[ \begin{array}{c}
132  * \cos(\frac{\theta}{2}) \\
133  * v_x \frac{\sin(\frac{\theta}{2})}{\theta} \\
134  * v_y \frac{\sin(\frac{\theta}{2})}{\theta} \\
135  * v_z \frac{\sin(\frac{\theta}{2})}{\theta}
136  * \end{array} \right] \f]
137  * where \f$ \theta = |\mathbf{v}| = \sqrt{v_x^2+v_y^2+v_z^2} \f$.
138  * \sa "Representing Attitude: Euler Angles, Unit Quaternions, and
139  * Rotation Vectors (2006)", James Diebel.
140  */
141  template <class ARRAYLIKE3>
142  void fromRodriguesVector(const ARRAYLIKE3& v)
143  {
144  MRPT_START
145  ASSERT_(v.size() == 3);
146  const T x = v[0], y = v[1], z = v[2];
147  const T theta_sq = x * x + y * y + z * z, theta = std::sqrt(theta_sq);
148  T r, i;
149  if (theta < 1e-6)
150  {
151  // Taylor series approximation:
152  const T theta_po4 = theta_sq * theta_sq;
153  i = T(0.5) - T(1.0 / 48.0) * theta_sq + T(1.0 / 3840.0) * theta_po4;
154  r = T(1.0) - T(0.5) * theta_sq + T(1.0 / 384.0) * theta_po4;
155  }
156  else
157  {
158  i = (std::sin(theta / 2)) / theta;
159  r = std::cos(theta / 2);
160  }
161  (*this)[0] = r;
162  (*this)[1] = x * i;
163  (*this)[2] = y * i;
164  (*this)[3] = z * i;
165  ASSERTMSG_(
166  normSqr() - 1.0 < 1e-6,
167  mrpt::format(
168  "fromRodriguesVector() failed, tangent_vector=[%g %g %g]", v[0],
169  v[1], v[2]));
170  MRPT_END
171  }
172 
173  /** @name Lie Algebra methods
174  @{ */
175 
176  /** Logarithm of the 3x3 matrix defined by this pose, generating the
177  * corresponding vector in the SO(3) Lie Algebra,
178  * which coincides with the so-called "rotation vector" (I don't have
179  * space here for the proof ;-).
180  * \param[out] out_ln The target vector, which can be: std::vector<>, or
181  * mrpt::math::CVectorDouble or any row or column Eigen::Matrix<>.
182  * \sa exp, mrpt::poses::SE_traits */
183  template <class ARRAYLIKE3>
184  inline void ln(ARRAYLIKE3& out_ln) const
185  {
186  if (out_ln.size() != 3) out_ln.resize(3);
187  this->ln_noresize(out_ln);
188  }
189  /** overload that returns by value */
190  template <class ARRAYLIKE3>
191  inline ARRAYLIKE3 ln() const
192  {
193  ARRAYLIKE3 out_ln;
194  this->ln(out_ln);
195  return out_ln;
196  }
197  /** Like ln() but does not try to resize the output vector. */
198  template <class ARRAYLIKE3>
199  void ln_noresize(ARRAYLIKE3& out_ln) const
200  {
201  using mrpt::square;
202  const T xyz_norm = std::sqrt(square(x()) + square(y()) + square(z()));
203  const T K = (xyz_norm < 1e-7) ? 2 : 2 * ::acos(r()) / xyz_norm;
204  out_ln[0] = K * x();
205  out_ln[1] = K * y();
206  out_ln[2] = K * z();
207  }
208 
209  /** Exponential map from the SO(3) Lie Algebra to unit quaternions.
210  * \sa ln, mrpt::poses::SE_traits */
211  template <class ARRAYLIKE3>
212  inline static CQuaternion<T> exp(const ARRAYLIKE3& v)
213  {
215  q.fromRodriguesVector(v);
216  return q;
217  }
218  /** \overload */
219  template <class ARRAYLIKE3>
220  inline static void exp(const ARRAYLIKE3& v, CQuaternion<T>& out_quat)
221  {
222  out_quat.fromRodriguesVector(v);
223  }
224 
225  /** @} */ // end of Lie algebra
226 
227  /** Calculate the "cross" product (or "composed rotation") of two
228  * quaternion: this = q1 x q2
229  * After the operation, "this" will represent the composed rotations of
230  * q1 and q2 (q2 applied "after" q1).
231  */
232  inline void crossProduct(const CQuaternion& q1, const CQuaternion& q2)
233  {
234  // First: compute result, then save in this object. In this way we avoid
235  // problems when q1 or q2 == *this !!
236  const T new_r = q1.r() * q2.r() - q1.x() * q2.x() - q1.y() * q2.y() -
237  q1.z() * q2.z();
238  const T new_x = q1.r() * q2.x() + q2.r() * q1.x() + q1.y() * q2.z() -
239  q2.y() * q1.z();
240  const T new_y = q1.r() * q2.y() + q2.r() * q1.y() + q1.z() * q2.x() -
241  q2.z() * q1.x();
242  const T new_z = q1.r() * q2.z() + q2.r() * q1.z() + q1.x() * q2.y() -
243  q2.x() * q1.y();
244  this->r(new_r);
245  this->x(new_x);
246  this->y(new_y);
247  this->z(new_z);
248  this->normalize();
249  }
250 
251  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this
252  * quaternion
253  */
254  void rotatePoint(
255  const double lx, const double ly, const double lz, double& gx,
256  double& gy, double& gz) const
257  {
258  const double t2 = r() * x();
259  const double t3 = r() * y();
260  const double t4 = r() * z();
261  const double t5 = -x() * x();
262  const double t6 = x() * y();
263  const double t7 = x() * z();
264  const double t8 = -y() * y();
265  const double t9 = y() * z();
266  const double t10 = -z() * z();
267  gx = 2 * ((t8 + t10) * lx + (t6 - t4) * ly + (t3 + t7) * lz) + lx;
268  gy = 2 * ((t4 + t6) * lx + (t5 + t10) * ly + (t9 - t2) * lz) + ly;
269  gz = 2 * ((t7 - t3) * lx + (t2 + t9) * ly + (t5 + t8) * lz) + lz;
270  }
271 
272  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse
273  * (conjugate) of this quaternion
274  */
275  void inverseRotatePoint(
276  const double lx, const double ly, const double lz, double& gx,
277  double& gy, double& gz) const
278  {
279  const double t2 = -r() * x();
280  const double t3 = -r() * y();
281  const double t4 = -r() * z();
282  const double t5 = -x() * x();
283  const double t6 = x() * y();
284  const double t7 = x() * z();
285  const double t8 = -y() * y();
286  const double t9 = y() * z();
287  const double t10 = -z() * z();
288  gx = 2 * ((t8 + t10) * lx + (t6 - t4) * ly + (t3 + t7) * lz) + lx;
289  gy = 2 * ((t4 + t6) * lx + (t5 + t10) * ly + (t9 - t2) * lz) + ly;
290  gz = 2 * ((t7 - t3) * lx + (t2 + t9) * ly + (t5 + t8) * lz) + lz;
291  }
292 
293  /** Return the squared norm of the quaternion */
294  inline double normSqr() const
295  {
296  using mrpt::square;
297  return square(r()) + square(x()) + square(y()) + square(z());
298  }
299 
300  /** Normalize this quaternion, so its norm becomes the unitity.
301  */
302  inline void normalize()
303  {
305  const T qq = 1.0 / std::sqrt(normSqr());
306  for (unsigned int i = 0; i < 4; i++) (*this)[i] *= qq;
307  }
308 
309  /** Calculate the 4x4 Jacobian of the normalization operation of this
310  * quaternion.
311  * The output matrix can be a dynamic or fixed size (4x4) matrix.
312  */
313  template <class MATRIXLIKE>
314  void normalizationJacobian(MATRIXLIKE& J) const
315  {
316  const T n = 1.0 / std::pow(normSqr(), T(1.5));
317  J.setSize(4, 4);
318  J(0, 0) = x() * x() + y() * y() + z() * z();
319  J(0, 1) = -r() * x();
320  J(0, 2) = -r() * y();
321  J(0, 3) = -r() * z();
322 
323  J(1, 0) = -x() * r();
324  J(1, 1) = r() * r() + y() * y() + z() * z();
325  J(1, 2) = -x() * y();
326  J(1, 3) = -x() * z();
327 
328  J(2, 0) = -y() * r();
329  J(2, 1) = -y() * x();
330  J(2, 2) = r() * r() + x() * x() + z() * z();
331  J(2, 3) = -y() * z();
332 
333  J(3, 0) = -z() * r();
334  J(3, 1) = -z() * x();
335  J(3, 2) = -z() * y();
336  J(3, 3) = r() * r() + x() * x() + y() * y();
337  J *= n;
338  }
339 
340  /** Compute the Jacobian of the rotation composition operation \f$ p =
341  * f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$
342  * \frac{\partial f}{\partial q_{this} } \f$.
343  * The output matrix can be a dynamic or fixed size (4x4) matrix.
344  */
345  template <class MATRIXLIKE>
346  inline void rotationJacobian(MATRIXLIKE& J) const
347  {
348  J.setSize(4, 4);
349  J(0, 0) = r();
350  J(0, 1) = -x();
351  J(0, 2) = -y();
352  J(0, 3) = -z();
353  J(1, 0) = x();
354  J(1, 1) = r();
355  J(1, 2) = -z();
356  J(1, 3) = y();
357  J(2, 0) = y();
358  J(2, 1) = z();
359  J(2, 2) = r();
360  J(2, 3) = -x();
361  J(3, 0) = z();
362  J(3, 1) = -y();
363  J(3, 2) = x();
364  J(3, 3) = r();
365  }
366 
367  /** Calculate the 3x3 rotation matrix associated to this quaternion: \f[
368  * \mathbf{R} = \left(
369  * \begin{array}{ccc}
370  * q_r^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y - q_r q_z) & 2(q_z
371  * q_x+q_r q_y) \\
372  * 2(q_x q_y+q_r q_z) & q_r^2-q_x^2+q_y^2-q_z^2 & 2(q_y
373  * q_z-q_r q_x) \\
374  * 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 +
375  * q_z^2
376  * \end{array}
377  * \right)\f]
378  *
379  */
380  template <class MATRIXLIKE>
381  inline void rotationMatrix(MATRIXLIKE& M) const
382  {
383  M.setSize(3, 3);
385  }
386 
387  template <class MATRIXLIKE>
388  inline MATRIXLIKE rotationMatrix() const
389  {
390  MATRIXLIKE M(3, 3);
392  return M;
393  }
394 
395  /** Fill out the top-left 3x3 block of the given matrix with the rotation
396  * matrix associated to this quaternion (does not resize the matrix, for
397  * that, see rotationMatrix). */
398  template <class MATRIXLIKE>
399  inline void rotationMatrixNoResize(MATRIXLIKE& M) const
400  {
401  M(0, 0) = r() * r() + x() * x() - y() * y() - z() * z();
402  M(0, 1) = 2 * (x() * y() - r() * z());
403  M(0, 2) = 2 * (z() * x() + r() * y());
404  M(1, 0) = 2 * (x() * y() + r() * z());
405  M(1, 1) = r() * r() - x() * x() + y() * y() - z() * z();
406  M(1, 2) = 2 * (y() * z() - r() * x());
407  M(2, 0) = 2 * (z() * x() - r() * y());
408  M(2, 1) = 2 * (y() * z() + r() * x());
409  M(2, 2) = r() * r() - x() * x() - y() * y() + z() * z();
410  }
411 
412  /** Return the conjugate quaternion */
413  inline void conj(CQuaternion& q_out) const
414  {
415  q_out.r(r());
416  q_out.x(-x());
417  q_out.y(-y());
418  q_out.z(-z());
419  }
420 
421  /** Return the conjugate quaternion */
422  inline CQuaternion conj() const
423  {
424  CQuaternion q_aux;
425  conj(q_aux);
426  return q_aux;
427  }
428 
429  /** Return the yaw, pitch & roll angles associated to quaternion
430  * \sa For the equations, see The MRPT Book, or see
431  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
432  * \sa rpy_and_jacobian
433  */
434  inline void rpy(T& roll, T& pitch, T& yaw) const
435  {
437  roll, pitch, yaw, static_cast<mrpt::math::CMatrixDouble*>(nullptr));
438  }
439 
440  /** Return the yaw, pitch & roll angles associated to quaternion, and
441  * (optionally) the 3x4 Jacobian of the transformation.
442  * Note that both the angles and the Jacobian have one set of normal
443  * equations, plus other special formulas for the degenerated cases of
444  * |pitch|=90 degrees.
445  * \sa For the equations, see The MRPT Book, or
446  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
447  * \sa rpy
448  */
449  template <class MATRIXLIKE>
450  void rpy_and_jacobian(
451  T& roll, T& pitch, T& yaw, MATRIXLIKE* out_dr_dq = nullptr,
452  bool resize_out_dr_dq_to3x4 = true) const
453  {
454  using mrpt::square;
455  using std::sqrt;
456 
457  if (out_dr_dq && resize_out_dr_dq_to3x4) out_dr_dq->setSize(3, 4);
458  const T discr = r() * y() - x() * z();
459  if (discr > 0.49999)
460  { // pitch = 90 deg
461  pitch = 0.5 * M_PI;
462  yaw = -2 * atan2(x(), r());
463  roll = 0;
464  if (out_dr_dq)
465  {
466  out_dr_dq->setZero();
467  (*out_dr_dq)(0, 0) = +2 / x();
468  (*out_dr_dq)(0, 2) = -2 * r() / (x() * x());
469  }
470  }
471  else if (discr < -0.49999)
472  { // pitch =-90 deg
473  pitch = -0.5 * M_PI;
474  yaw = 2 * atan2(x(), r());
475  roll = 0;
476  if (out_dr_dq)
477  {
478  out_dr_dq->setZero();
479  (*out_dr_dq)(0, 0) = -2 / x();
480  (*out_dr_dq)(0, 2) = +2 * r() / (x() * x());
481  }
482  }
483  else
484  { // Non-degenerate case:
485  yaw = ::atan2(
486  2 * (r() * z() + x() * y()), 1 - 2 * (y() * y() + z() * z()));
487  pitch = ::asin(2 * discr);
488  roll = ::atan2(
489  2 * (r() * x() + y() * z()), 1 - 2 * (x() * x() + y() * y()));
490  if (out_dr_dq)
491  {
492  // Auxiliary terms:
493  const double val1 = (2 * x() * x() + 2 * y() * y() - 1);
494  const double val12 = square(val1);
495  const double val2 = (2 * r() * x() + 2 * y() * z());
496  const double val22 = square(val2);
497  const double xy2 = 2 * x() * y();
498  const double rz2 = 2 * r() * z();
499  const double ry2 = 2 * r() * y();
500  const double val3 = (2 * y() * y() + 2 * z() * z() - 1);
501  const double val4 =
502  ((square(rz2 + xy2) / square(val3) + 1) * val3);
503  const double val5 = (4 * (rz2 + xy2)) / square(val3);
504  const double val6 =
505  1.0 / (square(rz2 + xy2) / square(val3) + 1);
506  const double val7 = 2.0 / sqrt(1 - square(ry2 - 2 * x() * z()));
507  const double val8 = (val22 / val12 + 1);
508  const double val9 = -2.0 / val8;
509  // row 1:
510  (*out_dr_dq)(0, 0) = -2 * z() / val4;
511  (*out_dr_dq)(0, 1) = -2 * y() / val4;
512  (*out_dr_dq)(0, 2) = -(2 * x() / val3 - y() * val5) * val6;
513  (*out_dr_dq)(0, 3) = -(2 * r() / val3 - z() * val5) * val6;
514  // row 2:
515  (*out_dr_dq)(1, 0) = y() * val7;
516  (*out_dr_dq)(1, 1) = -z() * val7;
517  (*out_dr_dq)(1, 2) = r() * val7;
518  (*out_dr_dq)(1, 3) = -x() * val7;
519  // row 3:
520  (*out_dr_dq)(2, 0) = val9 * x() / val1;
521  (*out_dr_dq)(2, 1) =
522  val9 * (r() / val1 - (2 * x() * val2) / val12);
523  (*out_dr_dq)(2, 2) =
524  val9 * (z() / val1 - (2 * y() * val2) / val12);
525  (*out_dr_dq)(2, 3) = val9 * y() / val1;
526  }
527  }
528  }
530  inline CQuaternion operator*(const T& factor)
531  {
532  CQuaternion q = *this;
533  q *= factor;
534  return q;
535  }
536 
537 }; // end class
538 
539 /** A quaternion of data type "double" */
541 /** A quaternion of data type "float" */
543 
544 } // namespace mrpt::math
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, 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:449
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:274
static CQuaternion< T > exp(const ARRAYLIKE3 &v)
Exponential map from the SO(3) Lie Algebra to unit quaternions.
Definition: CQuaternion.h:211
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
TConstructorFlags_Quaternions
Definition: CQuaternion.h:20
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
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:398
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:68
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double normSqr() const
Return the squared norm of the quaternion.
Definition: CQuaternion.h:293
This base provides a set of functions for maths stuff.
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
CQuaternion operator*(const T &factor)
Definition: CQuaternion.h:529
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:231
void rotationJacobian(MATRIXLIKE &J) const
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...
Definition: CQuaternion.h:345
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
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:253
void ln_noresize(ARRAYLIKE3 &out_ln) const
Like ln() but does not try to resize the output vector.
Definition: CQuaternion.h:198
return_t square(const num_t x)
Inline function for the square of a number.
ARRAYLIKE3 ln() const
overload that returns by value
Definition: CQuaternion.h:190
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
const float R
#define MRPT_END
Definition: exceptions.h:245
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
T w() const
Return w (real part) coordinate of the quaternion.
Definition: CQuaternion.h:103
void ensurePositiveRealPart()
Adhere to the convention of w>=0 to avoid ambiguity of quaternion double cover of SO(3) ...
Definition: CQuaternion.h:88
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:433
CQuaternion conj() const
Return the conjugate quaternion.
Definition: CQuaternion.h:421
CQuaternion()
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.
Definition: CQuaternion.h:58
MATRIXLIKE rotationMatrix() const
Definition: CQuaternion.h:387
CQuaternion(TConstructorFlags_Quaternions)
Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quat...
Definition: CQuaternion.h:55
void fromRodriguesVector(const ARRAYLIKE3 &v)
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If ...
Definition: CQuaternion.h:141



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020