MRPT  2.0.4
CPose3D.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/config.h> // for HAVE_SINCOS
13 #include <mrpt/core/bits_math.h> // for square
14 #include <mrpt/math/CMatrixDynamic.h> // for CMatrixD...
15 #include <mrpt/math/CMatrixF.h> // for CMatrixF
16 #include <mrpt/math/CMatrixFixed.h> // for CMatrixF...
17 #include <mrpt/math/CQuaternion.h> // for CQuatern...
18 #include <mrpt/math/CVectorFixed.h> // for CArrayDo...
19 #include <mrpt/math/TPoint3D.h>
20 #include <mrpt/math/geometry.h> // for skew_sym...
21 #include <mrpt/math/homog_matrices.h> // for homogene...
22 #include <mrpt/math/matrix_serialization.h> // for operator>>
23 #include <mrpt/math/ops_containers.h> // for dotProduct
24 #include <mrpt/math/utils_matlab.h>
25 #include <mrpt/math/wrap2pi.h> // for wrapToPi
26 #include <mrpt/poses/CPoint2D.h> // for CPoint2D
27 #include <mrpt/poses/CPoint3D.h> // for CPoint3D
28 #include <mrpt/poses/CPose2D.h> // for CPose2D
29 #include <mrpt/poses/CPose3D.h> // for CPose3D
30 #include <mrpt/poses/CPose3DQuat.h> // for CPose3DQuat
31 #include <mrpt/poses/Lie/SO.h>
34 #include <mrpt/serialization/CSerializable.h> // for CSeriali...
35 #include <Eigen/Dense>
36 #include <algorithm> // for move
37 #include <cmath> // for fabs
38 #include <iomanip> // for operator<<
39 #include <limits> // for numeric_...
40 #include <ostream> // for operator<<
41 #include <string> // for allocator
42 
43 using namespace mrpt;
44 using namespace mrpt::math;
45 using namespace mrpt::poses;
46 
48 
49 /*---------------------------------------------------------------
50  Constructors
51  ---------------------------------------------------------------*/
53 {
54  m_coords[0] = m_coords[1] = m_coords[2] = 0;
55  m_ROT.setIdentity();
56 }
57 
58 CPose3D::CPose3D(
59  const double x, const double y, const double z, const double yaw,
60  const double pitch, const double roll)
61  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
62 {
63  setFromValues(x, y, z, yaw, pitch, roll);
64 }
65 
66 CPose3D::CPose3D(const mrpt::math::TPose3D& o) : m_ypr_uptodate(false)
67 {
68  setFromValues(o.x, o.y, o.z, o.yaw, o.pitch, o.roll);
69 }
70 
71 CPose3D::CPose3D(const CPose2D& p) : m_ypr_uptodate(false)
72 {
73  setFromValues(p.x(), p.y(), 0, p.phi(), 0, 0);
74 }
75 
77  : m_ypr_uptodate(false), m_yaw(), m_pitch(), m_roll()
78 {
79  setFromValues(p.x(), p.y(), p.z());
80 }
81 
83  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
84 {
85  ASSERT_ABOVEEQ_(m.rows(), 3);
86  ASSERT_ABOVEEQ_(m.cols(), 4);
87  for (int r = 0; r < 3; r++)
88  for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
89  for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
90 }
91 
93  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
94 {
95  for (int r = 0; r < 3; r++)
96  for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
97  for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
98 }
99 
100 /** Constructor from a quaternion (which only represents the 3D rotation part)
101  * and a 3D displacement. */
103  const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
104  const double _z)
105  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
106 {
107  double yaw, pitch, roll;
108  q.rpy(roll, pitch, yaw);
109  this->setFromValues(_x, _y, _z, yaw, pitch, roll);
110 }
111 
112 /** Constructor from a quaternion-based full pose. */
114  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
115 {
116  // Extract XYZ + ROT from quaternion:
117  m_coords[0] = p.x();
118  m_coords[1] = p.y();
119  m_coords[2] = p.z();
121 }
122 
123 uint8_t CPose3D::serializeGetVersion() const { return 3; }
125 {
126  // v2 serialized the equivalent CPose3DQuat representation.
127  // But this led to (**really** tiny) numerical differences between the
128  // original and reconstructed poses. To ensure bit-by-bit equivalence before
129  // and after serialization, let's get back to serializing the actual SO(3)
130  // matrix in serialization v3:
131  for (int i = 0; i < 3; i++) out << m_coords[i];
132  for (int r = 0; r < 3; r++)
133  for (int c = 0; c < 3; c++) out << m_ROT(r, c);
134 }
136 {
137  switch (version)
138  {
139  case 0:
140  {
141  // The coordinates:
142  CMatrixF HM2;
143  in >> HM2;
144  ASSERT_(HM2.rows() == 4 && HM2.isSquare());
145 
146  m_ROT = HM2.block<3, 3>(0, 0).cast<double>();
147 
148  m_coords[0] = HM2(0, 3);
149  m_coords[1] = HM2(1, 3);
150  m_coords[2] = HM2(2, 3);
151  }
152  break;
153  case 1:
154  {
155  // The coordinates:
156  CMatrixDouble44 HM;
157  in >> HM;
158 
159  m_ROT = HM.block<3, 3>(0, 0);
160 
161  m_coords[0] = HM(0, 3);
162  m_coords[1] = HM(1, 3);
163  m_coords[2] = HM(2, 3);
164  }
165  break;
166  case 2:
167  {
168  // An equivalent CPose3DQuat
170  in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];
171 
172  // Extract XYZ + ROT from quaternion:
173  m_coords[0] = p.x();
174  m_coords[1] = p.y();
175  m_coords[2] = p.z();
176  p.quat().rotationMatrixNoResize(m_ROT);
177  }
178  break;
179  case 3:
180  {
181  for (int i = 0; i < 3; i++) in >> m_coords[i];
182  for (int r = 0; r < 3; r++)
183  for (int c = 0; c < 3; c++) in >> m_ROT(r, c);
184  }
185  break;
186  default:
188  };
189  m_ypr_uptodate = false;
190 }
191 
193 {
195  out["x"] = m_coords[0];
196  out["y"] = m_coords[1];
197  out["z"] = m_coords[2];
198  out["rot"] = CMatrixD(m_ROT);
199 }
201 {
202  uint8_t version;
204  switch (version)
205  {
206  case 1:
207  {
208  m_coords[0] = static_cast<double>(in["x"]);
209  m_coords[1] = static_cast<double>(in["y"]);
210  m_coords[2] = static_cast<double>(in["z"]);
211  CMatrixD m;
212  in["rot"].readTo(m);
213  m_ROT = m;
214  }
215  break;
216  default:
218  }
219 }
220 
221 /** Textual output stream function.
222  */
223 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
224 {
225  const std::streamsize old_pre = o.precision();
226  const std::ios_base::fmtflags old_flags = o.flags();
227  o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
228  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
229  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
230  << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
231  o.flags(old_flags);
232  o.precision(old_pre);
233  return o;
234 }
235 
236 /*---------------------------------------------------------------
237  Implements the writing to a mxArray for Matlab
238  ---------------------------------------------------------------*/
239 #if MRPT_HAS_MATLAB
240 // Add to implement mexplus::from template specialization
242 #endif
243 
245 {
246 #if MRPT_HAS_MATLAB
247  const char* fields[] = {"R", "t"};
248  mexplus::MxArray pose_struct(
249  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
250  pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
251  pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
252  return pose_struct.release();
253 #else
254  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
255 #endif
256 }
257 
258 /*---------------------------------------------------------------
259  normalizeAngles
260 ---------------------------------------------------------------*/
262 /*---------------------------------------------------------------
263  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
264 ---------------------------------------------------------------*/
266  const double x0, const double y0, const double z0, const double yaw,
267  const double pitch, const double roll)
268 {
269  m_coords[0] = x0;
270  m_coords[1] = y0;
271  m_coords[2] = z0;
272  this->m_yaw = mrpt::math::wrapToPi(yaw);
273  this->m_pitch = mrpt::math::wrapToPi(pitch);
274  this->m_roll = mrpt::math::wrapToPi(roll);
275 
276  m_ypr_uptodate = true;
277 
279 }
280 
282 {
284 }
285 
286 /*---------------------------------------------------------------
287  Scalar multiplication.
288 ---------------------------------------------------------------*/
289 void CPose3D::operator*=(const double s)
290 {
292  m_coords[0] *= s;
293  m_coords[1] *= s;
294  m_coords[2] *= s;
295  m_yaw *= s;
296  m_pitch *= s;
297  m_roll *= s;
299 }
300 
301 /*---------------------------------------------------------------
302  getYawPitchRoll
303 ---------------------------------------------------------------*/
304 void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
305 {
306  TPose3D::SO3_to_yaw_pitch_roll(m_ROT, yaw, pitch, roll);
307 }
308 
309 /*---------------------------------------------------------------
310  sphericalCoordinates
311 ---------------------------------------------------------------*/
313  const TPoint3D& point, double& out_range, double& out_yaw,
314  double& out_pitch) const
315 {
316  // Pass to coordinates as seen from this 6D pose:
317  TPoint3D local;
318  this->inverseComposePoint(
319  point.x, point.y, point.z, local.x, local.y, local.z);
320 
321  // Range:
322  out_range = local.norm();
323 
324  // Yaw:
325  if (local.y != 0 || local.x != 0)
326  out_yaw = atan2(local.y, local.x);
327  else
328  out_yaw = 0;
329 
330  // Pitch:
331  if (out_range != 0)
332  out_pitch = -asin(local.z / out_range);
333  else
334  out_pitch = 0;
335 }
336 
338 {
339  return CPose3D(
340  -m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
341 }
342 
343 /*---------------------------------------------------------------
344  addComponents
345 ---------------------------------------------------------------*/
347 {
349  m_coords[0] += p.m_coords[0];
350  m_coords[1] += p.m_coords[1];
351  m_coords[2] += p.m_coords[2];
352  m_yaw += p.m_yaw;
353  m_pitch += p.m_pitch;
354  m_roll += p.m_roll;
356 }
357 
358 /*---------------------------------------------------------------
359  distanceEuclidean6D
360 ---------------------------------------------------------------*/
361 double CPose3D::distanceEuclidean6D(const CPose3D& o) const
362 {
364  o.updateYawPitchRoll();
365  return sqrt(
366  square(o.m_coords[0] - m_coords[0]) +
367  square(o.m_coords[1] - m_coords[1]) +
368  square(o.m_coords[2] - m_coords[2]) +
369  square(wrapToPi(o.m_yaw - m_yaw)) +
371  square(wrapToPi(o.m_roll - m_roll)));
372 }
373 
374 /*---------------------------------------------------------------
375  composePoint
376 ---------------------------------------------------------------*/
378  double lx, double ly, double lz, double& gx, double& gy, double& gz,
382  bool use_small_rot_approx) const
383 {
384  // Jacob: df/dpoint
385  if (out_jacobian_df_dpoint) out_jacobian_df_dpoint.value().get() = m_ROT;
386 
387  // Jacob: df/dpose
388  if (out_jacobian_df_dpose)
389  {
390  if (use_small_rot_approx)
391  {
392  // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
393  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
394  1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
395  out_jacobian_df_dpose.value().get().loadFromArray(nums);
396  }
397  else
398  {
399  // Exact Jacobians:
401 #ifdef HAVE_SINCOS
402  double cy, sy;
403  ::sincos(m_yaw, &sy, &cy);
404  double cp, sp;
405  ::sincos(m_pitch, &sp, &cp);
406  double cr, sr;
407  ::sincos(m_roll, &sr, &cr);
408 #else
409  const double cy = cos(m_yaw);
410  const double sy = sin(m_yaw);
411  const double cp = cos(m_pitch);
412  const double sp = sin(m_pitch);
413  const double cr = cos(m_roll);
414  const double sr = sin(m_roll);
415 #endif
416 
417  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
418  1,
419  0,
420  0,
421  -lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
422  lz * (-sy * sp * cr + cy * sr), // d_x'/d_yaw
423  -lx * cy * sp + ly * (cy * cp * sr) +
424  lz * (cy * cp * cr), // d_x'/d_pitch
425  ly * (cy * sp * cr + sy * sr) +
426  lz * (-cy * sp * sr + sy * cr), // d_x'/d_roll
427  0,
428  1,
429  0,
430  lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
431  lz * (cy * sp * cr + sy * sr), // d_y'/d_yaw
432  -lx * sy * sp + ly * (sy * cp * sr) +
433  lz * (sy * cp * cr), // d_y'/d_pitch
434  ly * (sy * sp * cr - cy * sr) +
435  lz * (-sy * sp * sr - cy * cr), // d_y'/d_roll
436  0,
437  0,
438  1,
439  0, // d_z' / d_yaw
440  -lx * cp - ly * sp * sr - lz * sp * cr, // d_z' / d_pitch
441  ly * cp * cr - lz * cp * sr // d_z' / d_roll
442  };
443  out_jacobian_df_dpose.value().get().loadFromArray(nums);
444  }
445  }
446 
447  gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
448  gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
449  gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];
450 
451  // Jacob: df/dse3
452  if (out_jacobian_df_dse3)
453  {
454  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
455  1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
456  out_jacobian_df_dse3.value().get().loadFromArray(nums);
457  }
458 }
459 
461  const mrpt::math::TVector3D& l) const
462 {
464  g.x = m_ROT(0, 0) * l.x + m_ROT(0, 1) * l.y + m_ROT(0, 2) * l.z;
465  g.y = m_ROT(1, 0) * l.x + m_ROT(1, 1) * l.y + m_ROT(1, 2) * l.z;
466  g.z = m_ROT(2, 0) * l.x + m_ROT(2, 1) * l.y + m_ROT(2, 2) * l.z;
467  return g;
468 }
469 
471  const mrpt::math::TVector3D& g) const
472 {
474  l.x = m_ROT(0, 0) * g.x + m_ROT(1, 0) * g.y + m_ROT(2, 0) * g.z;
475  l.y = m_ROT(0, 1) * g.x + m_ROT(1, 1) * g.y + m_ROT(2, 1) * g.z;
476  l.z = m_ROT(0, 2) * g.x + m_ROT(1, 2) * g.y + m_ROT(2, 2) * g.z;
477  return l;
478 }
479 
480 void CPose3D::asVector(vector_t& r) const
481 {
483  r[0] = m_coords[0];
484  r[1] = m_coords[1];
485  r[2] = m_coords[2];
486  r[3] = m_yaw;
487  r[4] = m_pitch;
488  r[5] = m_roll;
489 }
490 
491 /*---------------------------------------------------------------
492  unary -
493 ---------------------------------------------------------------*/
495 {
498  return CPose3D(B_INV);
499 }
500 
504 {
507  .getAsQuaternion(q, out_dq_dr);
508 }
509 
510 bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
511 {
512  return (p1.m_coords == p2.m_coords) &&
513  ((p1.getRotationMatrix() - p2.getRotationMatrix())
514  .array()
515  .abs()
516  .maxCoeff() < 1e-6);
517 }
518 
519 bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
520 {
521  return (p1.m_coords != p2.m_coords) ||
522  ((p1.getRotationMatrix() - p2.getRotationMatrix())
523  .array()
524  .abs()
525  .maxCoeff() >= 1e-6);
526 }
527 
528 /*---------------------------------------------------------------
529  point3D = pose3D + point3D
530  ---------------------------------------------------------------*/
532 {
533  return CPoint3D(
534  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
535  m_ROT(0, 2) * b.z(),
536  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
537  m_ROT(1, 2) * b.z(),
538  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
539  m_ROT(2, 2) * b.z());
540 }
541 
542 /*---------------------------------------------------------------
543  point3D = pose3D + point2D
544  ---------------------------------------------------------------*/
546 {
547  return CPoint3D(
548  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
549  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
550  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
551 }
552 
553 /*---------------------------------------------------------------
554  this = A + B
555  ---------------------------------------------------------------*/
556 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
557 {
558  // The translation part HM(0:3,3)
559  if (this == &B)
560  {
561  // we need to make a temporary copy of the vector:
562  const CVectorFixedDouble<3> B_coords = B.m_coords;
563  for (int r = 0; r < 3; r++)
564  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
565  A.m_ROT(r, 1) * B_coords[1] +
566  A.m_ROT(r, 2) * B_coords[2];
567  }
568  else
569  {
570  for (int r = 0; r < 3; r++)
571  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
572  A.m_ROT(r, 1) * B.m_coords[1] +
573  A.m_ROT(r, 2) * B.m_coords[2];
574  }
575 
576  // Important: Make this multiplication AFTER the translational part, to cope
577  // with the case when A==this
578  m_ROT = A.m_ROT * B.m_ROT;
579 
580  m_ypr_uptodate = false;
581 }
582 
583 /** Convert this pose into its inverse, saving the result in itself. */
585 {
587  CVectorFixedDouble<3> inv_xyz;
588 
590 
591  m_ROT = inv_rot;
592  m_coords = inv_xyz;
593  m_ypr_uptodate = false;
594 }
595 
596 /*---------------------------------------------------------------
597  isHorizontal
598  ---------------------------------------------------------------*/
599 bool CPose3D::isHorizontal(const double tolerance) const
600 {
602  return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
603  (fabs(m_roll) <= tolerance ||
604  fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
605 }
606 
607 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
608  * than "this= A - B;" since it avoids the temporary object.
609  * \note A or B can be "this" without problems.
610  * \sa composeFrom, composePoint
611  */
613 {
614  // this = A (-) B
615  // HM_this = inv(HM_B) * HM_A
616  //
617  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
618  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
619  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
620  //
621 
622  // XYZ part:
624  CVectorFixedDouble<3> t_b_inv;
625  mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);
626 
627  for (int i = 0; i < 3; i++)
628  m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
629  R_b_inv(i, 1) * A.m_coords[1] +
630  R_b_inv(i, 2) * A.m_coords[2];
631 
632  // Rot part:
633  m_ROT = R_b_inv * A.m_ROT;
634  m_ypr_uptodate = false;
635 }
636 
637 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
638  * \sa composePoint, composeFrom
639  */
641  const double gx, const double gy, const double gz, double& lx, double& ly,
642  double& lz,
645  mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3) const
646 {
648  CVectorFixedDouble<3> t_inv;
650 
651  // Jacob: df/dpoint
652  if (out_jacobian_df_dpoint) out_jacobian_df_dpoint.value().get() = R_inv;
653 
654  // Jacob: df/dpose
655  if (out_jacobian_df_dpose)
656  {
657  // TODO: Perhaps this and the sin/cos's can be avoided if all needed
658  // terms are already in m_ROT ???
660 
661 #ifdef HAVE_SINCOS
662  double cy, sy;
663  ::sincos(m_yaw, &sy, &cy);
664  double cp, sp;
665  ::sincos(m_pitch, &sp, &cp);
666  double cr, sr;
667  ::sincos(m_roll, &sr, &cr);
668 #else
669  const double cy = cos(m_yaw);
670  const double sy = sin(m_yaw);
671  const double cp = cos(m_pitch);
672  const double sp = sin(m_pitch);
673  const double cr = cos(m_roll);
674  const double sr = sin(m_roll);
675 #endif
676 
677  const double m11_dy = -sy * cp;
678  const double m12_dy = cy * cp;
679  const double m13_dy = 0;
680  const double m11_dp = -cy * sp;
681  const double m12_dp = -sy * sp;
682  const double m13_dp = -cp;
683  const double m11_dr = 0;
684  const double m12_dr = 0;
685  const double m13_dr = 0;
686 
687  const double m21_dy = (-sy * sp * sr - cy * cr);
688  const double m22_dy = (cy * sp * sr - sy * cr);
689  const double m23_dy = 0;
690  const double m21_dp = (cy * cp * sr);
691  const double m22_dp = (sy * cp * sr);
692  const double m23_dp = -sp * sr;
693  const double m21_dr = (cy * sp * cr + sy * sr);
694  const double m22_dr = (sy * sp * cr - cy * sr);
695  const double m23_dr = cp * cr;
696 
697  const double m31_dy = (-sy * sp * cr + cy * sr);
698  const double m32_dy = (cy * sp * cr + sy * sr);
699  const double m33_dy = 0;
700  const double m31_dp = (cy * cp * cr);
701  const double m32_dp = (sy * cp * cr);
702  const double m33_dp = -sp * cr;
703  const double m31_dr = (-cy * sp * sr + sy * cr);
704  const double m32_dr = (-sy * sp * sr - cy * cr);
705  const double m33_dr = -cp * sr;
706 
707  const double Ax = gx - m_coords[0];
708  const double Ay = gy - m_coords[1];
709  const double Az = gz - m_coords[2];
710 
711  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
712  -m_ROT(0, 0),
713  -m_ROT(1, 0),
714  -m_ROT(2, 0),
715  Ax * m11_dy + Ay * m12_dy + Az * m13_dy, // d_x'/d_yaw
716  Ax * m11_dp + Ay * m12_dp + Az * m13_dp, // d_x'/d_pitch
717  Ax * m11_dr + Ay * m12_dr + Az * m13_dr, // d_x'/d_roll
718 
719  -m_ROT(0, 1),
720  -m_ROT(1, 1),
721  -m_ROT(2, 1),
722  Ax * m21_dy + Ay * m22_dy + Az * m23_dy, // d_x'/d_yaw
723  Ax * m21_dp + Ay * m22_dp + Az * m23_dp, // d_x'/d_pitch
724  Ax * m21_dr + Ay * m22_dr + Az * m23_dr, // d_x'/d_roll
725 
726  -m_ROT(0, 2),
727  -m_ROT(1, 2),
728  -m_ROT(2, 2),
729  Ax * m31_dy + Ay * m32_dy + Az * m33_dy, // d_x'/d_yaw
730  Ax * m31_dp + Ay * m32_dp + Az * m33_dp, // d_x'/d_pitch
731  Ax * m31_dr + Ay * m32_dr + Az * m33_dr, // d_x'/d_roll
732  };
733  out_jacobian_df_dpose.value().get().loadFromArray(nums);
734  }
735 
736  lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
737  ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
738  lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;
739 
740  // Jacob: df/dse3
741  if (out_jacobian_df_dse3)
742  {
743  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
744  -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
745  out_jacobian_df_dse3.value().get().loadFromArray(nums);
746  }
747 }
748 
750 {
751  for (int i = 0; i < 3; i++)
752  for (int j = 0; j < 3; j++)
753  m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();
754 
755  for (int i = 0; i < 3; i++)
756  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
757 }
758 
760 {
761  return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
762 }
763 
764 void CPose3D::fromString(const std::string& s)
765 {
766  using mrpt::DEG2RAD;
768  if (!m.fromMatlabStringFormat(s))
769  THROW_EXCEPTION("Malformed expression in ::fromString");
770  ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
771  this->setFromValues(
772  m(0, 0), m(0, 1), m(0, 2), DEG2RAD(m(0, 3)), DEG2RAD(m(0, 4)),
773  DEG2RAD(m(0, 5)));
774 }
775 
776 void CPose3D::fromStringRaw(const std::string& s)
777 {
778  this->fromString("[" + s + "]");
779 }
780 
782 {
783  auto M = out_HM.asEigen();
784  M.block<3, 3>(0, 0) = m_ROT.asEigen();
785  for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
786  out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
787  out_HM(3, 3) = 1.;
788 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
void inverseComposeFrom(const CPose3D &A, const CPose3D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose3D.cpp:612
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose3D.cpp:337
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:97
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It&#39;s not safe to set t...
Definition: CPose3D.h:102
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose3D.cpp:749
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards)...
Definition: CPose3D.cpp:599
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
Definition: CPose3D.h:106
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose3D.cpp:123
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
This file implements several operations that operate element-wise on individual or pairs of container...
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
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
Definition: CPose3D.h:249
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
Definition: CPose3D.cpp:281
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
Definition: CPose3D.h:116
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:584
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
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
Definition: CPose3D.cpp:460
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
Definition: CPose3D.cpp:361
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
This base provides a set of functions for maths stuff.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:556
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:290
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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 addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Definition: CPose3D.cpp:346
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
Definition: CPose3D.cpp:312
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: TPose3D.cpp:42
#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
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose3D.cpp:135
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 asVector(vector_t &v) const
Returns a 6x1 vector with [x y z yaw pitch roll]&#39;.
Definition: CPose3D.cpp:480
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
double m_yaw
These variables are updated every time that the object rotation matrix is modified (construction...
Definition: CPose3D.h:110
size_type rows() const
Number of rows in the matrix.
A class used to store a 2D point.
Definition: CPoint2D.h:32
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose3D.cpp:776
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
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.
void operator*=(const double s)
Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).
Definition: CPose3D.cpp:289
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
Definition: CPose3D.cpp:304
mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const
Inverse of rotateVector(), i.e.
Definition: CPose3D.cpp:470
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.cpp:764
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
virtual mxArray * writeToMatlab() const
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.
Definition: CSerializable.h:90
This file implements matrix/vector text and binary serialization.
constexpr double RAD2DEG(const double x)
Radians to degrees.
CPose3D()
Default constructor, with all the coordinates set to zero.
Definition: CPose3D.cpp:52
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:261
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
#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
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
Traits for SO(n), rotations in R^n space.
Definition: SO.h:21
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:781
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:433
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3D.cpp:124
mxArray * convertToMatlab(const MATRIX &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:35



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020