MRPT  1.9.9
CPose3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://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/math/types_math.h> // for CVectorD...
14 #include <mrpt/math/CMatrix.h> // for CMatrix
15 #include <mrpt/math/geometry.h> // for skew_sym...
16 #include <mrpt/math/matrix_serialization.h> // for operator>>
17 #include <mrpt/math/wrap2pi.h> // for wrapToPi
18 #include <mrpt/poses/CPoint2D.h> // for CPoint2D
19 #include <mrpt/poses/CPoint3D.h> // for CPoint3D
20 #include <mrpt/poses/CPose2D.h> // for CPose2D
21 #include <mrpt/poses/CPose3D.h> // for CPose3D
22 #include <mrpt/poses/CPose3DQuat.h> // for CPose3DQuat
23 #include <mrpt/poses/CPose3DRotVec.h> // for CPose3DR...
25 #include <algorithm> // for move
26 #include <cmath> // for fabs
27 #include <iomanip> // for operator<<
28 #include <limits> // for numeric_...
29 #include <ostream> // for operator<<
30 #include <string> // for allocator
31 #include <mrpt/math/CArrayNumeric.h> // for CArrayDo...
32 #include <mrpt/math/CMatrixFixedNumeric.h> // for CMatrixF...
33 #include <mrpt/math/CMatrixTemplateNumeric.h> // for CMatrixD...
34 #include <mrpt/math/CQuaternion.h> // for CQuatern...
35 #include <mrpt/math/homog_matrices.h> // for homogene...
36 #include <mrpt/math/lightweight_geom_data.h> // for TPoint3D
37 #include <mrpt/math/ops_containers.h> // for dotProduct
38 #include <mrpt/serialization/CSerializable.h> // for CSeriali...
39 #include <mrpt/core/bits_math.h> // for square
40 #include <mrpt/math/utils_matlab.h>
41 #include <mrpt/otherlibs/sophus/so3.hpp>
42 #include <mrpt/otherlibs/sophus/se3.hpp>
43 
44 using namespace mrpt;
45 using namespace mrpt::math;
46 
47 using namespace mrpt::poses;
48 
50 
51 /*---------------------------------------------------------------
52  Constructors
53  ---------------------------------------------------------------*/
54 CPose3D::CPose3D() : m_ypr_uptodate(true), m_yaw(0), m_pitch(0), m_roll(0)
55 {
56  m_coords[0] = m_coords[1] = m_coords[2] = 0;
57  m_ROT.unit(3, 1.0);
58 }
59 
60 CPose3D::CPose3D(
61  const double x, const double y, const double z, const double yaw,
62  const double pitch, const double roll)
63  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
64 {
65  setFromValues(x, y, z, yaw, pitch, roll);
66 }
67 
68 CPose3D::CPose3D(const mrpt::math::TPose3D& o) : m_ypr_uptodate(false)
69 {
70  setFromValues(o.x, o.y, o.z, o.yaw, o.pitch, o.roll);
71 }
72 
73 CPose3D::CPose3D(const CPose2D& p) : m_ypr_uptodate(false)
74 {
75  setFromValues(p.x(), p.y(), 0, p.phi(), 0, 0);
76 }
77 
79  : m_ypr_uptodate(false), m_yaw(), m_pitch(), m_roll()
80 {
81  setFromValues(p.x(), p.y(), p.z());
82 }
83 
85  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
86 {
87  ASSERT_ABOVEEQ_(m.rows(), 3);
88  ASSERT_ABOVEEQ_(m.cols(), 4);
89  for (int r = 0; r < 3; r++)
90  for (int c = 0; c < 3; c++) m_ROT(r, c) = m.get_unsafe(r, c);
91  for (int r = 0; r < 3; r++) m_coords[r] = m.get_unsafe(r, 3);
92 }
93 
95  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
96 {
97  for (int r = 0; r < 3; r++)
98  for (int c = 0; c < 3; c++) m_ROT(r, c) = m.get_unsafe(r, c);
99  for (int r = 0; r < 3; r++) m_coords[r] = m.get_unsafe(r, 3);
100 }
101 
102 /** Constructor from a quaternion (which only represents the 3D rotation part)
103  * and a 3D displacement. */
105  const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
106  const double _z)
107  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
108 {
109  double yaw, pitch, roll;
110  q.rpy(roll, pitch, yaw);
111  this->setFromValues(_x, _y, _z, yaw, pitch, roll);
112 }
113 
114 /** Constructor from a quaternion-based full pose. */
116  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
117 {
118  // Extract XYZ + ROT from quaternion:
119  m_coords[0] = p.x();
120  m_coords[1] = p.y();
121  m_coords[2] = p.z();
122  p.quat().rotationMatrixNoResize(m_ROT);
123 }
124 
125 /** Constructor from a rotation vector-based full pose. */
127  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
128 {
129  m_coords[0] = p.m_coords[0];
130  m_coords[1] = p.m_coords[1];
131  m_coords[2] = p.m_coords[2];
132 
133  this->setRotationMatrix(this->exp_rotation(p.m_rotvec));
134 }
135 
138 {
139  const CPose3DQuat q(*this);
140  // The coordinates:
141  out << q[0] << q[1] << q[2] << q[3] << q[4] << q[5] << q[6];
142 }
144 {
145  switch (version)
146  {
147  case 0:
148  {
149  // The coordinates:
150  CMatrix HM2;
151  in >> HM2;
152  ASSERT_(HM2.rows() == 4 && HM2.isSquare());
153 
154  m_ROT = HM2.block(0, 0, 3, 3).cast<double>();
155 
156  m_coords[0] = HM2.get_unsafe(0, 3);
157  m_coords[1] = HM2.get_unsafe(1, 3);
158  m_coords[2] = HM2.get_unsafe(2, 3);
159  m_ypr_uptodate = false;
160  }
161  break;
162  case 1:
163  {
164  // The coordinates:
165  CMatrixDouble44 HM;
166  in >> HM;
167 
168  m_ROT = HM.block(0, 0, 3, 3);
169 
170  m_coords[0] = HM.get_unsafe(0, 3);
171  m_coords[1] = HM.get_unsafe(1, 3);
172  m_coords[2] = HM.get_unsafe(2, 3);
173  m_ypr_uptodate = false;
174  }
175  break;
176  case 2:
177  {
178  // An equivalent CPose3DQuat
180  in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];
181 
182  // Extract XYZ + ROT from quaternion:
183  m_ypr_uptodate = false;
184  m_coords[0] = p.x();
185  m_coords[1] = p.y();
186  m_coords[2] = p.z();
187  p.quat().rotationMatrixNoResize(m_ROT);
188  }
189  break;
190  default:
192  };
193 }
194 
195 /** Textual output stream function.
196  */
197 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
198 {
199  const std::streamsize old_pre = o.precision();
200  const std::ios_base::fmtflags old_flags = o.flags();
201  o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
202  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
203  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
204  << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
205  o.flags(old_flags);
206  o.precision(old_pre);
207  return o;
208 }
209 
210 /*---------------------------------------------------------------
211  Implements the writing to a mxArray for Matlab
212  ---------------------------------------------------------------*/
213 #if MRPT_HAS_MATLAB
214 // Add to implement mexplus::from template specialization
216 #endif
217 
219 {
220 #if MRPT_HAS_MATLAB
221  const char* fields[] = {"R", "t"};
222  mexplus::MxArray pose_struct(
223  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
224  pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
225  pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
226  return pose_struct.release();
227 #else
228  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
229 #endif
230 }
231 
232 /*---------------------------------------------------------------
233  normalizeAngles
234 ---------------------------------------------------------------*/
236 /*---------------------------------------------------------------
237  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
238 ---------------------------------------------------------------*/
240  const double x0, const double y0, const double z0, const double yaw,
241  const double pitch, const double roll)
242 {
243  m_coords[0] = x0;
244  m_coords[1] = y0;
245  m_coords[2] = z0;
246  this->m_yaw = mrpt::math::wrapToPi(yaw);
247  this->m_pitch = mrpt::math::wrapToPi(pitch);
248  this->m_roll = mrpt::math::wrapToPi(roll);
249 
250  m_ypr_uptodate = true;
251 
253 }
254 
255 /*---------------------------------------------------------------
256  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
257 ---------------------------------------------------------------*/
259 {
260 #ifdef HAVE_SINCOS
261  double cy, sy;
262  ::sincos(m_yaw, &sy, &cy);
263  double cp, sp;
264  ::sincos(m_pitch, &sp, &cp);
265  double cr, sr;
266  ::sincos(m_roll, &sr, &cr);
267 #else
268  const double cy = cos(m_yaw);
269  const double sy = sin(m_yaw);
270  const double cp = cos(m_pitch);
271  const double sp = sin(m_pitch);
272  const double cr = cos(m_roll);
273  const double sr = sin(m_roll);
274 #endif
275 
276  alignas(MRPT_MAX_ALIGN_BYTES) const double rot_vals[] = {cy * cp,
277  cy * sp * sr - sy * cr,
278  cy * sp * cr + sy * sr,
279  sy * cp,
280  sy * sp * sr + cy * cr,
281  sy * sp * cr - cy * sr,
282  -sp,
283  cp * sr,
284  cp * cr};
285  m_ROT.loadFromArray(rot_vals);
286 }
287 
288 /*---------------------------------------------------------------
289  Scalar multiplication.
290 ---------------------------------------------------------------*/
291 void CPose3D::operator*=(const double s)
292 {
294  m_coords[0] *= s;
295  m_coords[1] *= s;
296  m_coords[2] *= s;
297  m_yaw *= s;
298  m_pitch *= s;
299  m_roll *= s;
301 }
302 
303 /*---------------------------------------------------------------
304  getYawPitchRoll
305 ---------------------------------------------------------------*/
306 void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
307 {
308  TPose3D::SO3_to_yaw_pitch_roll(m_ROT, yaw, pitch, roll);
309 }
310 
311 /*---------------------------------------------------------------
312  sphericalCoordinates
313 ---------------------------------------------------------------*/
315  const TPoint3D& point, double& out_range, double& out_yaw,
316  double& out_pitch) const
317 {
318  // Pass to coordinates as seen from this 6D pose:
319  TPoint3D local;
320  this->inverseComposePoint(
321  point.x, point.y, point.z, local.x, local.y, local.z);
322 
323  // Range:
324  out_range = local.norm();
325 
326  // Yaw:
327  if (local.y != 0 || local.x != 0)
328  out_yaw = atan2(local.y, local.x);
329  else
330  out_yaw = 0;
331 
332  // Pitch:
333  if (out_range != 0)
334  out_pitch = -asin(local.z / out_range);
335  else
336  out_pitch = 0;
337 }
338 
340 {
341  return CPose3D(
342  -m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
343 }
344 
345 /*---------------------------------------------------------------
346  addComponents
347 ---------------------------------------------------------------*/
349 {
351  m_coords[0] += p.m_coords[0];
352  m_coords[1] += p.m_coords[1];
353  m_coords[2] += p.m_coords[2];
354  m_yaw += p.m_yaw;
355  m_pitch += p.m_pitch;
356  m_roll += p.m_roll;
358 }
359 
360 /*---------------------------------------------------------------
361  distanceEuclidean6D
362 ---------------------------------------------------------------*/
363 double CPose3D::distanceEuclidean6D(const CPose3D& o) const
364 {
366  o.updateYawPitchRoll();
367  return sqrt(
368  square(o.m_coords[0] - m_coords[0]) +
369  square(o.m_coords[1] - m_coords[1]) +
370  square(o.m_coords[2] - m_coords[2]) +
371  square(wrapToPi(o.m_yaw - m_yaw)) +
373  square(wrapToPi(o.m_roll - m_roll)));
374 }
375 
376 /*---------------------------------------------------------------
377  composePoint
378 ---------------------------------------------------------------*/
380  double lx, double ly, double lz, double& gx, double& gy, double& gz,
381  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
382  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose,
383  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3,
384  bool use_small_rot_approx) const
385 {
386  // Jacob: df/dpoint
387  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = m_ROT;
388 
389  // Jacob: df/dpose
390  if (out_jacobian_df_dpose)
391  {
392  if (use_small_rot_approx)
393  {
394  // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
395  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[3 * 6] = {
396  1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
397  out_jacobian_df_dpose->loadFromArray(nums);
398  }
399  else
400  {
401  // Exact Jacobians:
403 #ifdef HAVE_SINCOS
404  double cy, sy;
405  ::sincos(m_yaw, &sy, &cy);
406  double cp, sp;
407  ::sincos(m_pitch, &sp, &cp);
408  double cr, sr;
409  ::sincos(m_roll, &sr, &cr);
410 #else
411  const double cy = cos(m_yaw);
412  const double sy = sin(m_yaw);
413  const double cp = cos(m_pitch);
414  const double sp = sin(m_pitch);
415  const double cr = cos(m_roll);
416  const double sr = sin(m_roll);
417 #endif
418 
419  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[3 * 6] = {
420  1, 0, 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, 0,
429  lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
430  lz * (cy * sp * cr + sy * sr), // d_y'/d_yaw
431  -lx * sy * sp + ly * (sy * cp * sr) +
432  lz * (sy * cp * cr), // d_y'/d_pitch
433  ly * (sy * sp * cr - cy * sr) +
434  lz * (-sy * sp * sr - cy * cr), // d_y'/d_roll
435  0,
436  0, 1,
437  0, // d_z' / d_yaw
438  -lx * cp - ly * sp * sr - lz * sp * cr, // d_z' / d_pitch
439  ly * cp * cr - lz * cp * sr // d_z' / d_roll
440  };
441  out_jacobian_df_dpose->loadFromArray(nums);
442  }
443  }
444 
445  gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
446  gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
447  gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];
448 
449  // Jacob: df/dse3
450  if (out_jacobian_df_dse3)
451  {
452  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[3 * 6] = {
453  1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
454  out_jacobian_df_dse3->loadFromArray(nums);
455  }
456 }
457 
458 // TODO: Use SSE2? OTOH, this forces mem align...
459 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
460 /*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
461 {
462  ASSERT_(((size_t)matrix & 15) == 0);
463  __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
464 _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
465  __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
466 _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
467  __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
468 _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
469 
470  return _mm_add_ps(_mm_add_ps(a0,a1),a2);
471 }*/
472 #endif // SSE2
473 
474 /*---------------------------------------------------------------
475  getAsVector
476 ---------------------------------------------------------------*/
478 {
480  r.resize(6);
481  r[0] = m_coords[0];
482  r[1] = m_coords[1];
483  r[2] = m_coords[2];
484  r[3] = m_yaw;
485  r[4] = m_pitch;
486  r[5] = m_roll;
487 }
488 
490 {
492  r[0] = m_coords[0];
493  r[1] = m_coords[1];
494  r[2] = m_coords[2];
495  r[3] = m_yaw;
496  r[4] = m_pitch;
497  r[5] = m_roll;
498 }
499 
500 /*---------------------------------------------------------------
501  unary -
502 ---------------------------------------------------------------*/
504 {
506  b.getInverseHomogeneousMatrix(B_INV);
507  return CPose3D(B_INV);
508 }
509 
513 {
516  .getAsQuaternion(q, out_dq_dr);
517 }
518 
519 bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
520 {
521  return (p1.m_coords == p2.m_coords) &&
522  ((p1.getRotationMatrix() - p2.getRotationMatrix())
523  .array()
524  .abs()
525  .maxCoeff() < 1e-6);
526 }
527 
528 bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
529 {
530  return (p1.m_coords != p2.m_coords) ||
531  ((p1.getRotationMatrix() - p2.getRotationMatrix())
532  .array()
533  .abs()
534  .maxCoeff() >= 1e-6);
535 }
536 
537 /*---------------------------------------------------------------
538  point3D = pose3D + point3D
539  ---------------------------------------------------------------*/
541 {
542  return CPoint3D(
543  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
544  m_ROT(0, 2) * b.z(),
545  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
546  m_ROT(1, 2) * b.z(),
547  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
548  m_ROT(2, 2) * b.z());
549 }
550 
551 /*---------------------------------------------------------------
552  point3D = pose3D + point2D
553  ---------------------------------------------------------------*/
555 {
556  return CPoint3D(
557  m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
558  m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
559  m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
560 }
561 
562 /*---------------------------------------------------------------
563  this = A + B
564  ---------------------------------------------------------------*/
565 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
566 {
567  // The translation part HM(0:3,3)
568  if (this == &B)
569  {
570  // we need to make a temporary copy of the vector:
571  const CArrayDouble<3> B_coords = B.m_coords;
572  for (int r = 0; r < 3; r++)
573  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
574  A.m_ROT(r, 1) * B_coords[1] +
575  A.m_ROT(r, 2) * B_coords[2];
576  }
577  else
578  {
579  for (int r = 0; r < 3; r++)
580  m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
581  A.m_ROT(r, 1) * B.m_coords[1] +
582  A.m_ROT(r, 2) * B.m_coords[2];
583  }
584 
585  // Important: Make this multiplication AFTER the translational part, to cope
586  // with the case when A==this
587  m_ROT.multiply_AB(A.m_ROT, B.m_ROT);
588 
589  m_ypr_uptodate = false;
590 }
591 
592 /** Convert this pose into its inverse, saving the result in itself. */
594 {
596  CArrayDouble<3> inv_xyz;
597 
599 
600  m_ROT = inv_rot;
601  m_coords = inv_xyz;
602  m_ypr_uptodate = false;
603 }
604 
605 /*---------------------------------------------------------------
606  isHorizontal
607  ---------------------------------------------------------------*/
608 bool CPose3D::isHorizontal(const double tolerance) const
609 {
611  return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
612  (fabs(m_roll) <= tolerance ||
613  fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
614 }
615 
616 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
617  * than "this= A - B;" since it avoids the temporary object.
618  * \note A or B can be "this" without problems.
619  * \sa composeFrom, composePoint
620  */
622 {
623  // this = A (-) B
624  // HM_this = inv(HM_B) * HM_A
625  //
626  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
627  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
628  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
629  //
630 
631  // XYZ part:
633  CArrayDouble<3> t_b_inv;
634  mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);
635 
636  for (int i = 0; i < 3; i++)
637  m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
638  R_b_inv(i, 1) * A.m_coords[1] +
639  R_b_inv(i, 2) * A.m_coords[2];
640 
641  // Rot part:
642  m_ROT.multiply_AB(R_b_inv, A.m_ROT);
643  m_ypr_uptodate = false;
644 }
645 
646 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
647  * \sa composePoint, composeFrom
648  */
650  const double gx, const double gy, const double gz, double& lx, double& ly,
651  double& lz,
652  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
653  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose,
654  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3) const
655 {
657  CArrayDouble<3> t_inv;
659 
660  // Jacob: df/dpoint
661  if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = R_inv;
662 
663  // Jacob: df/dpose
664  if (out_jacobian_df_dpose)
665  {
666  // TODO: Perhaps this and the sin/cos's can be avoided if all needed
667  // terms are already in m_ROT ???
669 
670 #ifdef HAVE_SINCOS
671  double cy, sy;
672  ::sincos(m_yaw, &sy, &cy);
673  double cp, sp;
674  ::sincos(m_pitch, &sp, &cp);
675  double cr, sr;
676  ::sincos(m_roll, &sr, &cr);
677 #else
678  const double cy = cos(m_yaw);
679  const double sy = sin(m_yaw);
680  const double cp = cos(m_pitch);
681  const double sp = sin(m_pitch);
682  const double cr = cos(m_roll);
683  const double sr = sin(m_roll);
684 #endif
685 
686  const double m11_dy = -sy * cp;
687  const double m12_dy = cy * cp;
688  const double m13_dy = 0;
689  const double m11_dp = -cy * sp;
690  const double m12_dp = -sy * sp;
691  const double m13_dp = -cp;
692  const double m11_dr = 0;
693  const double m12_dr = 0;
694  const double m13_dr = 0;
695 
696  const double m21_dy = (-sy * sp * sr - cy * cr);
697  const double m22_dy = (cy * sp * sr - sy * cr);
698  const double m23_dy = 0;
699  const double m21_dp = (cy * cp * sr);
700  const double m22_dp = (sy * cp * sr);
701  const double m23_dp = -sp * sr;
702  const double m21_dr = (cy * sp * cr + sy * sr);
703  const double m22_dr = (sy * sp * cr - cy * sr);
704  const double m23_dr = cp * cr;
705 
706  const double m31_dy = (-sy * sp * cr + cy * sr);
707  const double m32_dy = (cy * sp * cr + sy * sr);
708  const double m33_dy = 0;
709  const double m31_dp = (cy * cp * cr);
710  const double m32_dp = (sy * cp * cr);
711  const double m33_dp = -sp * cr;
712  const double m31_dr = (-cy * sp * sr + sy * cr);
713  const double m32_dr = (-sy * sp * sr - cy * cr);
714  const double m33_dr = -cp * sr;
715 
716  const double Ax = gx - m_coords[0];
717  const double Ay = gy - m_coords[1];
718  const double Az = gz - m_coords[2];
719 
720  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[3 * 6] = {
721  -m_ROT(0, 0),
722  -m_ROT(1, 0),
723  -m_ROT(2, 0),
724  Ax * m11_dy + Ay * m12_dy + Az * m13_dy, // d_x'/d_yaw
725  Ax * m11_dp + Ay * m12_dp + Az * m13_dp, // d_x'/d_pitch
726  Ax * m11_dr + Ay * m12_dr + Az * m13_dr, // d_x'/d_roll
727 
728  -m_ROT(0, 1),
729  -m_ROT(1, 1),
730  -m_ROT(2, 1),
731  Ax * m21_dy + Ay * m22_dy + Az * m23_dy, // d_x'/d_yaw
732  Ax * m21_dp + Ay * m22_dp + Az * m23_dp, // d_x'/d_pitch
733  Ax * m21_dr + Ay * m22_dr + Az * m23_dr, // d_x'/d_roll
734 
735  -m_ROT(0, 2),
736  -m_ROT(1, 2),
737  -m_ROT(2, 2),
738  Ax * m31_dy + Ay * m32_dy + Az * m33_dy, // d_x'/d_yaw
739  Ax * m31_dp + Ay * m32_dp + Az * m33_dp, // d_x'/d_pitch
740  Ax * m31_dr + Ay * m32_dr + Az * m33_dr, // d_x'/d_roll
741  };
742  out_jacobian_df_dpose->loadFromArray(nums);
743  }
744 
745  lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
746  ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
747  lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;
748 
749  // Jacob: df/dse3
750  if (out_jacobian_df_dse3)
751  {
752  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[3 * 6] = {
753  -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
754  out_jacobian_df_dse3->loadFromArray(nums);
755  }
756 }
757 
759  const mrpt::math::CArrayNumeric<double, 6>& mu, bool pseudo_exponential)
760 {
762  CPose3D::exp(mu, P, pseudo_exponential);
763  return P;
764 }
765 
767  const mrpt::math::CArrayNumeric<double, 6>& mu, CPose3D& out_pose,
768  bool pseudo_exponential)
769 {
770  if (pseudo_exponential)
771  {
772  auto R = Sophus::SO3<double>::exp(mu.block<3, 1>(3, 0));
773  out_pose.setRotationMatrix(R.matrix());
774  out_pose.x(mu[0]);
775  out_pose.y(mu[1]);
776  out_pose.z(mu[2]);
777  }
778  else
779  {
780  auto R = Sophus::SE3<double>::exp(mu);
781  out_pose = CPose3D(CMatrixDouble44(R.matrix()));
782  }
783 }
784 
786 {
787  Sophus::SO3<double> R(this->m_ROT);
788  const auto& r = R.log();
789  CArrayDouble<3> ret;
790  for (int i = 0; i < 3; i++) ret[i] = r[i];
791  return ret;
792 }
793 
796 {
797  auto R = Sophus::SO3<double>::exp(w);
798  return R.matrix();
799 }
800 
801 void CPose3D::ln(CArrayDouble<6>& result) const
802 {
803  const Sophus::SE3<double> RT(m_ROT, m_coords);
804  result = RT.log();
805 }
806 
807 /* The following code fragments are based on formulas originally reported in the
808  * TooN and RobotVision packages */
809 namespace mrpt::poses
810 {
811 template <class VEC3, class MAT33>
812 inline void deltaR(const MAT33& R, VEC3& v)
813 {
814  v[0] = R(2, 1) - R(1, 2);
815  v[1] = R(0, 2) - R(2, 0);
816  v[2] = R(1, 0) - R(0, 1);
817 }
818 
819 template <typename VEC3, typename MAT3x3, typename MAT3x9>
820 inline void M3x9(const VEC3& a, const MAT3x3& B, MAT3x9& RES)
821 {
822  alignas(MRPT_MAX_ALIGN_BYTES) const double vals[] = {
823  a[0], -B(0, 2), B(0, 1), B(0, 2), a[0], -B(0, 0), -B(0, 1),
824  B(0, 0), a[0], a[1], -B(1, 2), B(1, 1), B(1, 2), a[1],
825  -B(1, 0), -B(1, 1), B(1, 0), a[1], a[2], -B(2, 2), B(2, 1),
826  B(2, 2), a[2], -B(2, 0), -B(2, 1), B(2, 0), a[2]};
827  RES.loadFromArray(vals);
828 }
829 
831 {
832  const CMatrixDouble33& R = P.getRotationMatrix();
833  const CArrayDouble<3>& t = P.m_coords;
834 
835  CArrayDouble<3> abc;
836  deltaR(R, abc);
837  double a = abc[0];
838  double b = abc[1];
839  double c = abc[2];
840 
841  alignas(MRPT_MAX_ALIGN_BYTES) const double vals[] = {
842  -b * t[1] - c * t[2], 2 * b * t[0] - a * t[1],
843  2 * c * t[0] - a * t[2], -b * t[0] + 2 * a * t[1],
844  -a * t[0] - c * t[2], 2 * c * t[1] - b * t[2],
845  -c * t[0] + 2 * a * t[2], -c * t[1] + 2 * b * t[2],
846  -a * t[0] - b * t[1]};
847  return CMatrixDouble33(vals);
848 }
849 
851 {
854 
855  const CMatrixDouble33& R = P.getRotationMatrix();
856  const CArrayDouble<3>& t = P.m_coords;
857 
858  const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
859 
860  if (d > 0.9999)
861  {
862  a[0] = a[1] = a[2] = 0;
863  B.zeros();
864  }
865  else
866  {
867  const double theta = acos(d);
868  const double theta2 = square(theta);
869  const double oned2 = (1 - square(d));
870  const double sq = std::sqrt(oned2);
871  const double cot = 1. / tan(0.5 * theta);
872  const double csc2 = square(1. / sin(0.5 * theta));
873 
875  CArrayDouble<3> vr;
876  deltaR(R, vr);
877  mrpt::math::skew_symmetric3(vr, skewR);
878 
879  CArrayDouble<3> skewR_t;
880  skewR.multiply_Ab(t, skewR_t);
881 
882  skewR_t *= -(d * theta - sq) / (8 * pow(sq, 3));
883  a = skewR_t;
884 
886  skewR2.multiply_AB(skewR, skewR);
887 
888  CArrayDouble<3> skewR2_t;
889  skewR2.multiply_Ab(t, skewR2_t);
890  skewR2_t *=
891  (((theta * sq - d * theta2) * (0.5 * theta * cot - 1)) -
892  theta * sq * ((0.25 * theta * cot) + 0.125 * theta2 * csc2 - 1)) /
893  (4 * theta2 * square(oned2));
894  a += skewR2_t;
895 
897  B *= -0.5 * theta / (2 * sq);
898 
899  B += -(theta * cot - 2) / (8 * oned2) * ddeltaRt_dR(P);
900  }
901  M3x9(a, B, J);
902 }
903 }
904 
906 {
907  J.zeros();
908  // Jacobian structure 6x12:
909  // (3rows, for t) [ d_Vinvt_dR (3x9) | Vinv (3x3) ]
910  // [ -------------------------+------------- ]
911  // (3rows, for \omega) [ d_lnR_dR (3x9) | 0 (3x3) ]
912  //
913  // derivs wrt: R_col1 R_col2 R_col3 | t
914  //
915  // (Will be explained better in:
916  // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty )
917  //
918  {
920  ln_rot_jacob(m_ROT, M);
921  J.insertMatrix(3, 0, M);
922  }
923  {
925  dVinvt_dR(*this, M);
926  J.insertMatrix(0, 0, M);
927  }
928 
929  const CMatrixDouble33& R = m_ROT;
930  CArrayDouble<3> omega;
932 
934  V_inv.unit(3, 1.0); // Start with the identity_3
935 
936  const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
937  if (d > 0.99999)
938  {
939  mrpt::poses::deltaR(R, omega);
940  omega *= 0.5;
941  mrpt::math::skew_symmetric3(omega, Omega);
943  Omega2.multiply_AAt(Omega);
944  Omega2 *= 1.0 / 12.0;
945 
946  Omega *= 0.5;
947 
948  V_inv -= Omega;
949  V_inv -= Omega2;
950  }
951  else
952  {
953  mrpt::poses::deltaR(R, omega);
954 
955  const double theta = acos(d);
956  omega *= theta / (2 * std::sqrt(1 - d * d));
957 
958  mrpt::math::skew_symmetric3(omega, Omega);
959 
961  Omega2.multiply_AAt(Omega);
962 
963  Omega2 *= (1 - theta / (2 * std::tan(theta * 0.5))) / square(theta);
964  Omega *= 0.5;
965 
966  V_inv -= Omega;
967  V_inv += Omega2;
968  }
969  J.insertMatrix(0, 9, V_inv);
970 }
971 
974 {
975  const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
978  if (d > 0.99999)
979  {
980  a[0] = a[1] = a[2] = 0;
981  B.unit(3, -0.5);
982  }
983  else
984  {
985  const double theta = acos(d);
986  const double d2 = square(d);
987  const double sq = std::sqrt(1 - d2);
988  deltaR(R, a);
989  a *= (d * theta - sq) / (4 * (sq * sq * sq));
990  B.unit(3, -theta / (2 * sq));
991  }
992  M3x9(a, B, M);
993 }
994 
995 // Eq. 10.3.5 in tech report
996 // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
998  const CPose3D& D, Eigen::Matrix<double, 12, 6>& jacob)
999 {
1000  jacob.block<9, 3>(0, 0).setZero();
1001  jacob.block<3, 3>(9, 0).setIdentity();
1002  for (int i = 0; i < 3; i++)
1003  {
1004  Eigen::Block<Eigen::Matrix<double, 12, 6>, 3, 3, false> trg_blc =
1005  jacob.block<3, 3>(3 * i, 3);
1006  mrpt::math::skew_symmetric3_neg(D.m_ROT.block<3, 1>(0, i), trg_blc);
1007  }
1008  {
1009  Eigen::Block<Eigen::Matrix<double, 12, 6>, 3, 3, false> trg_blc =
1010  jacob.block<3, 3>(9, 3);
1012  }
1013 }
1014 
1015 // Eq. 10.3.7 in tech report
1016 // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
1018  const CPose3D& A, const CPose3D& D, Eigen::Matrix<double, 12, 6>& jacob)
1019 {
1020  jacob.block<9, 3>(0, 0).setZero();
1021  jacob.block<3, 3>(9, 0) = A.getRotationMatrix();
1022  Eigen::Matrix<double, 3, 3> aux;
1023  for (int i = 0; i < 3; i++)
1024  {
1026  D.getRotationMatrix().block<3, 1>(0, i), aux);
1027  jacob.block<3, 3>(3 * i, 3) = A.m_ROT * aux;
1028  }
1030  jacob.block<3, 3>(9, 3) = A.m_ROT * aux;
1031 }
1032 
1034 {
1035  for (int i = 0; i < 3; i++)
1036  for (int j = 0; j < 3; j++)
1037  m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();
1038 
1039  for (int i = 0; i < 3; i++)
1040  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
1041 }
1042 
1044 {
1045  return mrpt::math::TPose3D(x(), y(), z(), yaw(), pitch(), roll());
1046 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1043
#define local
Definition: zutil.h:47
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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:621
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose3D.cpp:339
GLdouble GLdouble t
Definition: glext.h:3689
#define MRPT_MAX_ALIGN_BYTES
GLdouble GLdouble z
Definition: glext.h:3872
static mrpt::math::CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric< double, 3 > &vect)
Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
Definition: CPose3D.cpp:794
double RAD2DEG(const double x)
Radians to degrees.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mxArray * convertToMatlab(const Eigen::EigenBase< Derived > &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:36
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:843
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:1033
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:608
double roll
Roll coordinate (rotation angle over X coordinate).
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in 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
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44
Definition: eigen_frwds.h:58
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose3D.cpp:136
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
Definition: CPose3D.cpp:785
This file implements several operations that operate element-wise on individual or pairs of container...
double x
X,Y,Z, coords.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:532
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
Definition: CPose3D.h:254
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
Definition: CPose3D.cpp:258
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< 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:510
double yaw
Yaw coordinate (rotation angle over Z axis).
GLdouble s
Definition: glext.h:3676
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:593
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:242
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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:363
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
static void jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
Definition: CPose3D.cpp:997
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
Definition: geometry.h:877
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
void dVinvt_dR(const CPose3D &P, CMatrixFixedNumeric< double, 3, 9 > &J)
Definition: CPose3D.cpp:850
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:96
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:565
const GLubyte * c
Definition: glext.h:6313
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:172
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:315
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Definition: CPose3D.cpp:348
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:314
void M3x9(const VEC3 &a, const MAT3x3 &B, MAT3x9 &RES)
Definition: CPose3D.cpp:820
GLubyte GLubyte b
Definition: glext.h:6279
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
double x
X,Y,Z coordinates.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose3D.cpp:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:46
void deltaR(const MAT33 &R, VEC3 &v)
Definition: CPose3D.cpp:812
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
double m_yaw
These variables are updated every time that the object rotation matrix is modified (construction...
Definition: CPose3D.h:109
A class used to store a 2D point.
Definition: CPoint2D.h:33
A class used to store a 3D point.
Definition: CPoint3D.h:31
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:538
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).
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:291
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:306
void ln_jacob(mrpt::math::CMatrixFixedNumeric< double, 6, 12 > &J) const
Jacobian of the logarithm of the 3x4 matrix defined by this pose.
Definition: CPose3D.cpp:905
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixDouble33 ddeltaRt_dR(const CPose3D &P)
Definition: CPose3D.cpp:830
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:164
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:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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:68
static void ln_rot_jacob(const mrpt::math::CMatrixDouble33 &R, mrpt::math::CMatrixFixedNumeric< double, 3, 9 > &M)
Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rot...
Definition: CPose3D.cpp:972
This file implements matrix/vector text and binary serialization.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< 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:379
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:57
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:51
CPose3D()
Default constructor, with all the coordinates set to zero.
Definition: CPose3D.cpp:54
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:239
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:235
mrpt::math::CArrayDouble< 6 > ln() const
Definition: CPose3D.h:680
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:42
GLenum GLint x
Definition: glext.h:3538
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:230
Lightweight 3D point.
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x6 vector with [x y z yaw pitch roll].
Definition: CPose3D.cpp:477
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:22
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
static CPose3D exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, bool pseudo_exponential=false)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
Definition: CPose3D.cpp:758
GLfloat GLfloat p
Definition: glext.h:6305
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3D.cpp:137
static void jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.
Definition: CPose3D.cpp:1017
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:649
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:138



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020