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 
46 using namespace mrpt::poses;
47 
49 
50 /*---------------------------------------------------------------
51  Constructors
52  ---------------------------------------------------------------*/
54 {
55  m_coords[0] = m_coords[1] = m_coords[2] = 0;
56  m_ROT.setIdentity();
57 }
58 
59 CPose3D::CPose3D(
60  const double x, const double y, const double z, const double yaw,
61  const double pitch, const double roll)
62  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
63 {
64  setFromValues(x, y, z, yaw, pitch, roll);
65 }
66 
67 CPose3D::CPose3D(const mrpt::math::TPose3D& o) : m_ypr_uptodate(false)
68 {
69  setFromValues(o.x, o.y, o.z, o.yaw, o.pitch, o.roll);
70 }
71 
72 CPose3D::CPose3D(const CPose2D& p) : m_ypr_uptodate(false)
73 {
74  setFromValues(p.x(), p.y(), 0, p.phi(), 0, 0);
75 }
76 
78  : m_ypr_uptodate(false), m_yaw(), m_pitch(), m_roll()
79 {
80  setFromValues(p.x(), p.y(), p.z());
81 }
82 
84  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
85 {
86  ASSERT_ABOVEEQ_(m.rows(), 3);
87  ASSERT_ABOVEEQ_(m.cols(), 4);
88  for (int r = 0; r < 3; r++)
89  for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
90  for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
91 }
92 
94  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
95 {
96  for (int r = 0; r < 3; r++)
97  for (int c = 0; c < 3; c++) m_ROT(r, c) = m(r, c);
98  for (int r = 0; r < 3; r++) m_coords[r] = m(r, 3);
99 }
100 
101 /** Constructor from a quaternion (which only represents the 3D rotation part)
102  * and a 3D displacement. */
104  const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
105  const double _z)
106  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
107 {
108  double yaw, pitch, roll;
109  q.rpy(roll, pitch, yaw);
110  this->setFromValues(_x, _y, _z, yaw, pitch, roll);
111 }
112 
113 /** Constructor from a quaternion-based full pose. */
115  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
116 {
117  // Extract XYZ + ROT from quaternion:
118  m_coords[0] = p.x();
119  m_coords[1] = p.y();
120  m_coords[2] = p.z();
121  p.quat().rotationMatrixNoResize(m_ROT);
122 }
123 
126 {
127  const CPose3DQuat q(*this);
128  // The coordinates:
129  out << q[0] << q[1] << q[2] << q[3] << q[4] << q[5] << q[6];
130 }
132 {
133  switch (version)
134  {
135  case 0:
136  {
137  // The coordinates:
138  CMatrixF HM2;
139  in >> HM2;
140  ASSERT_(HM2.rows() == 4 && HM2.isSquare());
141 
142  m_ROT = HM2.block<3, 3>(0, 0).cast<double>();
143 
144  m_coords[0] = HM2(0, 3);
145  m_coords[1] = HM2(1, 3);
146  m_coords[2] = HM2(2, 3);
147  m_ypr_uptodate = false;
148  }
149  break;
150  case 1:
151  {
152  // The coordinates:
153  CMatrixDouble44 HM;
154  in >> HM;
155 
156  m_ROT = HM.block<3, 3>(0, 0);
157 
158  m_coords[0] = HM(0, 3);
159  m_coords[1] = HM(1, 3);
160  m_coords[2] = HM(2, 3);
161  m_ypr_uptodate = false;
162  }
163  break;
164  case 2:
165  {
166  // An equivalent CPose3DQuat
168  in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];
169 
170  // Extract XYZ + ROT from quaternion:
171  m_ypr_uptodate = false;
172  m_coords[0] = p.x();
173  m_coords[1] = p.y();
174  m_coords[2] = p.z();
175  p.quat().rotationMatrixNoResize(m_ROT);
176  }
177  break;
178  default:
180  };
181 }
182 
184 {
186  out["x"] = m_coords[0];
187  out["y"] = m_coords[1];
188  out["z"] = m_coords[2];
189  out["rot"] = CMatrixD(m_ROT);
190 }
192 {
193  uint8_t version;
195  switch (version)
196  {
197  case 1:
198  {
199  m_coords[0] = static_cast<double>(in["x"]);
200  m_coords[1] = static_cast<double>(in["y"]);
201  m_coords[2] = static_cast<double>(in["z"]);
202  CMatrixD m;
203  in["rot"].readTo(m);
204  m_ROT = m;
205  }
206  break;
207  default:
209  }
210 }
211 
212 /** Textual output stream function.
213  */
214 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
215 {
216  const std::streamsize old_pre = o.precision();
217  const std::ios_base::fmtflags old_flags = o.flags();
218  o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
219  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
220  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
221  << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
222  o.flags(old_flags);
223  o.precision(old_pre);
224  return o;
225 }
226 
227 /*---------------------------------------------------------------
228  Implements the writing to a mxArray for Matlab
229  ---------------------------------------------------------------*/
230 #if MRPT_HAS_MATLAB
231 // Add to implement mexplus::from template specialization
233 #endif
234 
236 {
237 #if MRPT_HAS_MATLAB
238  const char* fields[] = {"R", "t"};
239  mexplus::MxArray pose_struct(
240  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
241  pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
242  pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
243  return pose_struct.release();
244 #else
245  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
246 #endif
247 }
248 
249 /*---------------------------------------------------------------
250  normalizeAngles
251 ---------------------------------------------------------------*/
253 /*---------------------------------------------------------------
254  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
255 ---------------------------------------------------------------*/
257  const double x0, const double y0, const double z0, const double yaw,
258  const double pitch, const double roll)
259 {
260  m_coords[0] = x0;
261  m_coords[1] = y0;
262  m_coords[2] = z0;
263  this->m_yaw = mrpt::math::wrapToPi(yaw);
264  this->m_pitch = mrpt::math::wrapToPi(pitch);
265  this->m_roll = mrpt::math::wrapToPi(roll);
266 
267  m_ypr_uptodate = true;
268 
270 }
271 
273 {
275 }
276 
277 /*---------------------------------------------------------------
278  Scalar multiplication.
279 ---------------------------------------------------------------*/
280 void CPose3D::operator*=(const double s)
281 {
283  m_coords[0] *= s;
284  m_coords[1] *= s;
285  m_coords[2] *= s;
286  m_yaw *= s;
287  m_pitch *= s;
288  m_roll *= s;
290 }
291 
292 /*---------------------------------------------------------------
293  getYawPitchRoll
294 ---------------------------------------------------------------*/
295 void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
296 {
297  TPose3D::SO3_to_yaw_pitch_roll(m_ROT, yaw, pitch, roll);
298 }
299 
300 /*---------------------------------------------------------------
301  sphericalCoordinates
302 ---------------------------------------------------------------*/
304  const TPoint3D& point, double& out_range, double& out_yaw,
305  double& out_pitch) const
306 {
307  // Pass to coordinates as seen from this 6D pose:
308  TPoint3D local;
309  this->inverseComposePoint(
310  point.x, point.y, point.z, local.x, local.y, local.z);
311 
312  // Range:
313  out_range = local.norm();
314 
315  // Yaw:
316  if (local.y != 0 || local.x != 0)
317  out_yaw = atan2(local.y, local.x);
318  else
319  out_yaw = 0;
320 
321  // Pitch:
322  if (out_range != 0)
323  out_pitch = -asin(local.z / out_range);
324  else
325  out_pitch = 0;
326 }
327 
329 {
330  return CPose3D(
331  -m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
332 }
333 
334 /*---------------------------------------------------------------
335  addComponents
336 ---------------------------------------------------------------*/
338 {
340  m_coords[0] += p.m_coords[0];
341  m_coords[1] += p.m_coords[1];
342  m_coords[2] += p.m_coords[2];
343  m_yaw += p.m_yaw;
344  m_pitch += p.m_pitch;
345  m_roll += p.m_roll;
347 }
348 
349 /*---------------------------------------------------------------
350  distanceEuclidean6D
351 ---------------------------------------------------------------*/
352 double CPose3D::distanceEuclidean6D(const CPose3D& o) const
353 {
355  o.updateYawPitchRoll();
356  return sqrt(
357  square(o.m_coords[0] - m_coords[0]) +
358  square(o.m_coords[1] - m_coords[1]) +
359  square(o.m_coords[2] - m_coords[2]) +
360  square(wrapToPi(o.m_yaw - m_yaw)) +
362  square(wrapToPi(o.m_roll - m_roll)));
363 }
364 
365 /*---------------------------------------------------------------
366  composePoint
367 ---------------------------------------------------------------*/
369  double lx, double ly, double lz, double& gx, double& gy, double& gz,
370  mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
371  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose,
372  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3,
373  bool use_small_rot_approx) const
374 {
375  // Jacob: df/dpoint
376  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = m_ROT;
377 
378  // Jacob: df/dpose
379  if (out_jacobian_df_dpose)
380  {
381  if (use_small_rot_approx)
382  {
383  // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
384  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
385  1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
386  out_jacobian_df_dpose->loadFromArray(nums);
387  }
388  else
389  {
390  // Exact Jacobians:
392 #ifdef HAVE_SINCOS
393  double cy, sy;
394  ::sincos(m_yaw, &sy, &cy);
395  double cp, sp;
396  ::sincos(m_pitch, &sp, &cp);
397  double cr, sr;
398  ::sincos(m_roll, &sr, &cr);
399 #else
400  const double cy = cos(m_yaw);
401  const double sy = sin(m_yaw);
402  const double cp = cos(m_pitch);
403  const double sp = sin(m_pitch);
404  const double cr = cos(m_roll);
405  const double sr = sin(m_roll);
406 #endif
407 
408  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
409  1,
410  0,
411  0,
412  -lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
413  lz * (-sy * sp * cr + cy * sr), // d_x'/d_yaw
414  -lx * cy * sp + ly * (cy * cp * sr) +
415  lz * (cy * cp * cr), // d_x'/d_pitch
416  ly * (cy * sp * cr + sy * sr) +
417  lz * (-cy * sp * sr + sy * cr), // d_x'/d_roll
418  0,
419  1,
420  0,
421  lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
422  lz * (cy * sp * cr + sy * sr), // d_y'/d_yaw
423  -lx * sy * sp + ly * (sy * cp * sr) +
424  lz * (sy * cp * cr), // d_y'/d_pitch
425  ly * (sy * sp * cr - cy * sr) +
426  lz * (-sy * sp * sr - cy * cr), // d_y'/d_roll
427  0,
428  0,
429  1,
430  0, // d_z' / d_yaw
431  -lx * cp - ly * sp * sr - lz * sp * cr, // d_z' / d_pitch
432  ly * cp * cr - lz * cp * sr // d_z' / d_roll
433  };
434  out_jacobian_df_dpose->loadFromArray(nums);
435  }
436  }
437 
438  gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
439  gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
440  gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];
441 
442  // Jacob: df/dse3
443  if (out_jacobian_df_dse3)
444  {
445  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
446  1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
447  out_jacobian_df_dse3->loadFromArray(nums);
448  }
449 }
450 
452  const mrpt::math::TVector3D& l) const
453 {
455  g.x = m_ROT(0, 0) * l.x + m_ROT(0, 1) * l.y + m_ROT(0, 2) * l.z;
456  g.y = m_ROT(1, 0) * l.x + m_ROT(1, 1) * l.y + m_ROT(1, 2) * l.z;
457  g.z = m_ROT(2, 0) * l.x + m_ROT(2, 1) * l.y + m_ROT(2, 2) * l.z;
458  return g;
459 }
460 
462  const mrpt::math::TVector3D& g) const
463 {
465  l.x = m_ROT(0, 0) * g.x + m_ROT(1, 0) * g.y + m_ROT(2, 0) * g.z;
466  l.y = m_ROT(0, 1) * g.x + m_ROT(1, 1) * g.y + m_ROT(2, 1) * g.z;
467  l.z = m_ROT(0, 2) * g.x + m_ROT(1, 2) * g.y + m_ROT(2, 2) * g.z;
468  return l;
469 }
470 
471 // TODO: Use SSE2? OTOH, this forces mem align...
472 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
473 /*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
474 {
475  ASSERT_(((size_t)matrix & 15) == 0);
476  __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
477 _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
478  __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
479 _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
480  __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
481 _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
482 
483  return _mm_add_ps(_mm_add_ps(a0,a1),a2);
484 }*/
485 #endif // SSE2
486 
487 void CPose3D::asVector(vector_t& r) const
488 {
490  r[0] = m_coords[0];
491  r[1] = m_coords[1];
492  r[2] = m_coords[2];
493  r[3] = m_yaw;
494  r[4] = m_pitch;
495  r[5] = m_roll;
496 }
497 
498 /*---------------------------------------------------------------
499  unary -
500 ---------------------------------------------------------------*/
502 {
504  b.getInverseHomogeneousMatrix(B_INV);
505  return CPose3D(B_INV);
506 }
507 
511 {
514  .getAsQuaternion(q, out_dq_dr);
515 }
516 
517 bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
518 {
519  return (p1.m_coords == p2.m_coords) &&
520  ((p1.getRotationMatrix() - p2.getRotationMatrix())
521  .array()
522  .abs()
523  .maxCoeff() < 1e-6);
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 /*---------------------------------------------------------------
536  point3D = pose3D + point3D
537  ---------------------------------------------------------------*/
539 {
540  return CPoint3D(
541  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
542  m_ROT(0, 2) * b.z(),
543  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
544  m_ROT(1, 2) * b.z(),
545  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
546  m_ROT(2, 2) * b.z());
547 }
548 
549 /*---------------------------------------------------------------
550  point3D = pose3D + point2D
551  ---------------------------------------------------------------*/
553 {
554  return CPoint3D(
555  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
556  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
557  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
558 }
559 
560 /*---------------------------------------------------------------
561  this = A + B
562  ---------------------------------------------------------------*/
563 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
564 {
565  // The translation part HM(0:3,3)
566  if (this == &B)
567  {
568  // we need to make a temporary copy of the vector:
569  const CVectorFixedDouble<3> B_coords = B.m_coords;
570  for (int r = 0; r < 3; r++)
571  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
572  A.m_ROT(r, 1) * B_coords[1] +
573  A.m_ROT(r, 2) * B_coords[2];
574  }
575  else
576  {
577  for (int r = 0; r < 3; r++)
578  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
579  A.m_ROT(r, 1) * B.m_coords[1] +
580  A.m_ROT(r, 2) * B.m_coords[2];
581  }
582 
583  // Important: Make this multiplication AFTER the translational part, to cope
584  // with the case when A==this
585  m_ROT = A.m_ROT * B.m_ROT;
586 
587  m_ypr_uptodate = false;
588 }
589 
590 /** Convert this pose into its inverse, saving the result in itself. */
592 {
594  CVectorFixedDouble<3> inv_xyz;
595 
597 
598  m_ROT = inv_rot;
599  m_coords = inv_xyz;
600  m_ypr_uptodate = false;
601 }
602 
603 /*---------------------------------------------------------------
604  isHorizontal
605  ---------------------------------------------------------------*/
606 bool CPose3D::isHorizontal(const double tolerance) const
607 {
609  return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
610  (fabs(m_roll) <= tolerance ||
611  fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
612 }
613 
614 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
615  * than "this= A - B;" since it avoids the temporary object.
616  * \note A or B can be "this" without problems.
617  * \sa composeFrom, composePoint
618  */
620 {
621  // this = A (-) B
622  // HM_this = inv(HM_B) * HM_A
623  //
624  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
625  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
626  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
627  //
628 
629  // XYZ part:
631  CVectorFixedDouble<3> t_b_inv;
632  mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);
633 
634  for (int i = 0; i < 3; i++)
635  m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
636  R_b_inv(i, 1) * A.m_coords[1] +
637  R_b_inv(i, 2) * A.m_coords[2];
638 
639  // Rot part:
640  m_ROT = R_b_inv * A.m_ROT;
641  m_ypr_uptodate = false;
642 }
643 
644 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
645  * \sa composePoint, composeFrom
646  */
648  const double gx, const double gy, const double gz, double& lx, double& ly,
649  double& lz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
650  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dpose,
651  mrpt::math::CMatrixFixed<double, 3, 6>* out_jacobian_df_dse3) const
652 {
654  CVectorFixedDouble<3> t_inv;
656 
657  // Jacob: df/dpoint
658  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = R_inv;
659 
660  // Jacob: df/dpose
661  if (out_jacobian_df_dpose)
662  {
663  // TODO: Perhaps this and the sin/cos's can be avoided if all needed
664  // terms are already in m_ROT ???
666 
667 #ifdef HAVE_SINCOS
668  double cy, sy;
669  ::sincos(m_yaw, &sy, &cy);
670  double cp, sp;
671  ::sincos(m_pitch, &sp, &cp);
672  double cr, sr;
673  ::sincos(m_roll, &sr, &cr);
674 #else
675  const double cy = cos(m_yaw);
676  const double sy = sin(m_yaw);
677  const double cp = cos(m_pitch);
678  const double sp = sin(m_pitch);
679  const double cr = cos(m_roll);
680  const double sr = sin(m_roll);
681 #endif
682 
683  const double m11_dy = -sy * cp;
684  const double m12_dy = cy * cp;
685  const double m13_dy = 0;
686  const double m11_dp = -cy * sp;
687  const double m12_dp = -sy * sp;
688  const double m13_dp = -cp;
689  const double m11_dr = 0;
690  const double m12_dr = 0;
691  const double m13_dr = 0;
692 
693  const double m21_dy = (-sy * sp * sr - cy * cr);
694  const double m22_dy = (cy * sp * sr - sy * cr);
695  const double m23_dy = 0;
696  const double m21_dp = (cy * cp * sr);
697  const double m22_dp = (sy * cp * sr);
698  const double m23_dp = -sp * sr;
699  const double m21_dr = (cy * sp * cr + sy * sr);
700  const double m22_dr = (sy * sp * cr - cy * sr);
701  const double m23_dr = cp * cr;
702 
703  const double m31_dy = (-sy * sp * cr + cy * sr);
704  const double m32_dy = (cy * sp * cr + sy * sr);
705  const double m33_dy = 0;
706  const double m31_dp = (cy * cp * cr);
707  const double m32_dp = (sy * cp * cr);
708  const double m33_dp = -sp * cr;
709  const double m31_dr = (-cy * sp * sr + sy * cr);
710  const double m32_dr = (-sy * sp * sr - cy * cr);
711  const double m33_dr = -cp * sr;
712 
713  const double Ax = gx - m_coords[0];
714  const double Ay = gy - m_coords[1];
715  const double Az = gz - m_coords[2];
716 
717  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
718  -m_ROT(0, 0),
719  -m_ROT(1, 0),
720  -m_ROT(2, 0),
721  Ax * m11_dy + Ay * m12_dy + Az * m13_dy, // d_x'/d_yaw
722  Ax * m11_dp + Ay * m12_dp + Az * m13_dp, // d_x'/d_pitch
723  Ax * m11_dr + Ay * m12_dr + Az * m13_dr, // d_x'/d_roll
724 
725  -m_ROT(0, 1),
726  -m_ROT(1, 1),
727  -m_ROT(2, 1),
728  Ax * m21_dy + Ay * m22_dy + Az * m23_dy, // d_x'/d_yaw
729  Ax * m21_dp + Ay * m22_dp + Az * m23_dp, // d_x'/d_pitch
730  Ax * m21_dr + Ay * m22_dr + Az * m23_dr, // d_x'/d_roll
731 
732  -m_ROT(0, 2),
733  -m_ROT(1, 2),
734  -m_ROT(2, 2),
735  Ax * m31_dy + Ay * m32_dy + Az * m33_dy, // d_x'/d_yaw
736  Ax * m31_dp + Ay * m32_dp + Az * m33_dp, // d_x'/d_pitch
737  Ax * m31_dr + Ay * m32_dr + Az * m33_dr, // d_x'/d_roll
738  };
739  out_jacobian_df_dpose->loadFromArray(nums);
740  }
741 
742  lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
743  ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
744  lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;
745 
746  // Jacob: df/dse3
747  if (out_jacobian_df_dse3)
748  {
749  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[3 * 6] = {
750  -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
751  out_jacobian_df_dse3->loadFromArray(nums);
752  }
753 }
754 
756 {
757  for (int i = 0; i < 3; i++)
758  for (int j = 0; j < 3; j++)
759  m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();
760 
761  for (int i = 0; i < 3; i++)
762  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
763 }
764 
766 {
767  return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
768 }
769 
771 {
772  using mrpt::DEG2RAD;
774  if (!m.fromMatlabStringFormat(s))
775  THROW_EXCEPTION("Malformed expression in ::fromString");
776  ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
777  this->setFromValues(
778  m(0, 0), m(0, 1), m(0, 2), DEG2RAD(m(0, 3)), DEG2RAD(m(0, 4)),
779  DEG2RAD(m(0, 5)));
780 }
781 
783 {
784  this->fromString("[" + s + "]");
785 }
786 
788 {
789  auto M = out_HM.asEigen();
790  M.block<3, 3>(0, 0) = m_ROT.asEigen();
791  for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
792  out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
793  out_HM(3, 3) = 1.;
794 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:765
#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:619
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose3D.cpp:328
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:368
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mxArray * convertToMatlab(const Eigen::EigenBase< Derived > &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:35
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:755
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:606
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:124
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:272
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:591
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
Definition: CPose3D.cpp:451
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:352
#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:647
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:563
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:337
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:508
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:303
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:131
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:487
#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:782
#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:280
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:295
mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const
Inverse of rotateVector(), i.e.
Definition: CPose3D.cpp:461
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:770
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:53
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:53
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:256
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:252
#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:787
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3D.cpp:125



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8a2d96f36 Mon Jul 15 00:01:58 2019 +0200 at lun jul 15 00:10:13 CEST 2019