MRPT  1.9.9
CPose3DQuat.cpp
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 
10 #include "poses-precomp.h" // Precompiled headers
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPose3DQuat.h>
14 #include <iomanip>
15 #include <limits>
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::math;
20 using namespace mrpt::poses;
21 
23 
24 /** Constructor from a CPose3D */
26 {
27  x() = p.x();
28  y() = p.y();
29  z() = p.z();
30  p.getAsQuaternion(m_quat);
31 }
32 
33 /** Constructor from a 4x4 homogeneous transformation matrix.
34  */
35 CPose3DQuat::CPose3DQuat(const CMatrixDouble44& M)
36  : m_quat(UNINITIALIZED_QUATERNION)
37 {
38  m_coords[0] = M.get_unsafe(0, 3);
39  m_coords[1] = M.get_unsafe(1, 3);
40  m_coords[2] = M.get_unsafe(2, 3);
41  CPose3D p(M);
42  p.getAsQuaternion(m_quat);
43 }
44 
45 /** Returns the corresponding 4x4 homogeneous transformation matrix for the
46  * point(translation) or pose (translation+orientation).
47  * \sa getInverseHomogeneousMatrix
48  */
50 {
52  out_HM.get_unsafe(0, 3) = m_coords[0];
53  out_HM.get_unsafe(1, 3) = m_coords[1];
54  out_HM.get_unsafe(2, 3) = m_coords[2];
55  out_HM.get_unsafe(3, 0) = out_HM.get_unsafe(3, 1) =
56  out_HM.get_unsafe(3, 2) = 0;
57  out_HM.get_unsafe(3, 3) = 1;
58 }
59 
60 /** Returns a 1x7 vector with [x y z qr qx qy qz] */
62 {
63  v.resize(7);
64  v[0] = m_coords[0];
65  v[1] = m_coords[1];
66  v[2] = m_coords[2];
67  v[3] = m_quat[0];
68  v[4] = m_quat[1];
69  v[5] = m_quat[2];
70  v[6] = m_quat[3];
71 }
72 
73 /** Makes "this = A (+) B"; this method is slightly more efficient than "this=
74  * A + B;" since it avoids the temporary object.
75  * \note A or B can be "this" without problems.
76  */
78 {
79  // The 3D point:
80  double gx, gy, gz;
82  B.m_coords[0], B.m_coords[1], B.m_coords[2], gx, gy, gz);
83  this->m_coords[0] = A.m_coords[0] + gx;
84  this->m_coords[1] = A.m_coords[1] + gy;
85  this->m_coords[2] = A.m_coords[2] + gz;
86 
87  // The 3D rotation:
88  this->m_quat.crossProduct(A.m_quat, B.m_quat);
89 }
90 
91 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
92  * than "this= A - B;" since it avoids the temporary object.
93  * \note A or B can be "this" without problems.
94  * \sa composeFrom
95  */
97 {
98  // The 3D point:
99  const CQuaternionDouble B_conj(
100  B.m_quat.r(), -B.m_quat.x(), -B.m_quat.y(), -B.m_quat.z());
101  B_conj.rotatePoint(
102  A.m_coords[0] - B.m_coords[0], A.m_coords[1] - B.m_coords[1],
103  A.m_coords[2] - B.m_coords[2], this->m_coords[0], this->m_coords[1],
104  this->m_coords[2]);
105  // The 3D rotation:
106  this->m_quat.crossProduct(B_conj, A.m_quat);
107 }
108 
109 /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
110  * \sa inverseComposeFrom
111  */
113  const double lx, const double ly, const double lz, double& gx, double& gy,
114  double& gz,
115  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
116  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacobian_df_dpose) const
117 {
118  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
119  {
120  const double qx2 = square(m_quat.x());
121  const double qy2 = square(m_quat.y());
122  const double qz2 = square(m_quat.z());
123 
124  // Jacob: df/dpoint
125  if (out_jacobian_df_dpoint)
126  {
127  // 3x3: df_{qr} / da
128 
129  alignas(MRPT_MAX_ALIGN_BYTES) const double vals[3 * 3] = {
130  1 - 2 * (qy2 + qz2),
131  2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
132  2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
133 
134  2 * (m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
135  1 - 2 * (qx2 + qz2),
136  2 * (m_quat.y() * m_quat.z() - m_quat.r() * m_quat.x()),
137 
138  2 * (m_quat.x() * m_quat.z() - m_quat.r() * m_quat.y()),
139  2 * (m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
140  1 - 2 * (qx2 + qy2)};
141  out_jacobian_df_dpoint->loadFromArray(vals);
142  }
143 
144  // Jacob: df/dpose
145  if (out_jacobian_df_dpose)
146  {
147  // 3x7: df_{qr} / dp
148  alignas(MRPT_MAX_ALIGN_BYTES) const double vals1[3 * 7] = {
149  1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
150  out_jacobian_df_dpose->loadFromArray(vals1);
151 
152  alignas(MRPT_MAX_ALIGN_BYTES) const double vals[3 * 4] = {
153  2 * (-m_quat.z() * ly + m_quat.y() * lz),
154  2 * (m_quat.y() * ly + m_quat.z() * lz),
155  2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
156  2 * (-2 * m_quat.z() * lx - m_quat.r() * ly + m_quat.x() * lz),
157 
158  2 * (m_quat.z() * lx - m_quat.x() * lz),
159  2 * (m_quat.y() * lx - 2 * m_quat.x() * ly - m_quat.r() * lz),
160  2 * (m_quat.x() * lx + m_quat.z() * lz),
161  2 * (m_quat.r() * lx - 2 * m_quat.z() * ly + m_quat.y() * lz),
162 
163  2 * (-m_quat.y() * lx + m_quat.x() * ly),
164  2 * (m_quat.z() * lx + m_quat.r() * ly - 2 * m_quat.x() * lz),
165  2 * (-m_quat.r() * lx + m_quat.z() * ly - 2 * m_quat.y() * lz),
166  2 * (m_quat.x() * lx + m_quat.y() * ly)};
167 
169  this->quat().normalizationJacobian(norm_jacob);
170 
171  out_jacobian_df_dpose->insertMatrix(
172  0, 3,
173  (CMatrixFixedNumeric<double, 3, 4>(vals) * norm_jacob).eval());
174  }
175  }
176 
177  // function itself:
178  m_quat.rotatePoint(lx, ly, lz, gx, gy, gz);
179  gx += m_coords[0];
180  gy += m_coords[1];
181  gz += m_coords[2];
182 }
183 
184 /** Computes the 3D point G such as \f$ L = G \ominus this \f$.
185  * \sa composeFrom
186  */
188  const double gx, const double gy, const double gz, double& lx, double& ly,
189  double& lz,
190  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
191  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacobian_df_dpose) const
192 {
193  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
194  {
195  const double qx2 = square(m_quat.x());
196  const double qy2 = square(m_quat.y());
197  const double qz2 = square(m_quat.z());
198 
199  // Jacob: df/dpoint
200  if (out_jacobian_df_dpoint)
201  {
202  // 3x3: df_{m_quat.r()} / da
203  // inv_df_da =
204  // [ - 2*qy^2 - 2*qz^2 + 1, 2*qx*qy - 2*qr*qz, 2*qr*qy
205  //+
206  // 2*qx*qz]
207  // [ 2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1, 2*qy*qz
208  //-
209  // 2*qr*qx]
210  // [ 2*qx*qz - 2*qr*qy, 2*qr*qx + 2*qy*qz, - 2*qx^2 -
211  // 2*qy^2 + 1]
212  //
213 
214  alignas(MRPT_MAX_ALIGN_BYTES) const double vals[3 * 3] = {
215  1 - 2 * (qy2 + qz2),
216  2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
217  2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
218 
219  2 * (-m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
220  1 - 2 * (qx2 + qz2),
221  2 * (m_quat.y() * m_quat.z() + m_quat.r() * m_quat.x()),
222 
223  2 * (m_quat.x() * m_quat.z() + m_quat.r() * m_quat.y()),
224  2 * (-m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
225  1 - 2 * (qx2 + qy2)};
226  out_jacobian_df_dpoint->loadFromArray(vals);
227  }
228 
229  // Jacob: df/dpose
230  if (out_jacobian_df_dpose)
231  {
232  // 3x7: df_{m_quat.r()} / dp
233  // inv_df_dp =
234  //[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy, 2*qr*qy - 2*qx*qz,
235  // 2*qz*(ay - y) - 2*qy*(az - z), 2*qy*(ay - y) +
236  // 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z),
237  // 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
238  //[ 2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz,
239  // 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) +
240  // 2*qr*(az - z), 2*qx*(ax - x) + 2*qz*(az - z),
241  // 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
242  //[ - 2*qr*qy - 2*qx*qz, 2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1,
243  // 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) -
244  // 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z),
245  // 2*qx*(ax - x) + 2*qy*(ay - y)]
246  //
247  const double qr = m_quat.r();
248  const double qx = m_quat.x();
249  const double qy = m_quat.y();
250  const double qz = m_quat.z();
251 
252  alignas(MRPT_MAX_ALIGN_BYTES) const double vals1[3 * 7] = {
253  2 * qy2 + 2 * qz2 - 1,
254  -2 * qr * qz - 2 * qx * qy,
255  2 * qr * qy - 2 * qx * qz,
256  0,
257  0,
258  0,
259  0,
260 
261  2 * qr * qz - 2 * qx * qy,
262  2 * qx2 + 2 * qz2 - 1,
263  -2 * qr * qx - 2 * qy * qz,
264  0,
265  0,
266  0,
267  0,
268 
269  -2 * qr * qy - 2 * qx * qz,
270  2 * qr * qx - 2 * qy * qz,
271  2 * qx2 + 2 * qy2 - 1,
272  0,
273  0,
274  0,
275  0,
276  };
277 
278  out_jacobian_df_dpose->loadFromArray(vals1);
279 
280  const double Ax = 2 * (gx - m_coords[0]);
281  const double Ay = 2 * (gy - m_coords[1]);
282  const double Az = 2 * (gz - m_coords[2]);
283 
284  alignas(MRPT_MAX_ALIGN_BYTES)
285  const double vals[3 * 4] = {-qy * Az + qz * Ay,
286  qy * Ay + qz * Az,
287  qx * Ay - 2 * qy * Ax - qr * Az,
288  qx * Az + qr * Ay - 2 * qz * Ax,
289 
290  qx * Az - qz * Ax,
291  qy * Ax - 2 * qx * Ay + qr * Az,
292  qx * Ax + qz * Az,
293  qy * Az - 2 * qz * Ay - qr * Ax,
294 
295  qy * Ax - qx * Ay,
296  qz * Ax - qr * Ay - 2 * qx * Az,
297  qr * Ax + qz * Ay - 2 * qy * Az,
298  qx * Ax + qy * Ay};
299 
301  this->quat().normalizationJacobian(norm_jacob);
302 
303  out_jacobian_df_dpose->insertMatrix(
304  0, 3,
305  (CMatrixFixedNumeric<double, 3, 4>(vals) * norm_jacob).eval());
306  }
307  }
308 
309  // function itself:
311  gx - m_coords[0], gy - m_coords[1], gz - m_coords[2], lx, ly, lz);
312 }
313 
314 /*---------------------------------------------------------------
315  *=
316  ---------------------------------------------------------------*/
317 void CPose3DQuat::operator*=(const double s)
318 {
319  m_coords[0] *= s;
320  m_coords[1] *= s;
321  m_coords[2] *= s;
322  m_quat[0] *= s;
323  m_quat[1] *= s;
324  m_quat[2] *= s;
325  m_quat[3] *= s;
326 }
327 
330 {
331  out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1]
332  << m_quat[2] << m_quat[3];
333 }
336 {
337  switch (version)
338  {
339  case 0:
340  {
341  in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >>
342  m_quat[1] >> m_quat[2] >> m_quat[3];
343  }
344  break;
345  default:
347  };
348 }
349 
350 /*---------------------------------------------------------------
351  sphericalCoordinates
352 ---------------------------------------------------------------*/
354  const TPoint3D& point, double& out_range, double& out_yaw,
355  double& out_pitch,
356  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacob_dryp_dpoint,
357  mrpt::math::CMatrixFixedNumeric<double, 3, 7>* out_jacob_dryp_dpose) const
358 {
359  const bool comp_jacobs =
360  out_jacob_dryp_dpoint != nullptr || out_jacob_dryp_dpose != nullptr;
361 
362  // Pass to coordinates as seen from this 6D pose:
363  CMatrixFixedNumeric<double, 3, 3> jacob_dinv_dpoint,
364  *ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : nullptr;
365  CMatrixFixedNumeric<double, 3, 7> jacob_dinv_dpose,
366  *ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : nullptr;
367 
368  TPoint3D local;
369  this->inverseComposePoint(
370  point.x, point.y, point.z, local.x, local.y, local.z, ptr_ja1, ptr_ja2);
371 
372  // Range:
373  out_range = local.norm();
374 
375  // Yaw:
376  if (local.y != 0 || local.x != 0)
377  out_yaw = atan2(local.y, local.x);
378  else
379  out_yaw = 0;
380 
381  // Pitch:
382  if (out_range != 0)
383  out_pitch = -asin(local.z / out_range);
384  else
385  out_pitch = 0;
386 
387  // Jacobians are:
388  // dryp_dpoint = dryp_dlocalpoint * dinv_dpoint
389  // dryp_dpose = dryp_dlocalpoint * dinv_dpose
390  if (comp_jacobs)
391  {
392  if (out_range == 0)
393  THROW_EXCEPTION("Jacobians are undefined for range=0");
394 
395  /* MATLAB:
396  syms H h_range h_yaw h_pitch real;
397  syms xi_ yi_ zi_ real;
398  h_range = sqrt(xi_^2+yi_^2+zi_^2);
399  h_yaw = atan(yi_/xi_);
400  % h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
401  h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
402  H=[ h_range ; h_yaw ; h_pitch ];
403  jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
404  */
405  const double _r = 1.0 / out_range;
406  const double x2 = square(local.x);
407  const double y2 = square(local.y);
408 
409  const double t2 = std::sqrt(x2 + y2);
410  const double _K = 1.0 / (t2 * square(out_range));
411 
412  double vals[3 * 3] = {local.x * _r,
413  local.y * _r,
414  local.z * _r,
415  -local.y / (x2 * (y2 / x2 + 1)),
416  1.0 / (local.x * (y2 / x2 + 1)),
417  0,
418  (local.x * local.z) * _K,
419  (local.y * local.z) * _K,
420  -t2 / square(out_range)};
421 
422  const CMatrixDouble33 dryp_dlocalpoint(vals);
423  if (out_jacob_dryp_dpoint)
424  out_jacob_dryp_dpoint->multiply(
425  dryp_dlocalpoint, jacob_dinv_dpoint);
426  if (out_jacob_dryp_dpose)
427  out_jacob_dryp_dpose->multiply(dryp_dlocalpoint, jacob_dinv_dpose);
428  }
429 }
430 
431 /** Textual output stream function.
432  */
433 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DQuat& p)
434 {
435  const std::streamsize old_pre = o.precision();
436  const ios_base::fmtflags old_flags = o.flags();
437  o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
438  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
439  << p.quat()[0] << "," << p.quat()[1] << "," << p.quat()[2] << ","
440  << p.quat()[3] << ")";
441  o.flags(old_flags);
442  o.precision(old_pre);
443  return o;
444 }
445 
446 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
447  * than a pose with all its arguments multiplied by "-1") */
449 {
450  CPose3DQuat ret = p;
451  ret.inverse();
452  return ret;
453 }
454 
455 /** Convert this pose into its inverse, saving the result in itself. \sa
456  * operator- */
458 {
459  // Invert translation:
460  this->inverseComposePoint(0, 0, 0, m_coords[0], m_coords[1], m_coords[2]);
461 
462  // Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
463  m_quat[1] = -m_quat[1];
464  m_quat[2] = -m_quat[2];
465  m_quat[3] = -m_quat[3];
466 }
467 
469 {
470  for (int i = 0; i < 3; i++)
471  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
472 
473  for (int i = 0; i < 4; i++)
474  quat()[i] = std::numeric_limits<double>::quiet_NaN();
475 }
476 
478 {
479  return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
480  p1.z() == p2.z();
481 }
482 
484 {
485  return !(p1 == p2);
486 }
487 
489 {
490  CPoint3D L;
491  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
492  return L;
493 }
494 
496 {
498  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
499  return L;
500 }
501 
503 {
504  return TPose3DQuat(
505  x(), y(), z(), m_quat.r(), m_quat.x(), m_quat.y(), m_quat.z());
506 }
#define local
Definition: zutil.h:47
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x7 vector with [x y z qr qx qy qz].
Definition: CPose3DQuat.cpp:61
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:243
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
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 .
#define MRPT_MAX_ALIGN_BYTES
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:89
const double G
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:358
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
mrpt::math::TPose3DQuat asTPose() const
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...
STL namespace.
void setToNaN() override
Set all data fields to quiet NaN.
GLdouble s
Definition: glext.h:3676
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
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 .
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:85
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:200
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
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:172
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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
void inverse()
Convert this pose into its inverse, saving the result in itself.
double x
X,Y,Z coordinates.
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
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:222
virtual void operator*=(const double s)
Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:87
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:281
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:164
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
GLuint in
Definition: glext.h:7274
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
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:55
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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.
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:91
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLfloat GLfloat p
Definition: glext.h:6305
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



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