MRPT  1.9.9
CPose3DQuat.cpp
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 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose3DQuat.h>
17 #include <Eigen/Dense>
18 #include <iomanip>
19 #include <limits>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::poses;
25 
27 
28 /** Constructor from a CPose3D */
30 {
31  x() = p.x();
32  y() = p.y();
33  z() = p.z();
34  p.getAsQuaternion(m_quat);
35 }
36 
37 /** Constructor from a 4x4 homogeneous transformation matrix.
38  */
39 CPose3DQuat::CPose3DQuat(const CMatrixDouble44& M)
40  : m_quat(UNINITIALIZED_QUATERNION)
41 {
42  m_coords[0] = M(0, 3);
43  m_coords[1] = M(1, 3);
44  m_coords[2] = M(2, 3);
45  CPose3D p(M);
47 }
48 
49 /** Returns the corresponding 4x4 homogeneous transformation matrix for the
50  * point(translation) or pose (translation+orientation).
51  * \sa getInverseHomogeneousMatrix
52  */
54 {
56  out_HM(0, 3) = m_coords[0];
57  out_HM(1, 3) = m_coords[1];
58  out_HM(2, 3) = m_coords[2];
59  out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0;
60  out_HM(3, 3) = 1;
61 }
62 
63 /** Returns a 1x7 vector with [x y z qr qx qy qz] */
64 void CPose3DQuat::asVector(vector_t& v) const
65 {
66  v[0] = m_coords[0];
67  v[1] = m_coords[1];
68  v[2] = m_coords[2];
69  v[3] = m_quat[0];
70  v[4] = m_quat[1];
71  v[5] = m_quat[2];
72  v[6] = m_quat[3];
73 }
74 
75 /** Makes "this = A (+) B"; this method is slightly more efficient than "this=
76  * A + B;" since it avoids the temporary object.
77  * \note A or B can be "this" without problems.
78  */
80 {
81  // The 3D point:
82  double gx, gy, gz;
83  A.m_quat.rotatePoint(
84  B.m_coords[0], B.m_coords[1], B.m_coords[2], gx, gy, gz);
85  this->m_coords[0] = A.m_coords[0] + gx;
86  this->m_coords[1] = A.m_coords[1] + gy;
87  this->m_coords[2] = A.m_coords[2] + gz;
88 
89  // The 3D rotation:
90  this->m_quat.crossProduct(A.m_quat, B.m_quat);
91 }
92 
93 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
94  * than "this= A - B;" since it avoids the temporary object.
95  * \note A or B can be "this" without problems.
96  * \sa composeFrom
97  */
99 {
100  // The 3D point:
101  const CQuaternionDouble B_conj(
102  B.m_quat.r(), -B.m_quat.x(), -B.m_quat.y(), -B.m_quat.z());
103  B_conj.rotatePoint(
104  A.m_coords[0] - B.m_coords[0], A.m_coords[1] - B.m_coords[1],
105  A.m_coords[2] - B.m_coords[2], this->m_coords[0], this->m_coords[1],
106  this->m_coords[2]);
107  // The 3D rotation:
108  this->m_quat.crossProduct(B_conj, A.m_quat);
109 }
110 
111 /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
112  * \sa inverseComposeFrom
113  */
115  const double lx, const double ly, const double lz, double& gx, double& gy,
116  double& gz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
117  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
118 {
119  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
120  {
121  const double qx2 = square(m_quat.x());
122  const double qy2 = square(m_quat.y());
123  const double qz2 = square(m_quat.z());
124 
125  // Jacob: df/dpoint
126  if (out_jacobian_df_dpoint)
127  {
128  // 3x3: df_{qr} / da
129 
130  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
131  1 - 2 * (qy2 + qz2),
132  2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
133  2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
134 
135  2 * (m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
136  1 - 2 * (qx2 + qz2),
137  2 * (m_quat.y() * m_quat.z() - m_quat.r() * m_quat.x()),
138 
139  2 * (m_quat.x() * m_quat.z() - m_quat.r() * m_quat.y()),
140  2 * (m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
141  1 - 2 * (qx2 + qy2)};
142  out_jacobian_df_dpoint->loadFromArray(vals);
143  }
144 
145  // Jacob: df/dpose
146  if (out_jacobian_df_dpose)
147  {
148  // 3x7: df_{qr} / dp
149  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
150  1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
151  out_jacobian_df_dpose->loadFromArray(vals1);
152 
153  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
154  2 * (-m_quat.z() * ly + m_quat.y() * lz),
155  2 * (m_quat.y() * ly + m_quat.z() * lz),
156  2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
157  2 * (-2 * m_quat.z() * lx - m_quat.r() * ly + m_quat.x() * lz),
158 
159  2 * (m_quat.z() * lx - m_quat.x() * lz),
160  2 * (m_quat.y() * lx - 2 * m_quat.x() * ly - m_quat.r() * lz),
161  2 * (m_quat.x() * lx + m_quat.z() * lz),
162  2 * (m_quat.r() * lx - 2 * m_quat.z() * ly + m_quat.y() * lz),
163 
164  2 * (-m_quat.y() * lx + m_quat.x() * ly),
165  2 * (m_quat.z() * lx + m_quat.r() * ly - 2 * m_quat.x() * lz),
166  2 * (-m_quat.r() * lx + m_quat.z() * ly - 2 * m_quat.y() * lz),
167  2 * (m_quat.x() * lx + m_quat.y() * ly)};
168 
170  this->quat().normalizationJacobian(norm_jacob);
171 
172  out_jacobian_df_dpose->asEigen().block<3, 4>(0, 3) =
173  (CMatrixFixed<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, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
190  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
191 {
192  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
193  {
194  const double qx2 = square(m_quat.x());
195  const double qy2 = square(m_quat.y());
196  const double qz2 = square(m_quat.z());
197 
198  // Jacob: df/dpoint
199  if (out_jacobian_df_dpoint)
200  {
201  // 3x3: df_{m_quat.r()} / da
202  // inv_df_da =
203  // [ - 2*qy^2 - 2*qz^2 + 1, 2*qx*qy - 2*qr*qz, 2*qr*qy
204  //+
205  // 2*qx*qz]
206  // [ 2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1, 2*qy*qz
207  //-
208  // 2*qr*qx]
209  // [ 2*qx*qz - 2*qr*qy, 2*qr*qx + 2*qy*qz, - 2*qx^2 -
210  // 2*qy^2 + 1]
211  //
212 
213  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
214  1 - 2 * (qy2 + qz2),
215  2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
216  2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
217 
218  2 * (-m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
219  1 - 2 * (qx2 + qz2),
220  2 * (m_quat.y() * m_quat.z() + m_quat.r() * m_quat.x()),
221 
222  2 * (m_quat.x() * m_quat.z() + m_quat.r() * m_quat.y()),
223  2 * (-m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
224  1 - 2 * (qx2 + qy2)};
225  out_jacobian_df_dpoint->loadFromArray(vals);
226  }
227 
228  // Jacob: df/dpose
229  if (out_jacobian_df_dpose)
230  {
231  // 3x7: df_{m_quat.r()} / dp
232  // inv_df_dp =
233  //[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy, 2*qr*qy - 2*qx*qz,
234  // 2*qz*(ay - y) - 2*qy*(az - z), 2*qy*(ay - y) +
235  // 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z),
236  // 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
237  //[ 2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz,
238  // 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) +
239  // 2*qr*(az - z), 2*qx*(ax - x) + 2*qz*(az - z),
240  // 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
241  //[ - 2*qr*qy - 2*qx*qz, 2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1,
242  // 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) -
243  // 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z),
244  // 2*qx*(ax - x) + 2*qy*(ay - y)]
245  //
246  const double qr = m_quat.r();
247  const double qx = m_quat.x();
248  const double qy = m_quat.y();
249  const double qz = m_quat.z();
250 
251  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
252  2 * qy2 + 2 * qz2 - 1,
253  -2 * qr * qz - 2 * qx * qy,
254  2 * qr * qy - 2 * qx * qz,
255  0,
256  0,
257  0,
258  0,
259 
260  2 * qr * qz - 2 * qx * qy,
261  2 * qx2 + 2 * qz2 - 1,
262  -2 * qr * qx - 2 * qy * qz,
263  0,
264  0,
265  0,
266  0,
267 
268  -2 * qr * qy - 2 * qx * qz,
269  2 * qr * qx - 2 * qy * qz,
270  2 * qx2 + 2 * qy2 - 1,
271  0,
272  0,
273  0,
274  0,
275  };
276 
277  out_jacobian_df_dpose->loadFromArray(vals1);
278 
279  const double Ax = 2 * (gx - m_coords[0]);
280  const double Ay = 2 * (gy - m_coords[1]);
281  const double Az = 2 * (gz - m_coords[2]);
282 
283  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
284  const double vals[3 * 4] = {-qy * Az + qz * Ay,
285  qy * Ay + qz * Az,
286  qx * Ay - 2 * qy * Ax - qr * Az,
287  qx * Az + qr * Ay - 2 * qz * Ax,
288 
289  qx * Az - qz * Ax,
290  qy * Ax - 2 * qx * Ay + qr * Az,
291  qx * Ax + qz * Az,
292  qy * Az - 2 * qz * Ay - qr * Ax,
293 
294  qy * Ax - qx * Ay,
295  qz * Ax - qr * Ay - 2 * qx * Az,
296  qr * Ax + qz * Ay - 2 * qy * Az,
297  qx * Ax + qy * Ay};
298 
300  this->quat().normalizationJacobian(norm_jacob);
301 
302  out_jacobian_df_dpose->insertMatrix(
303  0, 3, (CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval());
304  }
305  }
306 
307  // function itself:
309  gx - m_coords[0], gy - m_coords[1], gz - m_coords[2], lx, ly, lz);
310 }
311 
312 /*---------------------------------------------------------------
313  *=
314  ---------------------------------------------------------------*/
315 void CPose3DQuat::operator*=(const double s)
316 {
317  m_coords[0] *= s;
318  m_coords[1] *= s;
319  m_coords[2] *= s;
320  m_quat[0] *= s;
321  m_quat[1] *= s;
322  m_quat[2] *= s;
323  m_quat[3] *= s;
324 }
325 
326 uint8_t CPose3DQuat::serializeGetVersion() const { return 0; }
328 {
329  out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1]
330  << m_quat[2] << m_quat[3];
331 }
333  mrpt::serialization::CArchive& in, uint8_t version)
334 {
335  switch (version)
336  {
337  case 0:
338  {
339  in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >>
340  m_quat[1] >> m_quat[2] >> m_quat[3];
341  }
342  break;
343  default:
345  };
346 }
347 /** Serialize CSerializable Object to CSchemeArchiveBase derived object*/
350 {
352  CPoint3D point(m_coords[0], m_coords[1], m_coords[2]);
353  out["point"] = point;
354  out["orientation"]["r"] = m_quat[0];
355  out["orientation"]["x"] = m_quat[1];
356  out["orientation"]["y"] = m_quat[2];
357  out["orientation"]["z"] = m_quat[3];
358 }
359 /** Serialize CSchemeArchiveBase derived object to CSerializable Object*/
361 {
362  uint8_t version;
364  switch (version)
365  {
366  case 1:
367  {
368  CPoint3D point;
369  in["point"].readTo(point);
370  m_coords[0] = point.x();
371  m_coords[1] = point.y();
372  m_coords[2] = point.z();
373  m_quat[0] = static_cast<double>(in["orientation"]["r"]);
374  m_quat[1] = static_cast<double>(in["orientation"]["x"]);
375  m_quat[2] = static_cast<double>(in["orientation"]["y"]);
376  m_quat[3] = static_cast<double>(in["orientation"]["z"]);
377  }
378  break;
379  default:
381  }
382 }
383 /*---------------------------------------------------------------
384  sphericalCoordinates
385 ---------------------------------------------------------------*/
387  const TPoint3D& point, double& out_range, double& out_yaw,
388  double& out_pitch,
389  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint,
390  mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose) const
391 {
392  const bool comp_jacobs =
393  out_jacob_dryp_dpoint != nullptr || out_jacob_dryp_dpose != nullptr;
394 
395  // Pass to coordinates as seen from this 6D pose:
396  CMatrixFixed<double, 3, 3> jacob_dinv_dpoint,
397  *ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : nullptr;
398  CMatrixFixed<double, 3, 7> jacob_dinv_dpose,
399  *ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : nullptr;
400 
401  TPoint3D local;
402  this->inverseComposePoint(
403  point.x, point.y, point.z, local.x, local.y, local.z, ptr_ja1, ptr_ja2);
404 
405  // Range:
406  out_range = local.norm();
407 
408  // Yaw:
409  if (local.y != 0 || local.x != 0)
410  out_yaw = atan2(local.y, local.x);
411  else
412  out_yaw = 0;
413 
414  // Pitch:
415  if (out_range != 0)
416  out_pitch = -asin(local.z / out_range);
417  else
418  out_pitch = 0;
419 
420  // Jacobians are:
421  // dryp_dpoint = dryp_dlocalpoint * dinv_dpoint
422  // dryp_dpose = dryp_dlocalpoint * dinv_dpose
423  if (comp_jacobs)
424  {
425  if (out_range == 0)
426  THROW_EXCEPTION("Jacobians are undefined for range=0");
427 
428  /* MATLAB:
429  syms H h_range h_yaw h_pitch real;
430  syms xi_ yi_ zi_ real;
431  h_range = sqrt(xi_^2+yi_^2+zi_^2);
432  h_yaw = atan(yi_/xi_);
433  % h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
434  h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
435  H=[ h_range ; h_yaw ; h_pitch ];
436  jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
437  */
438  const double _r = 1.0 / out_range;
439  const double x2 = square(local.x);
440  const double y2 = square(local.y);
441 
442  const double t2 = std::sqrt(x2 + y2);
443  const double _K = 1.0 / (t2 * square(out_range));
444 
445  double vals[3 * 3] = {local.x * _r,
446  local.y * _r,
447  local.z * _r,
448  -local.y / (x2 * (y2 / x2 + 1)),
449  1.0 / (local.x * (y2 / x2 + 1)),
450  0,
451  (local.x * local.z) * _K,
452  (local.y * local.z) * _K,
453  -t2 / square(out_range)};
454 
455  const CMatrixDouble33 dryp_dlocalpoint(vals);
456  if (out_jacob_dryp_dpoint)
457  *out_jacob_dryp_dpoint = dryp_dlocalpoint * jacob_dinv_dpoint;
458  if (out_jacob_dryp_dpose)
459  *out_jacob_dryp_dpose = dryp_dlocalpoint * jacob_dinv_dpose;
460  }
461 }
462 
463 /** Textual output stream function.
464  */
465 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DQuat& p)
466 {
467  const std::streamsize old_pre = o.precision();
468  const ios_base::fmtflags old_flags = o.flags();
469  o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
470  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
471  << p.quat()[0] << "," << p.quat()[1] << "," << p.quat()[2] << ","
472  << p.quat()[3] << ")";
473  o.flags(old_flags);
474  o.precision(old_pre);
475  return o;
476 }
477 
478 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
479  * than a pose with all its arguments multiplied by "-1") */
481 {
482  CPose3DQuat ret = p;
483  ret.inverse();
484  return ret;
485 }
486 
487 /** Convert this pose into its inverse, saving the result in itself. \sa
488  * operator- */
490 {
491  // Invert translation:
492  this->inverseComposePoint(0, 0, 0, m_coords[0], m_coords[1], m_coords[2]);
493 
494  // Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
495  m_quat[1] = -m_quat[1];
496  m_quat[2] = -m_quat[2];
497  m_quat[3] = -m_quat[3];
498 }
499 
501 {
502  for (int i = 0; i < 3; i++)
503  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
504 
505  for (int i = 0; i < 4; i++)
506  quat()[i] = std::numeric_limits<double>::quiet_NaN();
507 }
508 
510 {
511  return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
512  p1.z() == p2.z();
513 }
514 
516 {
517  return !(p1 == p2);
518 }
519 
521 {
522  CPoint3D L;
523  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
524  return L;
525 }
526 
528 {
530  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
531  return L;
532 }
533 
535 {
536  return TPose3DQuat(
537  x(), y(), z(), m_quat.r(), m_quat.x(), m_quat.y(), m_quat.z());
538 }
539 
540 void CPose3DQuat::fromString(const std::string& s)
541 {
543  if (!m.fromMatlabStringFormat(s))
544  THROW_EXCEPTION("Malformed expression in ::fromString");
545  ASSERTMSG_(m.rows() == 1 && m.cols() == 7, "Expected vector length=7");
546  m_coords[0] = m(0, 0);
547  m_coords[1] = m(0, 1);
548  m_coords[2] = m(0, 2);
549  m_quat[0] = m(0, 3);
550  m_quat[1] = m(0, 4);
551  m_quat[2] = m(0, 5);
552  m_quat[3] = m(0, 6);
553 }
554 
555 void CPose3DQuat::fromStringRaw(const std::string& s)
556 {
557  this->fromString("[" + s + "]");
558 }
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
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
void asVector(vector_t &v) const
Returns a 7x1 vector with [x y z qr qx qy qz]&#39;.
Definition: CPose3DQuat.cpp:64
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
const double G
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
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
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
mrpt::math::TPose3DQuat asTPose() const
STL namespace.
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixed< 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...
void setToNaN() override
Set all data fields to quiet NaN.
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:501
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
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
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
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 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:79
void serializeTo(mrpt::serialization::CArchive &out) const override
Serialize CSerializable Object to CSchemeArchiveBase derived object.
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:356
void inverse()
Convert this pose into its inverse, saving the result in itself.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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
virtual void operator*=(const double s)
Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
size_type rows() const
Number of rows in the matrix.
A class used to store a 3D point.
Definition: CPoint3D.h:31
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:128
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
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:53
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:52
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:98
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:54
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
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
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Serialize CSchemeArchiveBase derived object to CSerializable Object.



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