MRPT  1.9.9
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-2019, 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();
120  p.quat().rotationMatrixNoResize(m_ROT);
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 // TODO: Use SSE2? OTOH, this forces mem align...
481 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
482 /*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
483 {
484  ASSERT_(((size_t)matrix & 15) == 0);
485  __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
486 _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
487  __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
488 _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
489  __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
490 _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
491 
492  return _mm_add_ps(_mm_add_ps(a0,a1),a2);
493 }*/
494 #endif // SSE2
495 
496 void CPose3D::asVector(vector_t& r) const
497 {
499  r[0] = m_coords[0];
500  r[1] = m_coords[1];
501  r[2] = m_coords[2];
502  r[3] = m_yaw;
503  r[4] = m_pitch;
504  r[5] = m_roll;
505 }
506 
507 /*---------------------------------------------------------------
508  unary -
509 ---------------------------------------------------------------*/
511 {
513  b.getInverseHomogeneousMatrix(B_INV);
514  return CPose3D(B_INV);
515 }
516 
520 {
523  .getAsQuaternion(q, out_dq_dr);
524 }
525 
526 bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
527 {
528  return (p1.m_coords == p2.m_coords) &&
529  ((p1.getRotationMatrix() - p2.getRotationMatrix())
530  .array()
531  .abs()
532  .maxCoeff() < 1e-6);
533 }
534 
535 bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
536 {
537  return (p1.m_coords != p2.m_coords) ||
538  ((p1.getRotationMatrix() - p2.getRotationMatrix())
539  .array()
540  .abs()
541  .maxCoeff() >= 1e-6);
542 }
543 
544 /*---------------------------------------------------------------
545  point3D = pose3D + point3D
546  ---------------------------------------------------------------*/
548 {
549  return CPoint3D(
550  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
551  m_ROT(0, 2) * b.z(),
552  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
553  m_ROT(1, 2) * b.z(),
554  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
555  m_ROT(2, 2) * b.z());
556 }
557 
558 /*---------------------------------------------------------------
559  point3D = pose3D + point2D
560  ---------------------------------------------------------------*/
562 {
563  return CPoint3D(
564  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
565  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
566  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
567 }
568 
569 /*---------------------------------------------------------------
570  this = A + B
571  ---------------------------------------------------------------*/
572 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
573 {
574  // The translation part HM(0:3,3)
575  if (this == &B)
576  {
577  // we need to make a temporary copy of the vector:
578  const CVectorFixedDouble<3> B_coords = B.m_coords;
579  for (int r = 0; r < 3; r++)
580  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
581  A.m_ROT(r, 1) * B_coords[1] +
582  A.m_ROT(r, 2) * B_coords[2];
583  }
584  else
585  {
586  for (int r = 0; r < 3; r++)
587  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
588  A.m_ROT(r, 1) * B.m_coords[1] +
589  A.m_ROT(r, 2) * B.m_coords[2];
590  }
591 
592  // Important: Make this multiplication AFTER the translational part, to cope
593  // with the case when A==this
594  m_ROT = A.m_ROT * B.m_ROT;
595 
596  m_ypr_uptodate = false;
597 }
598 
599 /** Convert this pose into its inverse, saving the result in itself. */
601 {
603  CVectorFixedDouble<3> inv_xyz;
604 
606 
607  m_ROT = inv_rot;
608  m_coords = inv_xyz;
609  m_ypr_uptodate = false;
610 }
611 
612 /*---------------------------------------------------------------
613  isHorizontal
614  ---------------------------------------------------------------*/
615 bool CPose3D::isHorizontal(const double tolerance) const
616 {
618  return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
619  (fabs(m_roll) <= tolerance ||
620  fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
621 }
622 
623 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
624  * than "this= A - B;" since it avoids the temporary object.
625  * \note A or B can be "this" without problems.
626  * \sa composeFrom, composePoint
627  */
629 {
630  // this = A (-) B
631  // HM_this = inv(HM_B) * HM_A
632  //
633  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
634  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
635  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
636  //
637 
638  // XYZ part:
640  CVectorFixedDouble<3> t_b_inv;
641  mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);
642 
643  for (int i = 0; i < 3; i++)
644  m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
645  R_b_inv(i, 1) * A.m_coords[1] +
646  R_b_inv(i, 2) * A.m_coords[2];
647 
648  // Rot part:
649  m_ROT = R_b_inv * A.m_ROT;
650  m_ypr_uptodate = false;
651 }
652 
653 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
654  * \sa composePoint, composeFrom
655  */
657  const double gx, const double gy, const double gz, double& lx, double& ly,
658  double& lz,
661  mrpt::optional_ref<mrpt::math::CMatrixDouble36> out_jacobian_df_dse3) const
662 {
664  CVectorFixedDouble<3> t_inv;
666 
667  // Jacob: df/dpoint
668  if (out_jacobian_df_dpoint) out_jacobian_df_dpoint.value().get() = R_inv;
669 
670  // Jacob: df/dpose
671  if (out_jacobian_df_dpose)
672  {
673  // TODO: Perhaps this and the sin/cos's can be avoided if all needed
674  // terms are already in m_ROT ???
676 
677 #ifdef HAVE_SINCOS
678  double cy, sy;
679  ::sincos(m_yaw, &sy, &cy);
680  double cp, sp;
681  ::sincos(m_pitch, &sp, &cp);
682  double cr, sr;
683  ::sincos(m_roll, &sr, &cr);
684 #else
685  const double cy = cos(m_yaw);
686  const double sy = sin(m_yaw);
687  const double cp = cos(m_pitch);
688  const double sp = sin(m_pitch);
689  const double cr = cos(m_roll);
690  const double sr = sin(m_roll);
691 #endif
692 
693  const double m11_dy = -sy * cp;
694  const double m12_dy = cy * cp;
695  const double m13_dy = 0;
696  const double m11_dp = -cy * sp;
697  const double m12_dp = -sy * sp;
698  const double m13_dp = -cp;
699  const double m11_dr = 0;
700  const double m12_dr = 0;
701  const double m13_dr = 0;
702 
703  const double m21_dy = (-sy * sp * sr - cy * cr);
704  const double m22_dy = (cy * sp * sr - sy * cr);
705  const double m23_dy = 0;
706  const double m21_dp = (cy * cp * sr);
707  const double m22_dp = (sy * cp * sr);
708  const double m23_dp = -sp * sr;
709  const double m21_dr = (cy * sp * cr + sy * sr);
710  const double m22_dr = (sy * sp * cr - cy * sr);
711  const double m23_dr = cp * cr;
712 
713  const double m31_dy = (-sy * sp * cr + cy * sr);
714  const double m32_dy = (cy * sp * cr + sy * sr);
715  const double m33_dy = 0;
716  const double m31_dp = (cy * cp * cr);
717  const double m32_dp = (sy * cp * cr);
718  const double m33_dp = -sp * cr;
719  const double m31_dr = (-cy * sp * sr + sy * cr);
720  const double m32_dr = (-sy * sp * sr - cy * cr);
721  const double m33_dr = -cp * sr;
722 
723  const double Ax = gx - m_coords[0];
724  const double Ay = gy - m_coords[1];
725  const double Az = gz - m_coords[2];
726 
727  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
728  -m_ROT(0, 0),
729  -m_ROT(1, 0),
730  -m_ROT(2, 0),
731  Ax * m11_dy + Ay * m12_dy + Az * m13_dy, // d_x'/d_yaw
732  Ax * m11_dp + Ay * m12_dp + Az * m13_dp, // d_x'/d_pitch
733  Ax * m11_dr + Ay * m12_dr + Az * m13_dr, // d_x'/d_roll
734 
735  -m_ROT(0, 1),
736  -m_ROT(1, 1),
737  -m_ROT(2, 1),
738  Ax * m21_dy + Ay * m22_dy + Az * m23_dy, // d_x'/d_yaw
739  Ax * m21_dp + Ay * m22_dp + Az * m23_dp, // d_x'/d_pitch
740  Ax * m21_dr + Ay * m22_dr + Az * m23_dr, // d_x'/d_roll
741 
742  -m_ROT(0, 2),
743  -m_ROT(1, 2),
744  -m_ROT(2, 2),
745  Ax * m31_dy + Ay * m32_dy + Az * m33_dy, // d_x'/d_yaw
746  Ax * m31_dp + Ay * m32_dp + Az * m33_dp, // d_x'/d_pitch
747  Ax * m31_dr + Ay * m32_dr + Az * m33_dr, // d_x'/d_roll
748  };
749  out_jacobian_df_dpose.value().get().loadFromArray(nums);
750  }
751 
752  lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
753  ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
754  lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;
755 
756  // Jacob: df/dse3
757  if (out_jacobian_df_dse3)
758  {
759  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
760  -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
761  out_jacobian_df_dse3.value().get().loadFromArray(nums);
762  }
763 }
764 
766 {
767  for (int i = 0; i < 3; i++)
768  for (int j = 0; j < 3; j++)
769  m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();
770 
771  for (int i = 0; i < 3; i++)
772  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
773 }
774 
776 {
777  return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
778 }
779 
781 {
782  using mrpt::DEG2RAD;
784  if (!m.fromMatlabStringFormat(s))
785  THROW_EXCEPTION("Malformed expression in ::fromString");
786  ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
787  this->setFromValues(
788  m(0, 0), m(0, 1), m(0, 2), DEG2RAD(m(0, 3)), DEG2RAD(m(0, 4)),
789  DEG2RAD(m(0, 5)));
790 }
791 
793 {
794  this->fromString("[" + s + "]");
795 }
796 
798 {
799  auto M = out_HM.asEigen();
800  M.block<3, 3>(0, 0) = m_ROT.asEigen();
801  for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
802  out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
803  out_HM(3, 3) = 1.;
804 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
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:628
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
GLdouble GLdouble z
Definition: glext.h:3879
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
#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:765
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:615
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
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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
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) ...
This file implements several operations that operate element-wise on individual or pairs of container...
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:177
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
GLdouble s
Definition: glext.h:3682
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:600
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:517
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:572
T square(const T x)
Inline function for the square of a number.
constexpr double DEG2RAD(const double x)
Degrees to radians.
const GLubyte * c
Definition: glext.h:6406
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
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
#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:496
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
GLsizei const GLchar ** string
Definition: glext.h:4116
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:792
#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
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
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:780
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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
GLuint in
Definition: glext.h:7391
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
GLenum GLint GLint y
Definition: glext.h:3542
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...
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLenum GLint x
Definition: glext.h:3542
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
Lightweight 3D point.
Definition: TPoint3D.h:90
Traits for SO(n), rotations in R^n space.
Definition: SO.h:21
GLfloat GLfloat p
Definition: glext.h:6398
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:797
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 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019