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 
125 {
126  const CPose3DQuat q(*this);
127  // The coordinates:
128  out << q[0] << q[1] << q[2] << q[3] << q[4] << q[5] << q[6];
129 }
131 {
132  switch (version)
133  {
134  case 0:
135  {
136  // The coordinates:
137  CMatrixF HM2;
138  in >> HM2;
139  ASSERT_(HM2.rows() == 4 && HM2.isSquare());
140 
141  m_ROT = HM2.block<3, 3>(0, 0).cast<double>();
142 
143  m_coords[0] = HM2(0, 3);
144  m_coords[1] = HM2(1, 3);
145  m_coords[2] = HM2(2, 3);
146  m_ypr_uptodate = false;
147  }
148  break;
149  case 1:
150  {
151  // The coordinates:
152  CMatrixDouble44 HM;
153  in >> HM;
154 
155  m_ROT = HM.block<3, 3>(0, 0);
156 
157  m_coords[0] = HM(0, 3);
158  m_coords[1] = HM(1, 3);
159  m_coords[2] = HM(2, 3);
160  m_ypr_uptodate = false;
161  }
162  break;
163  case 2:
164  {
165  // An equivalent CPose3DQuat
167  in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];
168 
169  // Extract XYZ + ROT from quaternion:
170  m_ypr_uptodate = false;
171  m_coords[0] = p.x();
172  m_coords[1] = p.y();
173  m_coords[2] = p.z();
174  p.quat().rotationMatrixNoResize(m_ROT);
175  }
176  break;
177  default:
179  };
180 }
181 
183 {
185  out["x"] = m_coords[0];
186  out["y"] = m_coords[1];
187  out["z"] = m_coords[2];
188  out["rot"] = CMatrixD(m_ROT);
189 }
191 {
192  uint8_t version;
194  switch (version)
195  {
196  case 1:
197  {
198  m_coords[0] = static_cast<double>(in["x"]);
199  m_coords[1] = static_cast<double>(in["y"]);
200  m_coords[2] = static_cast<double>(in["z"]);
201  CMatrixD m;
202  in["rot"].readTo(m);
203  m_ROT = m;
204  }
205  break;
206  default:
208  }
209 }
210 
211 /** Textual output stream function.
212  */
213 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
214 {
215  const std::streamsize old_pre = o.precision();
216  const std::ios_base::fmtflags old_flags = o.flags();
217  o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
218  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
219  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
220  << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
221  o.flags(old_flags);
222  o.precision(old_pre);
223  return o;
224 }
225 
226 /*---------------------------------------------------------------
227  Implements the writing to a mxArray for Matlab
228  ---------------------------------------------------------------*/
229 #if MRPT_HAS_MATLAB
230 // Add to implement mexplus::from template specialization
232 #endif
233 
235 {
236 #if MRPT_HAS_MATLAB
237  const char* fields[] = {"R", "t"};
238  mexplus::MxArray pose_struct(
239  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
240  pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
241  pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
242  return pose_struct.release();
243 #else
244  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
245 #endif
246 }
247 
248 /*---------------------------------------------------------------
249  normalizeAngles
250 ---------------------------------------------------------------*/
252 /*---------------------------------------------------------------
253  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
254 ---------------------------------------------------------------*/
256  const double x0, const double y0, const double z0, const double yaw,
257  const double pitch, const double roll)
258 {
259  m_coords[0] = x0;
260  m_coords[1] = y0;
261  m_coords[2] = z0;
262  this->m_yaw = mrpt::math::wrapToPi(yaw);
263  this->m_pitch = mrpt::math::wrapToPi(pitch);
264  this->m_roll = mrpt::math::wrapToPi(roll);
265 
266  m_ypr_uptodate = true;
267 
269 }
270 
272 {
274 }
275 
276 /*---------------------------------------------------------------
277  Scalar multiplication.
278 ---------------------------------------------------------------*/
279 void CPose3D::operator*=(const double s)
280 {
282  m_coords[0] *= s;
283  m_coords[1] *= s;
284  m_coords[2] *= s;
285  m_yaw *= s;
286  m_pitch *= s;
287  m_roll *= s;
289 }
290 
291 /*---------------------------------------------------------------
292  getYawPitchRoll
293 ---------------------------------------------------------------*/
294 void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
295 {
296  TPose3D::SO3_to_yaw_pitch_roll(m_ROT, yaw, pitch, roll);
297 }
298 
299 /*---------------------------------------------------------------
300  sphericalCoordinates
301 ---------------------------------------------------------------*/
303  const TPoint3D& point, double& out_range, double& out_yaw,
304  double& out_pitch) const
305 {
306  // Pass to coordinates as seen from this 6D pose:
307  TPoint3D local;
308  this->inverseComposePoint(
309  point.x, point.y, point.z, local.x, local.y, local.z);
310 
311  // Range:
312  out_range = local.norm();
313 
314  // Yaw:
315  if (local.y != 0 || local.x != 0)
316  out_yaw = atan2(local.y, local.x);
317  else
318  out_yaw = 0;
319 
320  // Pitch:
321  if (out_range != 0)
322  out_pitch = -asin(local.z / out_range);
323  else
324  out_pitch = 0;
325 }
326 
328 {
329  return CPose3D(
330  -m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
331 }
332 
333 /*---------------------------------------------------------------
334  addComponents
335 ---------------------------------------------------------------*/
337 {
339  m_coords[0] += p.m_coords[0];
340  m_coords[1] += p.m_coords[1];
341  m_coords[2] += p.m_coords[2];
342  m_yaw += p.m_yaw;
343  m_pitch += p.m_pitch;
344  m_roll += p.m_roll;
346 }
347 
348 /*---------------------------------------------------------------
349  distanceEuclidean6D
350 ---------------------------------------------------------------*/
351 double CPose3D::distanceEuclidean6D(const CPose3D& o) const
352 {
354  o.updateYawPitchRoll();
355  return sqrt(
356  square(o.m_coords[0] - m_coords[0]) +
357  square(o.m_coords[1] - m_coords[1]) +
358  square(o.m_coords[2] - m_coords[2]) +
359  square(wrapToPi(o.m_yaw - m_yaw)) +
361  square(wrapToPi(o.m_roll - m_roll)));
362 }
363 
364 /*---------------------------------------------------------------
365  composePoint
366 ---------------------------------------------------------------*/
368  double lx, double ly, double lz, double& gx, double& gy, double& gz,
369  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
370  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose,
371  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3,
372  bool use_small_rot_approx) const
373 {
374  // Jacob: df/dpoint
375  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = m_ROT;
376 
377  // Jacob: df/dpose
378  if (out_jacobian_df_dpose)
379  {
380  if (use_small_rot_approx)
381  {
382  // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
383  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
384  1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
385  out_jacobian_df_dpose->loadFromArray(nums);
386  }
387  else
388  {
389  // Exact Jacobians:
391 #ifdef HAVE_SINCOS
392  double cy, sy;
393  ::sincos(m_yaw, &sy, &cy);
394  double cp, sp;
395  ::sincos(m_pitch, &sp, &cp);
396  double cr, sr;
397  ::sincos(m_roll, &sr, &cr);
398 #else
399  const double cy = cos(m_yaw);
400  const double sy = sin(m_yaw);
401  const double cp = cos(m_pitch);
402  const double sp = sin(m_pitch);
403  const double cr = cos(m_roll);
404  const double sr = sin(m_roll);
405 #endif
406 
407  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
408  1,
409  0,
410  0,
411  -lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
412  lz * (-sy * sp * cr + cy * sr), // d_x'/d_yaw
413  -lx * cy * sp + ly * (cy * cp * sr) +
414  lz * (cy * cp * cr), // d_x'/d_pitch
415  ly * (cy * sp * cr + sy * sr) +
416  lz * (-cy * sp * sr + sy * cr), // d_x'/d_roll
417  0,
418  1,
419  0,
420  lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
421  lz * (cy * sp * cr + sy * sr), // d_y'/d_yaw
422  -lx * sy * sp + ly * (sy * cp * sr) +
423  lz * (sy * cp * cr), // d_y'/d_pitch
424  ly * (sy * sp * cr - cy * sr) +
425  lz * (-sy * sp * sr - cy * cr), // d_y'/d_roll
426  0,
427  0,
428  1,
429  0, // d_z' / d_yaw
430  -lx * cp - ly * sp * sr - lz * sp * cr, // d_z' / d_pitch
431  ly * cp * cr - lz * cp * sr // d_z' / d_roll
432  };
433  out_jacobian_df_dpose->loadFromArray(nums);
434  }
435  }
436 
437  gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
438  gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
439  gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];
440 
441  // Jacob: df/dse3
442  if (out_jacobian_df_dse3)
443  {
444  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
445  1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
446  out_jacobian_df_dse3->loadFromArray(nums);
447  }
448 }
449 
451  const mrpt::math::TVector3D& l) const
452 {
454  g.x = m_ROT(0, 0) * l.x + m_ROT(0, 1) * l.y + m_ROT(0, 2) * l.z;
455  g.y = m_ROT(1, 0) * l.x + m_ROT(1, 1) * l.y + m_ROT(1, 2) * l.z;
456  g.z = m_ROT(2, 0) * l.x + m_ROT(2, 1) * l.y + m_ROT(2, 2) * l.z;
457  return g;
458 }
459 
461  const mrpt::math::TVector3D& g) const
462 {
464  l.x = m_ROT(0, 0) * g.x + m_ROT(1, 0) * g.y + m_ROT(2, 0) * g.z;
465  l.y = m_ROT(0, 1) * g.x + m_ROT(1, 1) * g.y + m_ROT(2, 1) * g.z;
466  l.z = m_ROT(0, 2) * g.x + m_ROT(1, 2) * g.y + m_ROT(2, 2) * g.z;
467  return l;
468 }
469 
470 // TODO: Use SSE2? OTOH, this forces mem align...
471 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
472 /*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
473 {
474  ASSERT_(((size_t)matrix & 15) == 0);
475  __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
476 _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
477  __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
478 _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
479  __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
480 _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
481 
482  return _mm_add_ps(_mm_add_ps(a0,a1),a2);
483 }*/
484 #endif // SSE2
485 
486 void CPose3D::asVector(vector_t& r) const
487 {
489  r[0] = m_coords[0];
490  r[1] = m_coords[1];
491  r[2] = m_coords[2];
492  r[3] = m_yaw;
493  r[4] = m_pitch;
494  r[5] = m_roll;
495 }
496 
497 /*---------------------------------------------------------------
498  unary -
499 ---------------------------------------------------------------*/
501 {
503  b.getInverseHomogeneousMatrix(B_INV);
504  return CPose3D(B_INV);
505 }
506 
510 {
513  .getAsQuaternion(q, out_dq_dr);
514 }
515 
516 bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
517 {
518  return (p1.m_coords == p2.m_coords) &&
519  ((p1.getRotationMatrix() - p2.getRotationMatrix())
520  .array()
521  .abs()
522  .maxCoeff() < 1e-6);
523 }
524 
525 bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
526 {
527  return (p1.m_coords != p2.m_coords) ||
528  ((p1.getRotationMatrix() - p2.getRotationMatrix())
529  .array()
530  .abs()
531  .maxCoeff() >= 1e-6);
532 }
533 
534 /*---------------------------------------------------------------
535  point3D = pose3D + point3D
536  ---------------------------------------------------------------*/
538 {
539  return CPoint3D(
540  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
541  m_ROT(0, 2) * b.z(),
542  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
543  m_ROT(1, 2) * b.z(),
544  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
545  m_ROT(2, 2) * b.z());
546 }
547 
548 /*---------------------------------------------------------------
549  point3D = pose3D + point2D
550  ---------------------------------------------------------------*/
552 {
553  return CPoint3D(
554  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
555  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
556  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
557 }
558 
559 /*---------------------------------------------------------------
560  this = A + B
561  ---------------------------------------------------------------*/
562 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
563 {
564  // The translation part HM(0:3,3)
565  if (this == &B)
566  {
567  // we need to make a temporary copy of the vector:
568  const CVectorFixedDouble<3> B_coords = B.m_coords;
569  for (int r = 0; r < 3; r++)
570  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
571  A.m_ROT(r, 1) * B_coords[1] +
572  A.m_ROT(r, 2) * B_coords[2];
573  }
574  else
575  {
576  for (int r = 0; r < 3; r++)
577  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
578  A.m_ROT(r, 1) * B.m_coords[1] +
579  A.m_ROT(r, 2) * B.m_coords[2];
580  }
581 
582  // Important: Make this multiplication AFTER the translational part, to cope
583  // with the case when A==this
584  m_ROT = A.m_ROT * B.m_ROT;
585 
586  m_ypr_uptodate = false;
587 }
588 
589 /** Convert this pose into its inverse, saving the result in itself. */
591 {
593  CVectorFixedDouble<3> inv_xyz;
594 
596 
597  m_ROT = inv_rot;
598  m_coords = inv_xyz;
599  m_ypr_uptodate = false;
600 }
601 
602 /*---------------------------------------------------------------
603  isHorizontal
604  ---------------------------------------------------------------*/
605 bool CPose3D::isHorizontal(const double tolerance) const
606 {
608  return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
609  (fabs(m_roll) <= tolerance ||
610  fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
611 }
612 
613 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
614  * than "this= A - B;" since it avoids the temporary object.
615  * \note A or B can be "this" without problems.
616  * \sa composeFrom, composePoint
617  */
619 {
620  // this = A (-) B
621  // HM_this = inv(HM_B) * HM_A
622  //
623  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
624  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
625  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
626  //
627 
628  // XYZ part:
630  CVectorFixedDouble<3> t_b_inv;
631  mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);
632 
633  for (int i = 0; i < 3; i++)
634  m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
635  R_b_inv(i, 1) * A.m_coords[1] +
636  R_b_inv(i, 2) * A.m_coords[2];
637 
638  // Rot part:
639  m_ROT = R_b_inv * A.m_ROT;
640  m_ypr_uptodate = false;
641 }
642 
643 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
644  * \sa composePoint, composeFrom
645  */
647  const double gx, const double gy, const double gz, double& lx, double& ly,
648  double& lz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
649  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose,
650  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3) const
651 {
653  CVectorFixedDouble<3> t_inv;
655 
656  // Jacob: df/dpoint
657  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = R_inv;
658 
659  // Jacob: df/dpose
660  if (out_jacobian_df_dpose)
661  {
662  // TODO: Perhaps this and the sin/cos's can be avoided if all needed
663  // terms are already in m_ROT ???
665 
666 #ifdef HAVE_SINCOS
667  double cy, sy;
668  ::sincos(m_yaw, &sy, &cy);
669  double cp, sp;
670  ::sincos(m_pitch, &sp, &cp);
671  double cr, sr;
672  ::sincos(m_roll, &sr, &cr);
673 #else
674  const double cy = cos(m_yaw);
675  const double sy = sin(m_yaw);
676  const double cp = cos(m_pitch);
677  const double sp = sin(m_pitch);
678  const double cr = cos(m_roll);
679  const double sr = sin(m_roll);
680 #endif
681 
682  const double m11_dy = -sy * cp;
683  const double m12_dy = cy * cp;
684  const double m13_dy = 0;
685  const double m11_dp = -cy * sp;
686  const double m12_dp = -sy * sp;
687  const double m13_dp = -cp;
688  const double m11_dr = 0;
689  const double m12_dr = 0;
690  const double m13_dr = 0;
691 
692  const double m21_dy = (-sy * sp * sr - cy * cr);
693  const double m22_dy = (cy * sp * sr - sy * cr);
694  const double m23_dy = 0;
695  const double m21_dp = (cy * cp * sr);
696  const double m22_dp = (sy * cp * sr);
697  const double m23_dp = -sp * sr;
698  const double m21_dr = (cy * sp * cr + sy * sr);
699  const double m22_dr = (sy * sp * cr - cy * sr);
700  const double m23_dr = cp * cr;
701 
702  const double m31_dy = (-sy * sp * cr + cy * sr);
703  const double m32_dy = (cy * sp * cr + sy * sr);
704  const double m33_dy = 0;
705  const double m31_dp = (cy * cp * cr);
706  const double m32_dp = (sy * cp * cr);
707  const double m33_dp = -sp * cr;
708  const double m31_dr = (-cy * sp * sr + sy * cr);
709  const double m32_dr = (-sy * sp * sr - cy * cr);
710  const double m33_dr = -cp * sr;
711 
712  const double Ax = gx - m_coords[0];
713  const double Ay = gy - m_coords[1];
714  const double Az = gz - m_coords[2];
715 
716  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
717  -m_ROT(0, 0),
718  -m_ROT(1, 0),
719  -m_ROT(2, 0),
720  Ax * m11_dy + Ay * m12_dy + Az * m13_dy, // d_x'/d_yaw
721  Ax * m11_dp + Ay * m12_dp + Az * m13_dp, // d_x'/d_pitch
722  Ax * m11_dr + Ay * m12_dr + Az * m13_dr, // d_x'/d_roll
723 
724  -m_ROT(0, 1),
725  -m_ROT(1, 1),
726  -m_ROT(2, 1),
727  Ax * m21_dy + Ay * m22_dy + Az * m23_dy, // d_x'/d_yaw
728  Ax * m21_dp + Ay * m22_dp + Az * m23_dp, // d_x'/d_pitch
729  Ax * m21_dr + Ay * m22_dr + Az * m23_dr, // d_x'/d_roll
730 
731  -m_ROT(0, 2),
732  -m_ROT(1, 2),
733  -m_ROT(2, 2),
734  Ax * m31_dy + Ay * m32_dy + Az * m33_dy, // d_x'/d_yaw
735  Ax * m31_dp + Ay * m32_dp + Az * m33_dp, // d_x'/d_pitch
736  Ax * m31_dr + Ay * m32_dr + Az * m33_dr, // d_x'/d_roll
737  };
738  out_jacobian_df_dpose->loadFromArray(nums);
739  }
740 
741  lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
742  ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
743  lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;
744 
745  // Jacob: df/dse3
746  if (out_jacobian_df_dse3)
747  {
748  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
749  -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
750  out_jacobian_df_dse3->loadFromArray(nums);
751  }
752 }
753 
755 {
756  for (int i = 0; i < 3; i++)
757  for (int j = 0; j < 3; j++)
758  m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();
759 
760  for (int i = 0; i < 3; i++)
761  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
762 }
763 
765 {
766  return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
767 }
768 
770 {
771  using mrpt::DEG2RAD;
773  if (!m.fromMatlabStringFormat(s))
774  THROW_EXCEPTION("Malformed expression in ::fromString");
775  ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
776  this->setFromValues(
777  m(0, 0), m(0, 1), m(0, 2), DEG2RAD(m(0, 3)), DEG2RAD(m(0, 4)),
778  DEG2RAD(m(0, 5)));
779 }
780 
782 {
783  this->fromString("[" + s + "]");
784 }
785 
787 {
788  auto M = out_HM.asEigen();
789  M.block<3, 3>(0, 0) = m_ROT.asEigen();
790  for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
791  out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
792  out_HM(3, 3) = 1.;
793 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:764
#define local
Definition: zutil.h:47
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:618
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose3D.cpp:327
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
double RAD2DEG(const double x)
Radians to degrees.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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...
Definition: CPose3D.cpp:367
#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:96
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It&#39;s not safe to set t...
Definition: CPose3D.h:101
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose3D.cpp:754
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:605
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:37
#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:105
double DEG2RAD(const double x)
Degrees to radians.
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
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...
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
Definition: CPose3D.h:248
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
Definition: CPose3D.cpp:271
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:33
GLdouble s
Definition: glext.h:3682
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
Definition: CPose3D.h:115
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:590
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
Definition: CPose3D.cpp:450
unsigned char uint8_t
Definition: rptypes.h:44
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
T square(const T x)
Inline function for the square of a number.
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
Definition: CPose3D.cpp:351
#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 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, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:646
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:562
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:336
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:507
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
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:302
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:130
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:486
#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:109
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:554
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose3D.cpp:781
#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:35
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:279
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:294
mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const
Inverse of rotateVector(), i.e.
Definition: CPose3D.cpp:460
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:769
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:84
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.
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:255
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
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:251
#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:224
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:786
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: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019