Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/config.h> // for HAVE_SINCOS
13 #include <mrpt/utils/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...
24 #include <mrpt/utils/CStream.h> // for CStream
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/utils/CSerializable.h> // for CSeriali...
39 #include <mrpt/utils/bits.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 using namespace mrpt::utils;
47 using namespace mrpt::poses;
48 
49 
51 
52 /*---------------------------------------------------------------
53  Constructors
54  ---------------------------------------------------------------*/
56  : m_ypr_uptodate(true), m_yaw(0),m_pitch(0),m_roll(0)
57 {
58  m_coords[0] =
59  m_coords[1] =
60  m_coords[2] = 0;
61  m_ROT.unit(3,1.0);
62 }
63 
64 CPose3D::CPose3D(const double x,const double y,const double z, const double yaw, const double pitch, const double roll)
65  : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
66 {
68 }
69 
71  : m_ypr_uptodate(false)
72 {
73  setFromValues(o.x,o.y,o.z,o.yaw,o.pitch,o.roll);
74 }
75 
77  : m_ypr_uptodate(false)
78 {
79  setFromValues(p.x(),p.y(),0, p.phi(),0,0);
80 }
81 
83  : m_ypr_uptodate(false), m_yaw(),m_pitch(),m_roll()
84 {
85  setFromValues(p.x(),p.y(),p.z());
86 }
87 
89  : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false)
90 {
93  for (int r=0;r<3;r++)
94  for (int c=0;c<3;c++)
95  m_ROT(r,c)=m.get_unsafe(r,c);
96  for (int r=0;r<3;r++)
97  m_coords[r] = m.get_unsafe(r,3);
98 }
99 
100 
102  : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false)
103 {
104  for (int r=0;r<3;r++)
105  for (int c=0;c<3;c++)
106  m_ROT(r,c)=m.get_unsafe(r,c);
107  for (int r=0;r<3;r++)
108  m_coords[r] = m.get_unsafe(r,3);
109 }
110 
111 /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
112 CPose3D::CPose3D(const mrpt::math::CQuaternionDouble &q, const double _x, const double _y, const double _z )
113  : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false)
114 {
115  double yaw,pitch,roll;
116  q.rpy(roll,pitch,yaw);
117  this->setFromValues(_x,_y,_z,yaw,pitch,roll);
118 }
119 
120 /** Constructor from a quaternion-based full pose. */
122  : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false)
123 {
124  // Extract XYZ + ROT from quaternion:
125  m_coords[0] = p.x();
126  m_coords[1] = p.y();
127  m_coords[2] = p.z();
128  p.quat().rotationMatrixNoResize(m_ROT);
129 }
130 
131 /** Constructor from a rotation vector-based full pose. */
133  : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false)
134 {
135  m_coords[0] = p.m_coords[0];
136  m_coords[1] = p.m_coords[1];
137  m_coords[2] = p.m_coords[2];
138 
139  this->setRotationMatrix( this->exp_rotation( p.m_rotvec ) );
140 }
141 
142 /*---------------------------------------------------------------
143  Implements the writing to a CStream capability of
144  CSerializable objects
145  ---------------------------------------------------------------*/
147 {
148  if (version)
149  *version = 2;
150  else
151  {
152  const CPose3DQuat q(*this);
153  // The coordinates:
154  out << q[0] << q[1] << q[2] << q[3] << q[4] << q[5] << q[6];
155  }
156 }
157 
158 /*---------------------------------------------------------------
159  Implements the reading from a CStream capability of
160  CSerializable objects
161  ---------------------------------------------------------------*/
163 {
164  switch(version)
165  {
166  case 0:
167  {
168  // The coordinates:
169  CMatrix HM2;
170  in >> HM2;
171  ASSERT_(mrpt::math::size(HM2,1)==4 && HM2.isSquare())
172 
173  m_ROT = HM2.block(0,0,3,3).cast<double>();
174 
175  m_coords[0] = HM2.get_unsafe(0,3);
176  m_coords[1] = HM2.get_unsafe(1,3);
177  m_coords[2] = HM2.get_unsafe(2,3);
178  m_ypr_uptodate = false;
179  } break;
180  case 1:
181  {
182  // The coordinates:
183  CMatrixDouble44 HM;
184  in >> HM;
185 
186  m_ROT = HM.block(0,0,3,3);
187 
188  m_coords[0] = HM.get_unsafe(0,3);
189  m_coords[1] = HM.get_unsafe(1,3);
190  m_coords[2] = HM.get_unsafe(2,3);
191  m_ypr_uptodate = false;
192  } break;
193  case 2:
194  {
195  // An equivalent CPose3DQuat
197  in >>p[0]>>p[1]>>p[2]>>p[3]>>p[4]>>p[5]>>p[6];
198 
199  // Extract XYZ + ROT from quaternion:
200  m_ypr_uptodate = false;
201  m_coords[0] = p.x();
202  m_coords[1] = p.y();
203  m_coords[2] = p.z();
204  p.quat().rotationMatrixNoResize(m_ROT);
205  } break;
206  default:
208 
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) << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
219  << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg," << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
220  o.flags(old_flags);
221  o.precision(old_pre);
222  return o;
223 }
224 
225 /*---------------------------------------------------------------
226  Implements the writing to a mxArray for Matlab
227  ---------------------------------------------------------------*/
228 #if MRPT_HAS_MATLAB
229 // Add to implement mexplus::from template specialization
231 
233 {
234  const char* fields[] = {"R","t"};
235  mexplus::MxArray pose_struct( mexplus::MxArray::Struct(sizeof(fields)/sizeof(fields[0]),fields) );
236  pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
237  pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
238  return pose_struct.release();
239 }
240 #endif
241 
242 /*---------------------------------------------------------------
243  normalizeAngles
244 ---------------------------------------------------------------*/
246 {
248 }
249 
250 
251 /*---------------------------------------------------------------
252  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
253 ---------------------------------------------------------------*/
255  const double x0,
256  const double y0,
257  const double z0,
258  const double yaw,
259  const double pitch,
260  const double roll)
261 {
262  m_coords[0] = x0;
263  m_coords[1] = y0;
264  m_coords[2] = z0;
265  this->m_yaw = mrpt::math::wrapToPi(yaw);
266  this->m_pitch = mrpt::math::wrapToPi(pitch);
267  this->m_roll = mrpt::math::wrapToPi(roll);
268 
269  m_ypr_uptodate = true;
270 
272 }
273 
274 /*---------------------------------------------------------------
275  Set the pose from 3D point and yaw/pitch/roll angles, in radians.
276 ---------------------------------------------------------------*/
278 {
279 #ifdef HAVE_SINCOS
280  double cy,sy;
281  ::sincos(m_yaw,&sy,&cy);
282  double cp,sp;
283  ::sincos(m_pitch,&sp,&cp);
284  double cr,sr;
285  ::sincos(m_roll,&sr,&cr);
286 #else
287  const double cy = cos(m_yaw);
288  const double sy = sin(m_yaw);
289  const double cp = cos(m_pitch);
290  const double sp = sin(m_pitch);
291  const double cr = cos(m_roll);
292  const double sr = sin(m_roll);
293 #endif
294 
295  MRPT_ALIGN16 const double rot_vals[] = {
296  cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr,
297  sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr,
298  -sp, cp*sr, cp*cr
299  };
300  m_ROT.loadFromArray(rot_vals);
301 }
302 
303 /*---------------------------------------------------------------
304  Scalar multiplication.
305 ---------------------------------------------------------------*/
306 void CPose3D::operator *=(const double s)
307 {
309  m_coords[0]*=s;
310  m_coords[1]*=s;
311  m_coords[2]*=s;
312  m_yaw*=s;
313  m_pitch*=s;
314  m_roll*=s;
316 }
317 
318 /*---------------------------------------------------------------
319  getYawPitchRoll
320 ---------------------------------------------------------------*/
321 void CPose3D::getYawPitchRoll( double &yaw, double &pitch, double &roll ) const
322 {
323  ASSERTDEBMSG_( std::abs(sqrt(square(m_ROT(0,0))+square(m_ROT(1,0))+square(m_ROT(2,0))) - 1 ) < 3e-3, "Homogeneous matrix is not orthogonal & normalized!: "+m_ROT.inMatlabFormat() )
324  ASSERTDEBMSG_( std::abs(sqrt(square(m_ROT(0,1))+square(m_ROT(1,1))+square(m_ROT(2,1))) - 1 ) < 3e-3, "Homogeneous matrix is not orthogonal & normalized!: "+m_ROT.inMatlabFormat() )
325  ASSERTDEBMSG_( std::abs(sqrt(square(m_ROT(0,2))+square(m_ROT(1,2))+square(m_ROT(2,2))) - 1 ) < 3e-3, "Homogeneous matrix is not orthogonal & normalized!: "+m_ROT.inMatlabFormat() )
326 
327  // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
328  pitch = atan2( - m_ROT(2,0), hypot( m_ROT(0,0),m_ROT(1,0) ) ); //asin( - m_ROT(2,0) );
329 
330  // Roll:
331  if ( (fabs(m_ROT(2,1))+fabs(m_ROT(2,2)))<10*std::numeric_limits<double>::epsilon() )
332  {
333  //Gimbal lock between yaw and roll. This one is arbitrarily forced to be zero.
334  //Check http://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html. If cos(pitch)==0, the homogeneous matrix is:
335  //When sin(pitch)==1:
336  // /0 cysr-sycr cycr+sysr x\ /0 sin(r-y) cos(r-y) x\.
337  // |0 sysr+cycr sycr-cysr y| = |0 cos(r-y) -sin(r-y) y|
338  // |-1 0 0 z| |-1 0 0 z|
339  // \0 0 0 1/ \0 0 0 1/
340  //
341  //And when sin(pitch)=-1:
342  // /0 -cysr-sycr -cycr+sysr x\ /0 -sin(r+y) -cos(r+y) x\.
343  // |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y) -sin(r+y) y|
344  // |1 0 0 z| |1 0 0 z|
345  // \0 0 0 1/ \0 0 0 1/
346  //
347  //Both cases are in a "gimbal lock" status. This happens because pitch is vertical.
348 
349  roll = 0.0;
350  if (pitch>0) yaw=atan2(m_ROT(1,2),m_ROT(0,2));
351  else yaw=atan2(-m_ROT(1,2),-m_ROT(0,2));
352  }
353  else
354  {
355  roll = atan2( m_ROT(2,1), m_ROT(2,2) );
356  // Yaw:
357  yaw = atan2( m_ROT(1,0), m_ROT(0,0) );
358  }
359 }
360 
361 
362 /*---------------------------------------------------------------
363  sphericalCoordinates
364 ---------------------------------------------------------------*/
366  const TPoint3D &point,
367  double &out_range,
368  double &out_yaw,
369  double &out_pitch ) const
370 {
371  // Pass to coordinates as seen from this 6D pose:
372  TPoint3D local;
373  this->inverseComposePoint(point.x,point.y,point.z, local.x,local.y,local.z);
374 
375  // Range:
376  out_range = local.norm();
377 
378  // Yaw:
379  if (local.y!=0 || local.x!=0)
380  out_yaw = atan2(local.y,local.x);
381  else out_yaw = 0;
382 
383  // Pitch:
384  if (out_range!=0)
385  out_pitch = -asin( local.z / out_range );
386  else out_pitch = 0;
387 
388 }
389 
390 
391 /*---------------------------------------------------------------
392  addComponents
393 ---------------------------------------------------------------*/
395 {
397  m_coords[0]+=p.m_coords[0];
398  m_coords[1]+=p.m_coords[1];
399  m_coords[2]+=p.m_coords[2];
400  m_yaw+=p.m_yaw;
401  m_pitch+=p.m_pitch;
402  m_roll+=p.m_roll;
404 }
405 
406 
407 /*---------------------------------------------------------------
408  distanceEuclidean6D
409 ---------------------------------------------------------------*/
410 double CPose3D::distanceEuclidean6D( const CPose3D &o ) const
411 {
413  o.updateYawPitchRoll();
414  return sqrt(
415  square( o.m_coords[0] - m_coords[0] ) +
416  square( o.m_coords[1] - m_coords[1] ) +
417  square( o.m_coords[2] - m_coords[2] ) +
418  square( wrapToPi( o.m_yaw - m_yaw ) ) +
419  square( wrapToPi( o.m_pitch - m_pitch ) ) +
420  square( wrapToPi( o.m_roll - m_roll ) ) );
421 }
422 
423 
424 /*---------------------------------------------------------------
425  composePoint
426 ---------------------------------------------------------------*/
427 void CPose3D::composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
428  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
429  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose,
430  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3,
431  bool use_small_rot_approx ) const
432 {
433  // Jacob: df/dpoint
434  if (out_jacobian_df_dpoint)
435  *out_jacobian_df_dpoint = m_ROT;
436 
437  // Jacob: df/dpose
438  if (out_jacobian_df_dpose)
439  {
440  if (use_small_rot_approx)
441  {
442  // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
443  MRPT_ALIGN16 const double nums[3*6] = {
444  1, 0, 0, -ly, lz, 0,
445  0, 1, 0, lx, 0, -lz,
446  0, 0, 1, 0, -lx, ly };
447  out_jacobian_df_dpose->loadFromArray(nums);
448  }
449  else
450  {
451  // Exact Jacobians:
453  # ifdef HAVE_SINCOS
454  double cy,sy;
455  ::sincos(m_yaw,&sy,&cy);
456  double cp,sp;
457  ::sincos(m_pitch,&sp,&cp);
458  double cr,sr;
459  ::sincos(m_roll,&sr,&cr);
460  # else
461  const double cy = cos(m_yaw);
462  const double sy = sin(m_yaw);
463  const double cp = cos(m_pitch);
464  const double sp = sin(m_pitch);
465  const double cr = cos(m_roll);
466  const double sr = sin(m_roll);
467  # endif
468 
469  MRPT_ALIGN16 const double nums[3*6] = {
470  1, 0, 0,
471  -lx*sy*cp+ly*(-sy*sp*sr-cy*cr)+lz*(-sy*sp*cr+cy*sr), // d_x'/d_yaw
472  -lx*cy*sp+ly*(cy*cp*sr )+lz*(cy*cp*cr ), // d_x'/d_pitch
473  ly*(cy*sp*cr+sy*sr)+lz*(-cy*sp*sr+sy*cr), // d_x'/d_roll
474  0, 1, 0,
475  lx*cy*cp+ly*(cy*sp*sr-sy*cr)+lz*(cy*sp*cr+sy*sr), // d_y'/d_yaw
476  -lx*sy*sp+ly*(sy*cp*sr) +lz*(sy*cp*cr ), // d_y'/d_pitch
477  ly*(sy*sp*cr-cy*sr)+lz*(-sy*sp*sr-cy*cr), // d_y'/d_roll
478  0, 0, 1,
479  0, // d_z' / d_yaw
480  -lx*cp-ly*sp*sr-lz*sp*cr, // d_z' / d_pitch
481  ly*cp*cr-lz*cp*sr // d_z' / d_roll
482  };
483  out_jacobian_df_dpose->loadFromArray(nums);
484  }
485  }
486 
487  gx=m_ROT(0,0)*lx+m_ROT(0,1)*ly+m_ROT(0,2)*lz+m_coords[0];
488  gy=m_ROT(1,0)*lx+m_ROT(1,1)*ly+m_ROT(1,2)*lz+m_coords[1];
489  gz=m_ROT(2,0)*lx+m_ROT(2,1)*ly+m_ROT(2,2)*lz+m_coords[2];
490 
491  // Jacob: df/dse3
492  if (out_jacobian_df_dse3)
493  {
494  MRPT_ALIGN16 const double nums[3*6] = {
495  1, 0, 0, 0, gz, -gy,
496  0, 1, 0, -gz, 0, gx,
497  0, 0, 1, gy,-gx, 0 };
498  out_jacobian_df_dse3->loadFromArray(nums);
499  }
500 }
501 
502 
503 // TODO: Use SSE2? OTOH, this forces mem align...
504 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
505 /*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
506 {
507  ASSERT_(((size_t)matrix & 15) == 0);
508  __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
509  __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
510  __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)), _mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));
511 
512  return _mm_add_ps(_mm_add_ps(a0,a1),a2);
513 }*/
514 #endif // SSE2
515 
516 /*---------------------------------------------------------------
517  getAsVector
518 ---------------------------------------------------------------*/
520 {
522  r.resize(6);
523  r[0]=m_coords[0];
524  r[1]=m_coords[1];
525  r[2]=m_coords[2];
526  r[3]=m_yaw;
527  r[4]=m_pitch;
528  r[5]=m_roll;
529 }
530 
532 {
534  r[0]=m_coords[0];
535  r[1]=m_coords[1];
536  r[2]=m_coords[2];
537  r[3]=m_yaw;
538  r[4]=m_pitch;
539  r[5]=m_roll;
540 }
541 
542 
543 /*---------------------------------------------------------------
544  unary -
545 ---------------------------------------------------------------*/
547 {
549  b.getInverseHomogeneousMatrix( B_INV );
550  return CPose3D(B_INV);
551 }
552 
554 {
556  mrpt::math::TPose3D(0, 0, 0, m_yaw, m_pitch, m_roll).getAsQuaternion(q, out_dq_dr);
557 }
558 
559 bool mrpt::poses::operator==(const CPose3D &p1,const CPose3D &p2)
560 {
561  return (p1.m_coords==p2.m_coords)&&(p1.getRotationMatrix()==p2.getRotationMatrix());
562 }
563 
564 bool mrpt::poses::operator!=(const CPose3D &p1,const CPose3D &p2)
565 {
566  return (p1.m_coords!=p2.m_coords)||(p1.getRotationMatrix()!=p2.getRotationMatrix());
567 }
568 
569 /*---------------------------------------------------------------
570  point3D = pose3D + point3D
571  ---------------------------------------------------------------*/
573 {
574  return CPoint3D(
575  m_coords[0] + m_ROT(0,0)*b.x() + m_ROT(0,1)*b.y() +m_ROT(0,2)*b.z(),
576  m_coords[1] + m_ROT(1,0)*b.x() + m_ROT(1,1)*b.y() +m_ROT(1,2)*b.z(),
577  m_coords[2] + m_ROT(2,0)*b.x() + m_ROT(2,1)*b.y() +m_ROT(2,2)*b.z());
578 }
579 
580 /*---------------------------------------------------------------
581  point3D = pose3D + point2D
582  ---------------------------------------------------------------*/
584 {
585  return CPoint3D(
586  m_coords[0] + m_ROT(0,0)*b.x() + m_ROT(0,1)*b.y(),
587  m_coords[1] + m_ROT(1,0)*b.x() + m_ROT(1,1)*b.y(),
588  m_coords[2] + m_ROT(2,0)*b.x() + m_ROT(2,1)*b.y());
589 }
590 
591 
592 /*---------------------------------------------------------------
593  this = A + B
594  ---------------------------------------------------------------*/
595 void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B )
596 {
597  // The translation part HM(0:3,3)
598  if (this==&B)
599  {
600  // we need to make a temporary copy of the vector:
601  const CArrayDouble<3> B_coords = B.m_coords;
602  for (int r=0;r<3;r++)
603  m_coords[r] = A.m_coords[r] + A.m_ROT(r,0)*B_coords[0]+A.m_ROT(r,1)*B_coords[1]+A.m_ROT(r,2)*B_coords[2];
604  }
605  else
606  {
607  for (int r=0;r<3;r++)
608  m_coords[r] = A.m_coords[r] + A.m_ROT(r,0)*B.m_coords[0]+A.m_ROT(r,1)*B.m_coords[1]+A.m_ROT(r,2)*B.m_coords[2];
609  }
610 
611  // Important: Make this multiplication AFTER the translational part, to cope with the case when A==this
612  m_ROT.multiply_AB( A.m_ROT, B.m_ROT );
613 
614  m_ypr_uptodate=false;
615 }
616 
617 /** Convert this pose into its inverse, saving the result in itself. */
619 {
621  CArrayDouble<3> inv_xyz;
622 
624 
625  m_ROT = inv_rot;
626  m_coords = inv_xyz;
627  m_ypr_uptodate=false;
628 }
629 
630 /*---------------------------------------------------------------
631  isHorizontal
632  ---------------------------------------------------------------*/
633 bool CPose3D::isHorizontal( const double tolerance ) const
634 {
636  return (fabs(m_pitch)<=tolerance || M_PI-fabs(m_pitch) <=tolerance ) &&
637  ( fabs(m_roll)<=tolerance || fabs(mrpt::math::wrapToPi( m_roll-M_PI))<=tolerance );
638 }
639 
640 
641 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
642  * \note A or B can be "this" without problems.
643  * \sa composeFrom, composePoint
644  */
645 void CPose3D::inverseComposeFrom(const CPose3D& A, const CPose3D& B )
646 {
647  // this = A (-) B
648  // HM_this = inv(HM_B) * HM_A
649  //
650  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
651  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
652  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
653  //
654 
655  // XYZ part:
657  CArrayDouble<3> t_b_inv;
659 
660  for (int i=0;i<3;i++)
661  m_coords[i] = t_b_inv[i] + R_b_inv(i,0)*A.m_coords[0]+ R_b_inv(i,1)*A.m_coords[1]+ R_b_inv(i,2)*A.m_coords[2];
662 
663  // Rot part:
664  m_ROT.multiply_AB( R_b_inv, A.m_ROT );
665  m_ypr_uptodate=false;
666 }
667 
668 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
669  * \sa composePoint, composeFrom
670  */
671 void CPose3D::inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
672  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
673  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose,
674  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3
675  )const
676 {
678  CArrayDouble<3> t_inv;
680 
681  // Jacob: df/dpoint
682  if (out_jacobian_df_dpoint)
683  *out_jacobian_df_dpoint = R_inv;
684 
685  // Jacob: df/dpose
686  if (out_jacobian_df_dpose)
687  {
688  // TODO: Perhaps this and the sin/cos's can be avoided if all needed terms are already in m_ROT ???
690 
691 # ifdef HAVE_SINCOS
692  double cy,sy;
693  ::sincos(m_yaw,&sy,&cy);
694  double cp,sp;
695  ::sincos(m_pitch,&sp,&cp);
696  double cr,sr;
697  ::sincos(m_roll,&sr,&cr);
698 # else
699  const double cy = cos(m_yaw);
700  const double sy = sin(m_yaw);
701  const double cp = cos(m_pitch);
702  const double sp = sin(m_pitch);
703  const double cr = cos(m_roll);
704  const double sr = sin(m_roll);
705 # endif
706 
707  const double m11_dy = -sy*cp; const double m12_dy = cy*cp; const double m13_dy = 0;
708  const double m11_dp = -cy*sp; const double m12_dp = -sy*sp; const double m13_dp = -cp;
709  const double m11_dr = 0; const double m12_dr = 0; const double m13_dr = 0;
710 
711  const double m21_dy = (-sy*sp*sr-cy*cr); const double m22_dy = (cy*sp*sr-sy*cr); const double m23_dy = 0;
712  const double m21_dp = (cy*cp*sr ); const double m22_dp = (sy*cp*sr ); const double m23_dp = -sp*sr;
713  const double m21_dr = (cy*sp*cr+sy*sr); const double m22_dr = (sy*sp*cr-cy*sr); const double m23_dr = cp*cr;
714 
715  const double m31_dy = (-sy*sp*cr+cy*sr); const double m32_dy = (cy*sp*cr+sy*sr); const double m33_dy = 0;
716  const double m31_dp = (cy*cp*cr ); const double m32_dp = (sy*cp*cr ); const double m33_dp = -sp*cr;
717  const double m31_dr = (-cy*sp*sr+sy*cr); const double m32_dr = (-sy*sp*sr-cy*cr); const double m33_dr = -cp*sr;
718 
719  const double Ax = gx-m_coords[0];
720  const double Ay = gy-m_coords[1];
721  const double Az = gz-m_coords[2];
722 
723  MRPT_ALIGN16 const double nums[3*6] = {
724  -m_ROT(0,0), -m_ROT(1,0), -m_ROT(2,0),
725  Ax*m11_dy + Ay*m12_dy + Az*m13_dy , // d_x'/d_yaw
726  Ax*m11_dp + Ay*m12_dp + Az*m13_dp, // d_x'/d_pitch
727  Ax*m11_dr + Ay*m12_dr + Az*m13_dr, // d_x'/d_roll
728 
729  -m_ROT(0,1), -m_ROT(1,1), -m_ROT(2,1),
730  Ax*m21_dy + Ay*m22_dy + Az*m23_dy, // d_x'/d_yaw
731  Ax*m21_dp + Ay*m22_dp + Az*m23_dp, // d_x'/d_pitch
732  Ax*m21_dr + Ay*m22_dr + Az*m23_dr, // d_x'/d_roll
733 
734  -m_ROT(0,2), -m_ROT(1,2), -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  MRPT_ALIGN16 const double nums[3*6] = {
750  -1, 0, 0, 0,-lz, ly,
751  0, -1, 0, lz, 0, -lx,
752  0, 0, -1, -ly, lx, 0 };
753  out_jacobian_df_dse3->loadFromArray(nums);
754  }
755 }
756 
757 CPose3D CPose3D::exp(const mrpt::math::CArrayNumeric<double,6> & mu,bool pseudo_exponential )
758 {
760  CPose3D::exp(mu,P,pseudo_exponential);
761  return P;
762 }
763 
764 void CPose3D::exp(const mrpt::math::CArrayNumeric<double,6> & mu, CPose3D &out_pose, bool pseudo_exponential)
765 {
766  if (pseudo_exponential)
767  {
768  auto R = Sophus::SO3<double>::exp( mu.block<3,1>(3,0) );
769  out_pose.setRotationMatrix(R.matrix());
770  out_pose.x(mu[0]);
771  out_pose.y(mu[1]);
772  out_pose.z(mu[2]);
773  }
774  else
775  {
776  auto R = Sophus::SE3<double>::exp(mu);
777  out_pose = CPose3D( CMatrixDouble44(R.matrix()));
778  }
779 }
780 
782 {
783  Sophus::SO3<double> R(this->m_ROT);
784  return R.log();
785 }
786 
788 {
789  auto R = Sophus::SO3<double>::exp(w);
790  return R.matrix();
791 }
792 
793 void CPose3D::ln(CArrayDouble<6> &result) const
794 {
795  const Sophus::SE3<double> RT(m_ROT, m_coords);
796  result = RT.log();
797 }
798 
799 /* The following code fragments are based on formulas originally reported in the TooN and RobotVision packages */
800 namespace mrpt
801 {
802  namespace poses
803  {
804  template <class VEC3,class MAT33>
805  inline void deltaR(const MAT33& R, VEC3 &v)
806  {
807  v[0]=R(2,1)-R(1,2);
808  v[1]=R(0,2)-R(2,0);
809  v[2]=R(1,0)-R(0,1);
810  }
811 
812  template <typename VEC3,typename MAT3x3,typename MAT3x9>
813  inline void M3x9(
814  const VEC3 &a,
815  const MAT3x3 &B,
816  MAT3x9 &RES)
817  {
818  MRPT_ALIGN16 const double vals[] = {
819  a[0], -B(0,2), B(0,1), B(0,2), a[0], -B(0,0),-B(0,1), B(0,0), a[0],
820  a[1], -B(1,2), B(1,1), B(1,2), a[1], -B(1,0),-B(1,1), B(1,0), a[1],
821  a[2], -B(2,2), B(2,1), B(2,2), a[2], -B(2,0),-B(2,1), B(2,0), a[2]
822  };
823  RES.loadFromArray(vals);
824  }
825 
827  {
828  const CMatrixDouble33 &R = P.getRotationMatrix();
829  const CArrayDouble<3> &t = P.m_coords;
830 
831  CArrayDouble<3> abc;
832  deltaR(R,abc);
833  double a = abc[0];
834  double b = abc[1];
835  double c = abc[2];
836 
837  MRPT_ALIGN16 const double vals[] = {
838  -b*t[1]-c*t[2], 2*b*t[0]-a*t[1], 2*c*t[0]-a*t[2],
839  -b*t[0]+2*a*t[1],-a*t[0]-c*t[2], 2*c*t[1]-b*t[2],
840  -c*t[0]+2*a*t[2],-c*t[1]+2*b*t[2],-a*t[0]-b*t[1]
841  };
842  return CMatrixDouble33(vals);
843  }
844 
845 
847  {
850 
851  const CMatrixDouble33 &R = P.getRotationMatrix();
852  const CArrayDouble<3> &t = P.m_coords;
853 
854  const double d = 0.5*( R(0,0)+R(1,1)+R(2,2)-1);
855 
856  if (d>0.9999)
857  {
858  a[0]=a[1]=a[2]=0;
859  B.zeros();
860  }
861  else
862  {
863  const double theta = acos(d);
864  const double theta2 = square(theta);
865  const double oned2 = (1-square(d));
866  const double sq = std::sqrt(oned2);
867  const double cot = 1./tan(0.5*theta);
868  const double csc2 = square(1./sin(0.5*theta));
869 
871  CArrayDouble<3> vr;
872  deltaR(R,vr);
873  mrpt::math::skew_symmetric3(vr,skewR);
874 
875  CArrayDouble<3> skewR_t;
876  skewR.multiply_Ab(t,skewR_t);
877 
878  skewR_t*= -(d*theta-sq)/(8*pow(sq,3));
879  a = skewR_t;
880 
882  skewR2.multiply_AB(skewR,skewR);
883 
884  CArrayDouble<3> skewR2_t;
885  skewR2.multiply_Ab(t,skewR2_t);
886  skewR2_t*= (((theta*sq-d*theta2)*(0.5*theta*cot-1))-theta*sq*((0.25*theta*cot)+0.125*theta2*csc2-1))/(4*theta2*square(oned2));
887  a += skewR2_t;
888 
890  B *=-0.5*theta/(2*sq);
891 
892  B += -(theta*cot-2)/(8*oned2) * ddeltaRt_dR(P);
893  }
894  M3x9(a,B,J);
895  }
896  }
897 }
898 
900 {
901  J.zeros();
902  // Jacobian structure 6x12:
903  // (3rows, for t) [ d_Vinvt_dR (3x9) | Vinv (3x3) ]
904  // [ -------------------------+------------- ]
905  // (3rows, for \omega) [ d_lnR_dR (3x9) | 0 (3x3) ]
906  //
907  // derivs wrt: R_col1 R_col2 R_col3 | t
908  //
909  // (Will be explained better in: http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty )
910  //
911  {
913  ln_rot_jacob(m_ROT, M);
914  J.insertMatrix(3,0, M);
915  }
916  {
918  dVinvt_dR(*this,M);
919  J.insertMatrix(0,0, M);
920  }
921 
922  const CMatrixDouble33 & R = m_ROT;
923  CArrayDouble<3> omega;
925 
927  V_inv.unit(3,1.0); // Start with the identity_3
928 
929  const double d = 0.5*( R(0,0)+R(1,1)+R(2,2)-1);
930  if (d>0.99999)
931  {
932  mrpt::poses::deltaR(R,omega);
933  omega*=0.5;
934  mrpt::math::skew_symmetric3(omega,Omega);
936  Omega2.multiply_AAt(Omega);
937  Omega2*=1.0/12.0;
938 
939  Omega*=0.5;
940 
941  V_inv -= Omega;
942  V_inv -= Omega2;
943  }
944  else
945  {
946  mrpt::poses::deltaR(R,omega);
947 
948  const double theta = acos(d);
949  omega *=theta/(2*std::sqrt(1-d*d));
950 
951  mrpt::math::skew_symmetric3(omega,Omega);
952 
954  Omega2.multiply_AAt(Omega);
955 
956  Omega2 *= (1-theta/(2*std::tan(theta*0.5)))/square(theta);
957  Omega *= 0.5;
958 
959  V_inv -= Omega;
960  V_inv += Omega2;
961  }
962  J.insertMatrix(0,9, V_inv);
963 }
964 
966 {
967  const double d = 0.5*(R(0,0)+R(1,1)+R(2,2)-1);
970  if(d>0.99999)
971  {
972  a[0]=a[1]=a[2]=0;
973  B.unit(3,-0.5);
974  }
975  else
976  {
977  const double theta = acos(d);
978  const double d2 = square(d);
979  const double sq = std::sqrt(1-d2);
980  deltaR(R,a);
981  a *= (d*theta-sq)/(4*(sq*sq*sq));
982  B.unit(3, -theta/(2*sq) );
983  }
984  M3x9(a,B, M);
985 }
986 
987 // Eq. 10.3.5 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
988 void CPose3D::jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix<double,12,6> & jacob)
989 {
990  jacob.block<9,3>(0,0).setZero();
991  jacob.block<3,3>(9,0).setIdentity();
992  for (int i=0;i<3;i++) {
993  Eigen::Block< Eigen::Matrix<double,12,6>,3,3,false> trg_blc = jacob.block<3,3>(3*i,3);
994  mrpt::math::skew_symmetric3_neg( D.m_ROT.block<3,1>(0,i), trg_blc );
995  }
996  {
997  Eigen::Block< Eigen::Matrix<double,12,6>,3,3,false> trg_blc = jacob.block<3,3>(9,3);
999  }
1000 }
1001 
1002 // Eq. 10.3.7 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
1003 void CPose3D::jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix<double,12,6> & jacob)
1004 {
1005  jacob.block<9,3>(0,0).setZero();
1006  jacob.block<3,3>(9,0) = A.getRotationMatrix();
1007  Eigen::Matrix<double,3,3> aux;
1008  for (int i=0;i<3;i++) {
1009  mrpt::math::skew_symmetric3_neg( D.getRotationMatrix().block<3,1>(0,i),aux );
1010  jacob.block<3,3>(3*i,3) = A.m_ROT * aux;
1011  }
1013  jacob.block<3,3>(9,3) = A.m_ROT * aux;
1014 }
1015 
1017 {
1018  for (int i=0;i<3;i++)
1019  for (int j=0;j<3;j++)
1020  m_ROT(i,j) = std::numeric_limits<double>::quiet_NaN();
1021 
1022  for (int i=0;i<3;i++)
1023  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
1024 }
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CPose3D.cpp:162
#define local
Definition: zutil.h:47
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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:645
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
GLdouble GLdouble z
Definition: glext.h:3734
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:787
mxArray * convertToMatlab(const Eigen::EigenBase< Derived > &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:38
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:657
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It&#39;s not safe to set t...
Definition: CPose3D.h:83
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:633
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:85
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
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:781
This file implements several operations that operate element-wise on individual or pairs of container...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
double m_roll
These variables are updated every time that the object rotation matrix is modified (construction...
Definition: CPose3D.h:86
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:35
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44
Definition: eigen_frwds.h:49
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
Definition: CPose3D.h:190
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
Definition: CPose3D.cpp:277
#define M_PI
Definition: bits.h:78
double z
X,Y,Z coordinates.
double yaw
Yaw coordinate (rotation angle over Z axis).
GLdouble s
Definition: glext.h:3602
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
Definition: CPose3D.h:92
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:618
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:181
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
Definition: CPose3D.cpp:410
#define ASSERTDEBMSG_(f, __ERROR_MSG)
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:988
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:682
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void dVinvt_dR(const CPose3D &P, CMatrixFixedNumeric< double, 3, 9 > &J)
Definition: CPose3D.cpp:846
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:81
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:595
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
Definition: CPose3D.cpp:1016
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:138
CPose2D BASE_IMPEXP 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:307
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Definition: CPose3D.cpp:394
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:365
void M3x9(const VEC3 &a, const MAT3x3 &B, MAT3x9 &RES)
Definition: CPose3D.cpp:813
GLubyte GLubyte b
Definition: glext.h:5575
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
int version
Definition: mrpt_jpeglib.h:898
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
void deltaR(const MAT33 &R, VEC3 &v)
Definition: CPose3D.cpp:805
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
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:306
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:321
#define RAD2DEG
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:899
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CMatrixDouble33 ddeltaRt_dR(const CPose3D &P)
Definition: CPose3D.cpp:826
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:671
#define ASSERT_ABOVEEQ_(__A, __B)
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
size_t size(const MATRIXLIKE &m, const int dim)
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
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:79
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:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:965
This file implements matrix/vector text and binary serialization.
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:48
CPose3D()
Default constructor, with all the coordinates set to zero.
Definition: CPose3D.cpp:55
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:254
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
GLenum GLint GLint y
Definition: glext.h:3516
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
Definition: CPose3D.cpp:245
mrpt::math::CArrayDouble< 6 > ln() const
Definition: CPose3D.h:477
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:41
GLenum GLint x
Definition: glext.h:3516
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
Lightweight 3D point.
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x6 vector with [x y z yaw pitch roll].
Definition: CPose3D.cpp:519
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
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:757
GLfloat GLfloat p
Definition: glext.h:5587
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CPose3D.cpp:146
double z
X,Y,Z, coords.
#define MRPT_ALIGN16
void getAsQuaternion(mrpt::math::CQuaternionDouble &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) ...
Definition: CPose3D.cpp:553
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:1003
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:17
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:106



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019