Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DRotVec.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/poses/CPose3D.h>
13 #include <mrpt/poses/CPoint2D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
16 #include <mrpt/poses/CPoint3D.h>
17 #include <mrpt/math/geometry.h> // skew_symmetric3()
18 #include <mrpt/utils/CStream.h>
19 #include <iomanip>
20 #include <limits>
21 
22 
23 using namespace mrpt;
24 using namespace mrpt::math;
25 using namespace mrpt::utils;
26 using namespace mrpt::poses;
27 
29 
30 MRPT_TODO("Complete missing methods")
31 
32 /*---------------------------------------------------------------
33  Constructors
34  ---------------------------------------------------------------*/
36 {
37  m_coords[0] = m(0,3);
38  m_coords[1] = m(1,3);
39  m_coords[2] = m(2,3);
40 
41  m_rotvec = rotVecFromRotMat( m );
42 }
43 
45 {
46  m_coords[0] = m.x();
47  m_coords[1] = m.y();
48  m_coords[2] = m.z();
51  m_rotvec = rotVecFromRotMat( R );
52 }
53 
54 /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
55 CPose3DRotVec::CPose3DRotVec(const mrpt::math::CQuaternionDouble &q, const double _x, const double _y, const double _z )
56 {
57  m_coords[0]=_x; m_coords[1]=_y; m_coords[2]=_z;
58  const double a = sqrt( q.x()*q.x() + q.y()*q.y() + q.z()*q.z() );
59  const double TH = 0.001;
60  const double k = a < TH ? 2 : 2*acos(q.r())/sqrt(1-q.r()*q.r());
61  m_rotvec[0] = k*q.x();
62  m_rotvec[1] = k*q.y();
63  m_rotvec[2] = k*q.z();
64 }
65 
66 /*---------------------------------------------------------------
67  Implements the writing to a CStream capability of
68  CSerializable objects
69  ---------------------------------------------------------------*/
71 {
72  if (version)
73  *version = 0;
74  else
75  {
76  out << m_coords[0] << m_coords[1] << m_coords[2] << m_rotvec[0] << m_rotvec[1] << m_rotvec[2];
77  }
78 }
79 
80 /*---------------------------------------------------------------
81  Implements the reading from a CStream capability of
82  CSerializable objects
83  ---------------------------------------------------------------*/
85 {
86  switch(version)
87  {
88  case 0:
89  {
90  in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_rotvec[0] >> m_rotvec[1] >> m_rotvec[2];
91  } break;
92  default:
94  };
95 }
96 
97 void CPose3DRotVec::setFromXYZAndAngles( const double x,const double y,const double z,const double yaw, const double pitch, const double roll)
98 {
99  CPose3D aux(x,y,z,yaw,pitch,roll);
100  this->m_coords[0] = aux.m_coords[0];
101  this->m_coords[1] = aux.m_coords[1];
102  this->m_coords[2] = aux.m_coords[2];
103  this->m_rotvec = aux.ln_rotation();
104 }
105 
107 {
108  // go through cpose3d
109  CArrayDouble<3> out;
110  CPose3D aux(m);
111  out = aux.ln_rotation();
112  return out;
113 } // end-rotVecFromRotMat
114 
115 /** Textual output stream function.
116  */
117 std::ostream& mrpt::poses::operator << (std::ostream& o, const CPose3DRotVec& p)
118 {
119  const std::streamsize old_pre = o.precision();
120  const std::ios_base::fmtflags old_flags = o.flags();
121  o << "(x,y,z,vx,vy,vz)=(" << std::fixed << std::setprecision(4) << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
122  << p.m_rotvec[0] << "," << p.m_rotvec[1] << "," << p.m_rotvec[2] << ")";
123  o.flags(old_flags);
124  o.precision(old_pre);
125  return o;
126 }
127 
128 /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
130 {
131  // through cpose3D
132  ROT = CPose3D().exp_rotation( this->m_rotvec );
133 // cout << "CPOSE_3D: " << ROT;
134 }
135 
136 /*---------------------------------------------------------------
137  sphericalCoordinates
138 ---------------------------------------------------------------*/
140  const TPoint3D &point,
141  double &out_range,
142  double &out_yaw,
143  double &out_pitch ) const
144 {
145  // Pass to coordinates as seen from this 6D pose:
146  TPoint3D local;
147  this->inverseComposePoint(point.x,point.y,point.z, local.x,local.y,local.z);
148 
149  // Range:
150  out_range = local.norm();
151 
152  // Yaw:
153  if (local.y!=0 || local.x!=0)
154  out_yaw = atan2(local.y,local.x);
155  else out_yaw = 0;
156 
157  // Pitch:
158  if (out_range!=0)
159  out_pitch = -asin( local.z / out_range );
160  else out_pitch = 0;
161 }
162 
163 /*---------------------------------------------------------------
164  composePoint
165 ---------------------------------------------------------------*/
166 void CPose3DRotVec::composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
167  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
168  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose) const
169 {
170  const double angle = this->m_rotvec.norm();
171  const double K1 = sin(angle)/angle;
172  const double K2 = (1-cos(angle))/(angle*angle);
173 
174  const double tx = this->m_coords[0];
175  const double ty = this->m_coords[1];
176  const double tz = this->m_coords[2];
177 
178  const double w1 = this->m_rotvec[0];
179  const double w2 = this->m_rotvec[1];
180  const double w3 = this->m_rotvec[2];
181 
182  const double w1_2 = w1*w1;
183  const double w2_2 = w2*w2;
184  const double w3_2 = w3*w3;
185 
186  gx = lx*(1-K2*(w2_2+w3_2)) + ly*(K2*w1*w2-K1*w3) + lz*(K1*w2+K2*w1*w3) + tx;
187  gy = lx*(K1*w3+K2*w1*w2) + ly*(1-K2*(w1_2+w3_2)) + lz*(K2*w2*w3-K1*w1) + ty;
188  gz = lx*(K2*w1*w3-K1*w2) + ly*(K1*w1+K2*w2*w3) + lz*(1-K2*(w1_2+w2_2)) + tz;
189 
190  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
191  {
192  MRPT_TODO("Jacobians")
193  THROW_EXCEPTION("Jacobians not implemented yet")
194  }
195 }
196 
197 /*---------------------------------------------------------------
198  unary -
199 ---------------------------------------------------------------*/
201 {
203  b.getInverseHomogeneousMatrix(B_INV);
204  return CPose3DRotVec(B_INV);
205 }
206 
208 {
209  return (p1.m_coords==p2.m_coords)&&(p1.m_rotvec==p2.m_rotvec);
210 }
211 
213 {
214  return (p1.m_coords!=p2.m_coords)||(p1.m_rotvec!=p2.m_rotvec);
215 }
216 
217 /*---------------------------------------------------------------
218  point3D = pose3D + point3D
219  ---------------------------------------------------------------*/
221 {
222  CPoint3D outPoint;
223 
224  this->composePoint(b.m_coords[0], b.m_coords[1], b.m_coords[2],
225  outPoint.m_coords[0],
226  outPoint.m_coords[1],
227  outPoint.m_coords[2]);
228 
229  return outPoint;
230 }
231 
232 /*---------------------------------------------------------------
233  point3D = pose3D + point2D
234  ---------------------------------------------------------------*/
236 {
237  CPoint3D outPoint;
238 
239  this->composePoint( b.m_coords[0],
240  b.m_coords[1],
241  0,
242  outPoint.m_coords[0],
243  outPoint.m_coords[1],
244  outPoint.m_coords[2] );
245 
246  return outPoint;
247 
248 }
249 
251 {
252  q.m_coords[0] = this->m_coords[0];
253  q.m_coords[1] = this->m_coords[1];
254  q.m_coords[2] = this->m_coords[2];
255 
256  const double a = sqrt( this->m_rotvec[0]*this->m_rotvec[0]+this->m_rotvec[1]*this->m_rotvec[1]+this->m_rotvec[2]*this->m_rotvec[2] );
257  if(a < 0.001)
258  {
259  q.m_quat.r(1);
260  q.m_quat.x(0.5*this->m_rotvec[0]);
261  q.m_quat.y(0.5*this->m_rotvec[1]);
262  q.m_quat.z(0.5*this->m_rotvec[2]);
263  // TO DO: output of the jacobian
264  // df_dr = 0.25*[-r';2*eye(3)];
265  }
266  else
267  {
268  q.m_quat.fromRodriguesVector( this->m_rotvec );
269 
270  // TO DO: output of the jacobian
271 // a2 = a*a;
272 // a3 = a2*a;
273 //
274 // r1 = r(1);
275 // r2 = r(2);
276 // r3 = r(3);
277 // s = sin(a/2);
278 // c = cos(a/2);
279 //
280 // A = a*c-2*s;
281 //
282 // df_dr = 1/a3*[-r1*a2*s -r2*a2*s -r3*a2*s; ...
283 // 2*a2*s+r1*r1*A r1*r2*A r1*r3*A; ...
284 // r1*r2*A 2*a2*s+r2*r2*A r2*r3*A; ...
285 // r1*r3*A r2*r3*A 2*a2*s+r3*r3*A]; % jacobian of transf.
286 //
287  }
288 }
289 
290 /*---------------------------------------------------------------
291  this = A + B
292  ---------------------------------------------------------------*/
294  const CPose3DRotVec& B,
295  mrpt::math::CMatrixFixedNumeric<double,6,6> *out_jacobian_drvtC_drvtA,
296  mrpt::math::CMatrixFixedNumeric<double,6,6> *out_jacobian_drvtC_drvtB)
297 
298 {
299  const double a1 = A.m_rotvec.norm(); // angles
300  const double a2 = B.m_rotvec.norm();
301 
302  // if the angles are small, we can compute the approximate composition easily
303  if( a1 < 0.01 && a2 < 0.01 )
304  {
305  const CMatrixDouble33 Ra = Eigen::MatrixXd::Identity(3,3) + mrpt::math::skew_symmetric3(A.m_rotvec);
306 
307  this->m_rotvec = A.m_rotvec + B.m_rotvec;
308  this->m_coords = A.m_coords + Ra*B.m_coords;
309 
310  if( out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB )
311  {
312  if( out_jacobian_drvtC_drvtA ) // jacobian wrt A
313  {
314  out_jacobian_drvtC_drvtA->setIdentity(6,6);
315  out_jacobian_drvtC_drvtA->insertMatrix(3,0,mrpt::math::skew_symmetric3_neg(B.m_coords));
316  }
317 
318  if( out_jacobian_drvtC_drvtB ) // jacobian wrt B
319  {
320  out_jacobian_drvtC_drvtB->setIdentity(6,6);
321  out_jacobian_drvtC_drvtB->insertMatrix(3,3,mrpt::math::skew_symmetric3(A.m_rotvec));
322  (*out_jacobian_drvtC_drvtB)(3,3) =
323  (*out_jacobian_drvtC_drvtB)(4,4) =
324  (*out_jacobian_drvtC_drvtB)(5,5) = 1;
325  }
326  }
327  return;
328  }
329 
330  CMatrixDouble33 RA,RB;
331  A.getRotationMatrix(RA);
332  B.getRotationMatrix(RB);
333 
334  // Translation part
336  this->m_coords[0] = coords[0];
337  this->m_coords[1] = coords[1];
338  this->m_coords[2] = coords[2];
339 
340  // Rotation part:
341 #if 0
342  else if (A_is_small) this->m_rotvec = B.m_rotvec;
343  else if (B_is_small) this->m_rotvec = A.m_rotvec;
344  else
345  {
346  const double a1_inv = 1/a1;
347  const double a2_inv = 1/a2;
348 
349  const double sin_a1_2 = sin(0.5*a1);
350  const double cos_a1_2 = cos(0.5*a1);
351  const double sin_a2_2 = sin(0.5*a2);
352  const double cos_a2_2 = cos(0.5*a2);
353 
354  const double KA = sin_a1_2*sin_a2_2;
355  const double KB = sin_a1_2*cos_a2_2;
356  const double KC = cos_a1_2*sin_a2_2;
357  const double KD = cos_a1_2*cos_a2_2;
358 
359  const double r11 = a1_inv*A.m_rotvec[0];
360  const double r12 = a1_inv*A.m_rotvec[1];
361  const double r13 = a1_inv*A.m_rotvec[2];
362 
363  const double r21 = a2_inv*B.m_rotvec[0];
364  const double r22 = a2_inv*B.m_rotvec[1];
365  const double r23 = a2_inv*B.m_rotvec[2];
366 
367  const double q3[] = {
368  KD - KA*(r11*r21+r12*r22+r13*r23),
369  KC*r21 + KB*r11 + KA*(r22*r13-r23*r12),
370  KC*r22 + KB*r12 + KA*(r23*r11-r21*r13),
371  KC*r23 + KB*r13 + KA*(r21*r12-r22*r11)
372  };
373 
374  const double param = 2*acos(q3[0])/sqrt(1-q3[0]*q3[0]);
375  this->m_rotvec[0] = param*q3[1];
376  this->m_rotvec[1] = param*q3[2];
377  this->m_rotvec[2] = param*q3[3];
378  }
379 #endif
380 
381  /* */
382 
383  // Rotation part
385  aux.setRotationMatrix( RA*RB );
386  this->m_rotvec = aux.ln_rotation();
387 
388 // cout << "WO Approx: " << *this << endl;
389 
390  /* */
391 
392  // Output Jacobians (if desired)
393  if( out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB )
394  {
395  CPose3DQuat qA,qB,qC;
396  this->toQuatXYZ(qC);
397 
398  const double &qCr = qC.m_quat[0];
399  const double &qCx = qC.m_quat[1];
400  const double &qCy = qC.m_quat[2];
401  const double &qCz = qC.m_quat[3];
402 
403  const double &r1 = this->m_rotvec[0];
404  const double &r2 = this->m_rotvec[1];
405  const double &r3 = this->m_rotvec[2];
406 
407  const double C = 1/(1-qCr*qCr);
408  const double D = acos(qCr)/sqrt(1-qCr*qCr);
409 
410  MRPT_ALIGN16 const double aux_valsH[] = {
411  2*C*qCx*(D*qCr-1), 2*D, 0, 0,
412  2*C*qCy*(D*qCr-1), 0, 2*D, 0,
413  2*C*qCz*(D*qCr-1), 0, 0, 2*D
414  };
415 
416  CMatrixFixedNumeric<double,3,4> H(aux_valsH);
417 
418  const double alpha = sqrt(r1*r1+r2*r2+r3*r3);
419  const double alpha2 = alpha*alpha;
420  const double KA = alpha*cos(alpha/2)-2*sin(alpha/2);
421 
422  MRPT_ALIGN16 const double aux_valsG[] = {
423  -r1*alpha2*sin(alpha/2), -r2*alpha2*sin(alpha/2), -r3*alpha2*sin(alpha/2),
424  2*alpha2*sin(alpha/2)+r1*r1*KA, r1*r2*KA, r1*r3*KA,
425  r1*r2*KA, 2*alpha2*sin(alpha/2)+r2*r2*KA, r2*r3*KA,
426  r1*r3*KA, r2*r3*KA, 2*alpha2*sin(alpha/2)+r3*r3*KA
427  };
428 
429  CMatrixFixedNumeric<double,4,3> G(aux_valsG);
430 
431  if( out_jacobian_drvtC_drvtB )
432  {
433  A.toQuatXYZ(qA);
434 
435  const double &qAr = qA.m_quat[0];
436  const double &qAx = qA.m_quat[1];
437  const double &qAy = qA.m_quat[2];
438  const double &qAz = qA.m_quat[3];
439 
440  MRPT_ALIGN16 const double aux_valsQA[] = {qAr, -qAx, -qAy, -qAz, qAx, qAr, qAz, -qAy, qAy, -qAz, qAr, qAx, qAz, qAy, -qAx, qAr};
441  CMatrixDouble44 QA(aux_valsQA);
442 
443  CMatrixDouble33 jac_rot_B;
444  jac_rot_B.multiply_ABC(H,QA,G);
445 
446  out_jacobian_drvtC_drvtB->fill(0);
447  out_jacobian_drvtC_drvtB->insertMatrix(0,0,jac_rot_B);
448  out_jacobian_drvtC_drvtB->insertMatrix(3,3,RA);
449  }
450  if( out_jacobian_drvtC_drvtA )
451  {
452  B.toQuatXYZ(qB);
453 
454  const double &qBr = qB.m_quat[0];
455  const double &qBx = qB.m_quat[1];
456  const double &qBy = qB.m_quat[2];
457  const double &qBz = qB.m_quat[3];
458 
459  MRPT_ALIGN16 const double aux_valsQB[] = {qBr, -qBx, -qBy, -qBz, qBx, qBr, -qBz, qBy, qBy, qBz, qBr, -qBx, qBz, -qBy, qBx, qBr};
460  CMatrixDouble44 QB(aux_valsQB);
461 
462  CMatrixDouble33 jac_rot_A, id3;
463  jac_rot_A.multiply_ABC(H,QB,G);
464 
465  id3.eye();
466  out_jacobian_drvtC_drvtA->fill(0);
467  out_jacobian_drvtC_drvtA->insertMatrix(0,0,jac_rot_A);
468  out_jacobian_drvtC_drvtB->insertMatrix(3,3,id3);
469  }
470  }
471 } // end composeFrom
472 
473 /** Convert this pose into its inverse, saving the result in itself. */
475 {
477  this->getInverseHomogeneousMatrix(B_INV);
478  this->setFromTransformationMatrix(B_INV);
479 }
480 
481 /** Compute the inverse of this pose and return the result. */
483 {
486  this->getInverseHomogeneousMatrix(B_INV);
487  inv_rvt.setFromTransformationMatrix(B_INV);
488 
489  return inv_rvt;
490 }
491 
492 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
493  * \note A or B can be "this" without problems.
494  * \sa composeFrom, composePoint
495  */
497 {
498  // this = A (-) B
499  // HM_this = inv(HM_B) * HM_A
500  //
501  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
502  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
503  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
504  //
506  HM_B_inv(UNINITIALIZED_MATRIX),
507  HM_C(UNINITIALIZED_MATRIX);
508 
509  A.getHomogeneousMatrix(HM_A);
510  B.getInverseHomogeneousMatrix(HM_B_inv);
511 
512  HM_C.multiply_AB(HM_B_inv,HM_A);
513 
514  this->m_rotvec = this->rotVecFromRotMat(HM_C);
515 
516  this->m_coords[0] = HM_C(0,3);
517  this->m_coords[1] = HM_C(1,3);
518  this->m_coords[2] = HM_C(2,3);
519 }
520 
521 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
522  * \sa composePoint, composeFrom
523  */
524 void CPose3DRotVec::inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
525  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
526  mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose ) const
527 {
528  MRPT_UNUSED_PARAM(out_jacobian_df_dpoint); MRPT_UNUSED_PARAM(out_jacobian_df_dpose);
529  CPose3DRotVec rvt = this->getInverse();
530  rvt.composePoint( gx, gy, gz, lx, ly, lz );
531  MRPT_TODO("Jacobians");
532 }
533 
534 /** Exponentiate a Vector in the SE3 Lie Algebra to generate a new CPose3DRotVec.
535  */
537 {
538  return CPose3DRotVec(mu);
539 }
540 
541 /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra.
542  */
544 {
545  return m_rotvec;
546 }
547 
548 
549 /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE3 Lie Algebra.
550  */
552 {
553  result.head<3>() = m_coords;
554  result.tail<3>() = m_rotvec;
555 }
556 
558 {
559  for (int i=0;i<3;i++) {
560  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
561  m_rotvec[i] = std::numeric_limits<double>::quiet_NaN();
562  }
563 }
static CPose3DRotVec exp(const mrpt::math::CArrayDouble< 6 > &vect)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3DRotVec (static method)...
#define local
Definition: zutil.h:47
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
CPose3DRotVec operator+(const CPose3DRotVec &b) const
The operator is the pose compounding operator.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
const GLshort * coords
Definition: glext.h:6387
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
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) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
void inverse()
Convert this pose into its inverse, saving the result in itself.
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
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPose3DRotVec getInverse() const
Compute the inverse of this pose and return the result.
mrpt::math::CArrayDouble< 3 > m_coords
[x,y,z]
Definition: CPoint3D.h:38
#define THROW_EXCEPTION(msg)
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
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void setFromTransformationMatrix(const mrpt::math::CMatrixDouble44 &m)
double z
X,Y,Z coordinates.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:181
const mrpt::math::CMatrixDouble33 getRotationMatrix() const
#define MRPT_TODO(x)
void ln(mrpt::math::CArrayDouble< 6 > &out_ln) const
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the...
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
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 composeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtA=NULL, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtB=NULL)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
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
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
#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
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:201
GLbyte ty
Definition: glext.h:5441
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
mrpt::math::CArrayDouble< 3 > rotVecFromRotMat(const math::CMatrixDouble44 &m)
Create a vector with 3 components according to the input transformation matrix (only the rotation wil...
GLubyte GLubyte b
Definition: glext.h:5575
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:4993
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix part of this pose, generating the corresponding vector ...
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:48
void toQuatXYZ(CPose3DQuat &q) const
Convert this RVT into a quaternion + XYZ.
GLuint in
Definition: glext.h:6301
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) const
Computes the 3D point L such as .
void inverseComposeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
GLbyte GLbyte tz
Definition: glext.h:5441
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:48
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
Lightweight 3D point.
GLfloat param
Definition: glext.h:3705
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
CPose3DRotVec()
Default constructor, with all the coordinates set to zero.
Definition: CPose3DRotVec.h:54
#define MRPT_ALIGN16
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:166
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
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:4993
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DRotVec.h:47



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