Main MRPT website > C++ reference for MRPT 1.9.9
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-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/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()
19 #include <iomanip>
20 #include <limits>
21 
22 using namespace mrpt;
23 using namespace mrpt::math;
24 
25 using namespace mrpt::poses;
26 
28 
29 MRPT_TODO("Complete missing methods")
30 
31 /*---------------------------------------------------------------
32  Constructors
33  ---------------------------------------------------------------*/
35 {
36  m_coords[0] = m(0, 3);
37  m_coords[1] = m(1, 3);
38  m_coords[2] = m(2, 3);
39 
40  m_rotvec = rotVecFromRotMat(m);
41 }
42 
43 CPose3DRotVec::CPose3DRotVec(const CPose3D& m)
44 {
45  m_coords[0] = m.x();
46  m_coords[1] = m.y();
47  m_coords[2] = m.z();
50  m_rotvec = rotVecFromRotMat(R);
51 }
52 
53 /** Constructor from a quaternion (which only represents the 3D rotation part)
54  * and a 3D displacement. */
55 CPose3DRotVec::CPose3DRotVec(
56  const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
57  const double _z)
58 {
59  m_coords[0] = _x;
60  m_coords[1] = _y;
61  m_coords[2] = _z;
62  const double a = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
63  const double TH = 0.001;
64  const double k = a < TH ? 2 : 2 * acos(q.r()) / sqrt(1 - q.r() * q.r());
65  m_rotvec[0] = k * q.x();
66  m_rotvec[1] = k * q.y();
67  m_rotvec[2] = k * q.z();
68 }
69 
70 uint8_t CPose3DRotVec::serializeGetVersion() const { return 0; }
71 void CPose3DRotVec::serializeTo(mrpt::serialization::CArchive& out) const
72 {
73  out << m_coords[0] << m_coords[1] << m_coords[2] << m_rotvec[0]
74  << m_rotvec[1] << m_rotvec[2];
75 }
76 void CPose3DRotVec::serializeFrom(
78 {
79  switch (version)
80  {
81  case 0:
82  {
83  in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_rotvec[0] >>
84  m_rotvec[1] >> m_rotvec[2];
85  }
86  break;
87  default:
89  };
90 }
91 
92 void CPose3DRotVec::setFromXYZAndAngles(
93  const double x, const double y, const double z, const double yaw,
94  const double pitch, const double roll)
95 {
96  CPose3D aux(x, y, z, yaw, pitch, roll);
97  this->m_coords[0] = aux.m_coords[0];
98  this->m_coords[1] = aux.m_coords[1];
99  this->m_coords[2] = aux.m_coords[2];
100  this->m_rotvec = aux.ln_rotation();
101 }
102 
103 CArrayDouble<3> CPose3DRotVec::rotVecFromRotMat(const math::CMatrixDouble44& m)
104 {
105  // go through cpose3d
106  CArrayDouble<3> out;
107  CPose3D aux(m);
108  out = aux.ln_rotation();
109  return out;
110 } // end-rotVecFromRotMat
111 
112 /** Textual output stream function.
113  */
114 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DRotVec& p)
115 {
116  const std::streamsize old_pre = o.precision();
117  const std::ios_base::fmtflags old_flags = o.flags();
118  o << "(x,y,z,vx,vy,vz)=(" << std::fixed << std::setprecision(4)
119  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
120  << p.m_rotvec[0] << "," << p.m_rotvec[1] << "," << p.m_rotvec[2] << ")";
121  o.flags(old_flags);
122  o.precision(old_pre);
123  return o;
124 }
125 
126 /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix */
127 void CPose3DRotVec::getRotationMatrix(mrpt::math::CMatrixDouble33& ROT) const
128 {
129  // through cpose3D
130  ROT = CPose3D().exp_rotation(this->m_rotvec);
131  // cout << "CPOSE_3D: " << ROT;
132 }
133 
134 /*---------------------------------------------------------------
135  sphericalCoordinates
136 ---------------------------------------------------------------*/
137 void CPose3DRotVec::sphericalCoordinates(
138  const TPoint3D& point, double& out_range, double& out_yaw,
139  double& out_pitch) const
140 {
141  // Pass to coordinates as seen from this 6D pose:
142  TPoint3D local;
143  this->inverseComposePoint(
144  point.x, point.y, point.z, local.x, local.y, local.z);
145 
146  // Range:
147  out_range = local.norm();
148 
149  // Yaw:
150  if (local.y != 0 || local.x != 0)
151  out_yaw = atan2(local.y, local.x);
152  else
153  out_yaw = 0;
154 
155  // Pitch:
156  if (out_range != 0)
157  out_pitch = -asin(local.z / out_range);
158  else
159  out_pitch = 0;
160 }
161 
162 /*---------------------------------------------------------------
163  composePoint
164 ---------------------------------------------------------------*/
165 void CPose3DRotVec::composePoint(
166  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) +
187  lz * (K1 * w2 + K2 * w1 * w3) + tx;
188  gy = lx * (K1 * w3 + K2 * w1 * w2) + ly * (1 - K2 * (w1_2 + w3_2)) +
189  lz * (K2 * w2 * w3 - K1 * w1) + ty;
190  gz = lx * (K2 * w1 * w3 - K1 * w2) + ly * (K1 * w1 + K2 * w2 * w3) +
191  lz * (1 - K2 * (w1_2 + w2_2)) + tz;
192 
193  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
194  {
195  MRPT_TODO("Jacobians")
196  THROW_EXCEPTION("Jacobians not implemented yet");
197  }
198 }
199 
200 /*---------------------------------------------------------------
201  unary -
202 ---------------------------------------------------------------*/
204 {
206  b.getInverseHomogeneousMatrix(B_INV);
207  return CPose3DRotVec(B_INV);
208 }
209 
211 {
212  return (p1.m_coords == p2.m_coords) && (p1.m_rotvec == p2.m_rotvec);
213 }
214 
216 {
217  return (p1.m_coords != p2.m_coords) || (p1.m_rotvec != p2.m_rotvec);
218 }
219 
220 /*---------------------------------------------------------------
221  point3D = pose3D + point3D
222  ---------------------------------------------------------------*/
224 {
225  CPoint3D outPoint;
226 
227  this->composePoint(
228  b.m_coords[0], b.m_coords[1], b.m_coords[2], outPoint.m_coords[0],
229  outPoint.m_coords[1], outPoint.m_coords[2]);
230 
231  return outPoint;
232 }
233 
234 /*---------------------------------------------------------------
235  point3D = pose3D + point2D
236  ---------------------------------------------------------------*/
238 {
239  CPoint3D outPoint;
240 
241  this->composePoint(
242  b.m_coords[0], b.m_coords[1], 0, outPoint.m_coords[0],
243  outPoint.m_coords[1], outPoint.m_coords[2]);
244 
245  return outPoint;
246 }
247 
248 void CPose3DRotVec::toQuatXYZ(CPose3DQuat& q) const
249 {
250  q.m_coords[0] = this->m_coords[0];
251  q.m_coords[1] = this->m_coords[1];
252  q.m_coords[2] = this->m_coords[2];
253 
254  const double a = sqrt(
255  this->m_rotvec[0] * this->m_rotvec[0] +
256  this->m_rotvec[1] * this->m_rotvec[1] +
257  this->m_rotvec[2] * this->m_rotvec[2]);
258  if (a < 0.001)
259  {
260  q.m_quat.r(1);
261  q.m_quat.x(0.5 * this->m_rotvec[0]);
262  q.m_quat.y(0.5 * this->m_rotvec[1]);
263  q.m_quat.z(0.5 * this->m_rotvec[2]);
264  // TO DO: output of the jacobian
265  // df_dr = 0.25*[-r';2*eye(3)];
266  }
267  else
268  {
269  q.m_quat.fromRodriguesVector(this->m_rotvec);
270 
271  // TO DO: output of the jacobian
272  // a2 = a*a;
273  // a3 = a2*a;
274  //
275  // r1 = r(1);
276  // r2 = r(2);
277  // r3 = r(3);
278  // s = sin(a/2);
279  // c = cos(a/2);
280  //
281  // A = a*c-2*s;
282  //
283  // df_dr = 1/a3*[-r1*a2*s -r2*a2*s -r3*a2*s; ...
284  // 2*a2*s+r1*r1*A r1*r2*A r1*r3*A; ...
285  // r1*r2*A 2*a2*s+r2*r2*A r2*r3*A; ...
286  // r1*r3*A r2*r3*A 2*a2*s+r3*r3*A]; % jacobian of
287  // transf.
288  //
289  }
290 }
291 
292 /*---------------------------------------------------------------
293  this = A + B
294  ---------------------------------------------------------------*/
295 void CPose3DRotVec::composeFrom(
296  const CPose3DRotVec& A, const CPose3DRotVec& B,
297  mrpt::math::CMatrixFixedNumeric<double, 6, 6>* out_jacobian_drvtC_drvtA,
298  mrpt::math::CMatrixFixedNumeric<double, 6, 6>* out_jacobian_drvtC_drvtB)
299 
300 {
301  const double a1 = A.m_rotvec.norm(); // angles
302  const double a2 = B.m_rotvec.norm();
303 
304  // if the angles are small, we can compute the approximate composition
305  // easily
306  if (a1 < 0.01 && a2 < 0.01)
307  {
308  const CMatrixDouble33 Ra = Eigen::MatrixXd::Identity(3, 3) +
310 
311  this->m_rotvec = A.m_rotvec + B.m_rotvec;
312  this->m_coords = A.m_coords + Ra * B.m_coords;
313 
314  if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
315  {
316  if (out_jacobian_drvtC_drvtA) // jacobian wrt A
317  {
318  out_jacobian_drvtC_drvtA->setIdentity(6, 6);
319  out_jacobian_drvtC_drvtA->insertMatrix(
321  }
322 
323  if (out_jacobian_drvtC_drvtB) // jacobian wrt B
324  {
325  out_jacobian_drvtC_drvtB->setIdentity(6, 6);
326  out_jacobian_drvtC_drvtB->insertMatrix(
328  (*out_jacobian_drvtC_drvtB)(3, 3) = (*out_jacobian_drvtC_drvtB)(
329  4, 4) = (*out_jacobian_drvtC_drvtB)(5, 5) = 1;
330  }
331  }
332  return;
333  }
334 
335  CMatrixDouble33 RA, RB;
336  A.getRotationMatrix(RA);
337  B.getRotationMatrix(RB);
338 
339  // Translation part
340  const auto coords = RA * B.m_coords + A.m_coords;
341  m_coords = coords;
342 
343 // Rotation part:
344 #if 0
345  else if (A_is_small) this->m_rotvec = B.m_rotvec;
346  else if (B_is_small) this->m_rotvec = A.m_rotvec;
347  else
348  {
349  const double a1_inv = 1/a1;
350  const double a2_inv = 1/a2;
351 
352  const double sin_a1_2 = sin(0.5*a1);
353  const double cos_a1_2 = cos(0.5*a1);
354  const double sin_a2_2 = sin(0.5*a2);
355  const double cos_a2_2 = cos(0.5*a2);
356 
357  const double KA = sin_a1_2*sin_a2_2;
358  const double KB = sin_a1_2*cos_a2_2;
359  const double KC = cos_a1_2*sin_a2_2;
360  const double KD = cos_a1_2*cos_a2_2;
361 
362  const double r11 = a1_inv*A.m_rotvec[0];
363  const double r12 = a1_inv*A.m_rotvec[1];
364  const double r13 = a1_inv*A.m_rotvec[2];
365 
366  const double r21 = a2_inv*B.m_rotvec[0];
367  const double r22 = a2_inv*B.m_rotvec[1];
368  const double r23 = a2_inv*B.m_rotvec[2];
369 
370  const double q3[] = {
371  KD - KA*(r11*r21+r12*r22+r13*r23),
372  KC*r21 + KB*r11 + KA*(r22*r13-r23*r12),
373  KC*r22 + KB*r12 + KA*(r23*r11-r21*r13),
374  KC*r23 + KB*r13 + KA*(r21*r12-r22*r11)
375  };
376 
377  const double param = 2*acos(q3[0])/sqrt(1-q3[0]*q3[0]);
378  this->m_rotvec[0] = param*q3[1];
379  this->m_rotvec[1] = param*q3[2];
380  this->m_rotvec[2] = param*q3[3];
381  }
382 #endif
383 
384  /* */
385 
386  // Rotation part
388  aux.setRotationMatrix(RA * RB);
389  this->m_rotvec = aux.ln_rotation();
390 
391  // cout << "WO Approx: " << *this << endl;
392 
393  /* */
394 
395  // Output Jacobians (if desired)
396  if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
397  {
398  CPose3DQuat qA, qB, qC;
399  this->toQuatXYZ(qC);
400 
401  const double& qCr = qC.m_quat[0];
402  const double& qCx = qC.m_quat[1];
403  const double& qCy = qC.m_quat[2];
404  const double& qCz = qC.m_quat[3];
405 
406  const double& r1 = this->m_rotvec[0];
407  const double& r2 = this->m_rotvec[1];
408  const double& r3 = this->m_rotvec[2];
409 
410  const double C = 1 / (1 - qCr * qCr);
411  const double D = acos(qCr) / sqrt(1 - qCr * qCr);
412 
413  alignas(MRPT_MAX_ALIGN_BYTES) const double aux_valsH[] = {
414  2 * C * qCx * (D * qCr - 1), 2 * D, 0, 0,
415  2 * C * qCy * (D * qCr - 1), 0, 2 * D, 0,
416  2 * C * qCz * (D * qCr - 1), 0, 0, 2 * D};
417 
419 
420  const double alpha = sqrt(r1 * r1 + r2 * r2 + r3 * r3);
421  const double alpha2 = alpha * alpha;
422  const double KA = alpha * cos(alpha / 2) - 2 * sin(alpha / 2);
423 
424  alignas(MRPT_MAX_ALIGN_BYTES) const double aux_valsG[] = {
425  -r1 * alpha2 * sin(alpha / 2),
426  -r2 * alpha2 * sin(alpha / 2),
427  -r3 * alpha2 * sin(alpha / 2),
428  2 * alpha2 * sin(alpha / 2) + r1 * r1 * KA,
429  r1 * r2 * KA,
430  r1 * r3 * KA,
431  r1 * r2 * KA,
432  2 * alpha2 * sin(alpha / 2) + r2 * r2 * KA,
433  r2 * r3 * KA,
434  r1 * r3 * KA,
435  r2 * r3 * KA,
436  2 * alpha2 * sin(alpha / 2) + r3 * r3 * KA};
437 
439 
440  if (out_jacobian_drvtC_drvtB)
441  {
442  A.toQuatXYZ(qA);
443 
444  const double& qAr = qA.m_quat[0];
445  const double& qAx = qA.m_quat[1];
446  const double& qAy = qA.m_quat[2];
447  const double& qAz = qA.m_quat[3];
448 
449  alignas(MRPT_MAX_ALIGN_BYTES) const double aux_valsQA[] = {
450  qAr, -qAx, -qAy, -qAz, qAx, qAr, qAz, -qAy,
451  qAy, -qAz, qAr, qAx, qAz, qAy, -qAx, qAr};
452  CMatrixDouble44 QA(aux_valsQA);
453 
454  CMatrixDouble33 jac_rot_B;
455  jac_rot_B.multiply_ABC(H, QA, G);
456 
457  out_jacobian_drvtC_drvtB->fill(0);
458  out_jacobian_drvtC_drvtB->insertMatrix(0, 0, jac_rot_B);
459  out_jacobian_drvtC_drvtB->insertMatrix(3, 3, RA);
460  }
461  if (out_jacobian_drvtC_drvtA)
462  {
463  B.toQuatXYZ(qB);
464 
465  const double& qBr = qB.m_quat[0];
466  const double& qBx = qB.m_quat[1];
467  const double& qBy = qB.m_quat[2];
468  const double& qBz = qB.m_quat[3];
469 
470  alignas(MRPT_MAX_ALIGN_BYTES) const double aux_valsQB[] = {
471  qBr, -qBx, -qBy, -qBz, qBx, qBr, -qBz, qBy,
472  qBy, qBz, qBr, -qBx, qBz, -qBy, qBx, qBr};
473  CMatrixDouble44 QB(aux_valsQB);
474 
475  CMatrixDouble33 jac_rot_A, id3;
476  jac_rot_A.multiply_ABC(H, QB, G);
477 
478  id3.eye();
479  out_jacobian_drvtC_drvtA->fill(0);
480  out_jacobian_drvtC_drvtA->insertMatrix(0, 0, jac_rot_A);
481  out_jacobian_drvtC_drvtB->insertMatrix(3, 3, id3);
482  }
483  }
484 } // end composeFrom
485 
486 /** Convert this pose into its inverse, saving the result in itself. */
488 {
490  this->getInverseHomogeneousMatrix(B_INV);
491  this->setFromTransformationMatrix(B_INV);
492 }
493 
494 /** Compute the inverse of this pose and return the result. */
495 CPose3DRotVec CPose3DRotVec::getInverse() const
496 {
499  this->getInverseHomogeneousMatrix(B_INV);
500  inv_rvt.setFromTransformationMatrix(B_INV);
501 
502  return inv_rvt;
503 }
504 
505 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
506  * than "this= A - B;" since it avoids the temporary object.
507  * \note A or B can be "this" without problems.
508  * \sa composeFrom, composePoint
509  */
510 void CPose3DRotVec::inverseComposeFrom(
511  const CPose3DRotVec& A, const CPose3DRotVec& B)
512 {
513  // this = A (-) B
514  // HM_this = inv(HM_B) * HM_A
515  //
516  // [ R_b | t_b ] -1 [ R_a | t_a ] [ R_b^t * Ra | .. ]
517  // [ ------+-----] * [ ------+-----] = [ ---------- +----------]
518  // [ 0 0 0 | 1 ] [ 0 0 0 | 1 ] [ 0 0 0 | 1 ]
519  //
521  HM_C(UNINITIALIZED_MATRIX);
522 
523  A.getHomogeneousMatrix(HM_A);
524  B.getInverseHomogeneousMatrix(HM_B_inv);
525 
526  HM_C.multiply_AB(HM_B_inv, HM_A);
527 
528  this->m_rotvec = this->rotVecFromRotMat(HM_C);
529 
530  this->m_coords[0] = HM_C(0, 3);
531  this->m_coords[1] = HM_C(1, 3);
532  this->m_coords[2] = HM_C(2, 3);
533 }
534 
535 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
536  * \sa composePoint, composeFrom
537  */
538 void CPose3DRotVec::inverseComposePoint(
539  const double gx, const double gy, const double gz, double& lx, double& ly,
540  double& lz,
541  mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
542  mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose) const
543 {
544  MRPT_UNUSED_PARAM(out_jacobian_df_dpoint);
545  MRPT_UNUSED_PARAM(out_jacobian_df_dpose);
546  CPose3DRotVec rvt = this->getInverse();
547  rvt.composePoint(gx, gy, gz, lx, ly, lz);
548  MRPT_TODO("Jacobians");
549 }
550 
551 /** Exponentiate a Vector in the SE3 Lie Algebra to generate a new
552  * CPose3DRotVec.
553  */
554 CPose3DRotVec CPose3DRotVec::exp(const mrpt::math::CArrayDouble<6>& mu)
555 {
556  return CPose3DRotVec(mu);
557 }
558 
559 /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding
560  * vector in the Lie Algebra.
561  */
562 CArrayDouble<3> CPose3DRotVec::ln_rotation() const { return m_rotvec; }
563 /** Take the logarithm of the 3x4 matrix defined by this pose, generating the
564  * corresponding vector in the SE3 Lie Algebra.
565  */
566 void CPose3DRotVec::ln(CArrayDouble<6>& result) const
567 {
568  result.head<3>() = m_coords;
569  result.tail<3>() = m_rotvec;
570 }
571 
572 void CPose3DRotVec::setToNaN()
573 {
574  for (int i = 0; i < 3; i++)
575  {
576  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
577  m_rotvec[i] = std::numeric_limits<double>::quiet_NaN();
578  }
579 }
#define local
Definition: zutil.h:47
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
const GLshort * coords
Definition: glext.h:7386
#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
const double TH
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:845
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::CArrayDouble< 3 > m_coords
[x,y,z]
Definition: CPoint3D.h:40
const double G
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
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
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)
TColor operator+(const TColor &first, const TColor &second)
Pairwise addition of their corresponding RGBA members.
Definition: TColor.cpp:20
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:244
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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:879
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
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:98
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:174
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) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
GLbyte ty
Definition: glext.h:6092
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
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
GLubyte GLubyte b
Definition: glext.h:6279
double x
X,Y,Z coordinates.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:5566
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
#define MRPT_TODO(x)
Definition: common.h:129
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
Definition: CPose3DRotVec.h:53
void toQuatXYZ(CPose3DQuat &q) const
Convert this RVT into a quaternion + XYZ.
GLuint in
Definition: glext.h:7274
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:287
GLbyte GLbyte tz
Definition: glext.h:6092
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:57
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
Definition: CPose3DRotVec.h:44
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:46
Lightweight 3D point.
GLfloat param
Definition: glext.h:3831
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
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:223
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:140
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5566
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DRotVec.h:51



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019