Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DQuat.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 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPose3DQuat.h>
13 #include <mrpt/utils/CStream.h>
14 #include <iomanip>
15 #include <limits>
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::math;
20 using namespace mrpt::utils;
21 using namespace mrpt::poses;
22 
23 
25 
26 /** Constructor from a CPose3D */
28 {
29  x() = p.x();
30  y() = p.y();
31  z() = p.z();
32  p.getAsQuaternion(m_quat);
33 }
34 
35 /** Constructor from a 4x4 homogeneous transformation matrix.
36  */
37 CPose3DQuat::CPose3DQuat(const CMatrixDouble44 &M) : m_quat(UNINITIALIZED_QUATERNION)
38 {
39  m_coords[0] = M.get_unsafe(0,3);
40  m_coords[1] = M.get_unsafe(1,3);
41  m_coords[2] = M.get_unsafe(2,3);
42  CPose3D p(M);
43  p.getAsQuaternion(m_quat);
44 }
45 
46 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
47  * \sa getInverseHomogeneousMatrix
48  */
50 {
52  out_HM.get_unsafe(0,3) = m_coords[0];
53  out_HM.get_unsafe(1,3) = m_coords[1];
54  out_HM.get_unsafe(2,3) = m_coords[2];
55  out_HM.get_unsafe(3,0) = out_HM.get_unsafe(3,1) = out_HM.get_unsafe(3,2) = 0;
56  out_HM.get_unsafe(3,3) = 1;
57 }
58 
59 /** Returns a 1x7 vector with [x y z qr qx qy qz] */
61 {
62  v.resize(7);
63  v[0] = m_coords[0];
64  v[1] = m_coords[1];
65  v[2] = m_coords[2];
66  v[3] = m_quat[0];
67  v[4] = m_quat[1];
68  v[5] = m_quat[2];
69  v[6] = m_quat[3];
70 }
71 
72 /** Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
73  * \note A or B can be "this" without problems.
74  */
76 {
77  // The 3D point:
78  double gx,gy,gz;
79  A.m_quat.rotatePoint(B.m_coords[0],B.m_coords[1],B.m_coords[2], gx,gy,gz);
80  this->m_coords[0] = A.m_coords[0] + gx;
81  this->m_coords[1] = A.m_coords[1] + gy;
82  this->m_coords[2] = A.m_coords[2] + gz;
83 
84  // The 3D rotation:
85  this->m_quat.crossProduct(A.m_quat,B.m_quat);
86 }
87 
88 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
89  * \note A or B can be "this" without problems.
90  * \sa composeFrom
91  */
93 {
94  // The 3D point:
95  const CQuaternionDouble B_conj(B.m_quat.r(),-B.m_quat.x(),-B.m_quat.y(),-B.m_quat.z());
96  B_conj.rotatePoint(A.m_coords[0]-B.m_coords[0],A.m_coords[1]-B.m_coords[1],A.m_coords[2]-B.m_coords[2], this->m_coords[0],this->m_coords[1],this->m_coords[2]);
97  // The 3D rotation:
98  this->m_quat.crossProduct(B_conj,A.m_quat);
99 }
100 
101 
102 /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
103  * \sa inverseComposeFrom
104  */
105 void CPose3DQuat::composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
106  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
107  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose ) const
108 {
109  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
110  {
111  const double qx2 = square(m_quat.x());
112  const double qy2 = square(m_quat.y());
113  const double qz2 = square(m_quat.z());
114 
115  // Jacob: df/dpoint
116  if (out_jacobian_df_dpoint)
117  {
118  // 3x3: df_{qr} / da
119 
120  MRPT_ALIGN16 const double vals[3*3] = {
121  1-2*(qy2+qz2) ,
122  2*(m_quat.x()*m_quat.y() - m_quat.r()*m_quat.z() ) ,
123  2*(m_quat.r()*m_quat.y() + m_quat.x()*m_quat.z() ) ,
124 
125  2*(m_quat.r()*m_quat.z() + m_quat.x()*m_quat.y() ) ,
126  1 - 2*( qx2+qz2) ,
127  2*(m_quat.y()*m_quat.z() - m_quat.r()*m_quat.x() ) ,
128 
129  2*(m_quat.x()*m_quat.z() - m_quat.r()*m_quat.y() ) ,
130  2*(m_quat.r()*m_quat.x() + m_quat.y()*m_quat.z() ) ,
131  1-2*(qx2+qy2)
132  };
133  out_jacobian_df_dpoint->loadFromArray(vals);
134  }
135 
136  // Jacob: df/dpose
137  if (out_jacobian_df_dpose)
138  {
139  // 3x7: df_{qr} / dp
140  MRPT_ALIGN16 const double vals1[3*7] = {
141  1,0,0, 0,0,0,0,
142  0,1,0, 0,0,0,0,
143  0,0,1, 0,0,0,0 };
144  out_jacobian_df_dpose->loadFromArray(vals1);
145 
146 
147  MRPT_ALIGN16 const double vals[3*4] = {
148  2*(-m_quat.z()*ly +m_quat.y()*lz ),
149  2*(m_quat.y()*ly + m_quat.z()*lz ),
150  2*(-2*m_quat.y()*lx + m_quat.x()*ly +m_quat.r()*lz ),
151  2*(-2*m_quat.z()*lx - m_quat.r()*ly +m_quat.x()*lz ),
152 
153  2*(m_quat.z()*lx - m_quat.x()*lz ),
154  2*(m_quat.y()*lx - 2*m_quat.x()*ly -m_quat.r()*lz ),
155  2*(m_quat.x()*lx +m_quat.z()*lz ),
156  2*(m_quat.r()*lx - 2*m_quat.z()*ly +m_quat.y()*lz ),
157 
158  2*(-m_quat.y()*lx + m_quat.x()*ly ),
159  2*( m_quat.z()*lx + m_quat.r()*ly - 2*m_quat.x()*lz ),
160  2*(-m_quat.r()*lx + m_quat.z()*ly - 2*m_quat.y()*lz ),
161  2*( m_quat.x()*lx + m_quat.y()*ly )
162  };
163 
165  this->quat().normalizationJacobian(norm_jacob);
166 
167  out_jacobian_df_dpose->insertMatrix(0,3, (CMatrixFixedNumeric<double,3,4>(vals)*norm_jacob).eval() );
168  }
169  }
170 
171  // function itself:
172  m_quat.rotatePoint(lx,ly,lz, gx,gy,gz);
173  gx+=m_coords[0];
174  gy+=m_coords[1];
175  gz+=m_coords[2];
176 }
177 
178 /** Computes the 3D point G such as \f$ L = G \ominus this \f$.
179  * \sa composeFrom
180  */
181 void CPose3DQuat::inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
182  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint,
183  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose) const
184 {
185  if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
186  {
187  const double qx2 = square(m_quat.x());
188  const double qy2 = square(m_quat.y());
189  const double qz2 = square(m_quat.z());
190 
191  // Jacob: df/dpoint
192  if (out_jacobian_df_dpoint)
193  {
194  // 3x3: df_{m_quat.r()} / da
195  // inv_df_da =
196  // [ - 2*qy^2 - 2*qz^2 + 1, 2*qx*qy - 2*qr*qz, 2*qr*qy + 2*qx*qz]
197  // [ 2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1, 2*qy*qz - 2*qr*qx]
198  // [ 2*qx*qz - 2*qr*qy, 2*qr*qx + 2*qy*qz, - 2*qx^2 - 2*qy^2 + 1]
199  //
200 
201  MRPT_ALIGN16 const double vals[3*3] = {
202  1-2*(qy2+qz2),
203  2*(m_quat.x()*m_quat.y() + m_quat.r()*m_quat.z() ) ,
204  2*(-m_quat.r()*m_quat.y() + m_quat.x()*m_quat.z() ) ,
205 
206  2*(-m_quat.r()*m_quat.z() + m_quat.x()*m_quat.y() ) ,
207  1 - 2*( qx2+qz2) ,
208  2*(m_quat.y()*m_quat.z() + m_quat.r()*m_quat.x() ) ,
209 
210  2*(m_quat.x()*m_quat.z() + m_quat.r()*m_quat.y() ) ,
211  2*(-m_quat.r()*m_quat.x() + m_quat.y()*m_quat.z() ) ,
212  1-2*(qx2+qy2)
213  };
214  out_jacobian_df_dpoint->loadFromArray(vals);
215  }
216 
217  // Jacob: df/dpose
218  if (out_jacobian_df_dpose)
219  {
220  // 3x7: df_{m_quat.r()} / dp
221  // inv_df_dp =
222  //[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy, 2*qr*qy - 2*qx*qz, 2*qz*(ay - y) - 2*qy*(az - z), 2*qy*(ay - y) + 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z), 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
223  //[ 2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz, 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) + 2*qr*(az - z), 2*qx*(ax - x) + 2*qz*(az - z), 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
224  //[ - 2*qr*qy - 2*qx*qz, 2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1, 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) - 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z), 2*qx*(ax - x) + 2*qy*(ay - y)]
225  //
226  const double qr = m_quat.r();
227  const double qx = m_quat.x();
228  const double qy = m_quat.y();
229  const double qz = m_quat.z();
230 
231 
232  MRPT_ALIGN16 const double vals1[3*7] = {
233  2*qy2 + 2*qz2 - 1,
234  -2*qr*qz - 2*qx*qy ,
235  2*qr*qy - 2*qx*qz ,
236  0,0,0,0,
237 
238  2*qr*qz - 2*qx*qy ,
239  2*qx2 + 2*qz2 - 1 ,
240  -2*qr*qx - 2*qy*qz ,
241  0,0,0,0,
242 
243  -2*qr*qy - 2*qx*qz ,
244  2*qr*qx - 2*qy*qz ,
245  2*qx2 + 2*qy2 - 1,
246  0,0,0,0,
247  };
248 
249  out_jacobian_df_dpose->loadFromArray(vals1);
250 
251  const double Ax = 2*(gx - m_coords[0]);
252  const double Ay = 2*(gy - m_coords[1]);
253  const double Az = 2*(gz - m_coords[2]);
254 
255  MRPT_ALIGN16 const double vals[3*4] = {
256  -qy*Az + qz*Ay ,
257  qy*Ay + qz*Az ,
258  qx*Ay - 2*qy*Ax - qr*Az ,
259  qx*Az + qr*Ay - 2*qz*Ax ,
260 
261  qx*Az - qz*Ax,
262  qy*Ax - 2*qx*Ay + qr*Az,
263  qx*Ax + qz*Az,
264  qy*Az - 2*qz*Ay - qr*Ax,
265 
266  qy*Ax - qx*Ay,
267  qz*Ax - qr*Ay - 2*qx*Az,
268  qr*Ax + qz*Ay - 2*qy*Az,
269  qx*Ax + qy*Ay
270  };
271 
273  this->quat().normalizationJacobian(norm_jacob);
274 
275  out_jacobian_df_dpose->insertMatrix(0,3, (CMatrixFixedNumeric<double,3,4>(vals)*norm_jacob).eval() );
276  }
277  }
278 
279  // function itself:
280  m_quat.inverseRotatePoint(gx-m_coords[0],gy-m_coords[1],gz-m_coords[2], lx,ly,lz);
281 }
282 
283 /*---------------------------------------------------------------
284  *=
285  ---------------------------------------------------------------*/
286 void CPose3DQuat::operator *=(const double s)
287 {
288  m_coords[0]*=s;
289  m_coords[1]*=s;
290  m_coords[2]*=s;
291  m_quat[0]*=s;
292  m_quat[1]*=s;
293  m_quat[2]*=s;
294  m_quat[3]*=s;
295 }
296 
297 /*---------------------------------------------------------------
298  Implements the writing to a CStream capability of
299  CSerializable objects
300  ---------------------------------------------------------------*/
302 {
303  if (version)
304  *version = 0;
305  else
306  {
307  out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1] << m_quat[2] << m_quat[3];
308  }
309 }
310 
311 /*---------------------------------------------------------------
312  Implements the reading from a CStream capability of
313  CSerializable objects
314  ---------------------------------------------------------------*/
316 {
317  switch(version)
318  {
319  case 0:
320  {
321  in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >> m_quat[1] >> m_quat[2] >> m_quat[3];
322  } break;
323  default:
325 
326  };
327 }
328 
329 /*---------------------------------------------------------------
330  sphericalCoordinates
331 ---------------------------------------------------------------*/
333  const TPoint3D &point,
334  double &out_range,
335  double &out_yaw,
336  double &out_pitch,
337  mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint,
338  mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose
339  ) const
340 {
341  const bool comp_jacobs = out_jacob_dryp_dpoint!=NULL || out_jacob_dryp_dpose!=NULL;
342 
343  // Pass to coordinates as seen from this 6D pose:
344  CMatrixFixedNumeric<double,3,3> jacob_dinv_dpoint, *ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : NULL;
345  CMatrixFixedNumeric<double,3,7> jacob_dinv_dpose, *ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : NULL;
346 
347  TPoint3D local;
348  this->inverseComposePoint(point.x,point.y,point.z, local.x,local.y,local.z,ptr_ja1,ptr_ja2);
349 
350  // Range:
351  out_range = local.norm();
352 
353  // Yaw:
354  if (local.y!=0 || local.x!=0)
355  out_yaw = atan2(local.y,local.x);
356  else out_yaw = 0;
357 
358  // Pitch:
359  if (out_range!=0)
360  out_pitch = -asin( local.z / out_range );
361  else out_pitch = 0;
362 
363  // Jacobians are:
364  // dryp_dpoint = dryp_dlocalpoint * dinv_dpoint
365  // dryp_dpose = dryp_dlocalpoint * dinv_dpose
366  if (comp_jacobs)
367  {
368  if (out_range==0)
369  THROW_EXCEPTION("Jacobians are undefined for range=0")
370 
371  /* MATLAB:
372  syms H h_range h_yaw h_pitch real;
373  syms xi_ yi_ zi_ real;
374  h_range = sqrt(xi_^2+yi_^2+zi_^2);
375  h_yaw = atan(yi_/xi_);
376  % h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
377  h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
378  H=[ h_range ; h_yaw ; h_pitch ];
379  jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
380  */
381  const double _r = 1.0/out_range;
382  const double x2 = square(local.x);
383  const double y2 = square(local.y);
384 
385  const double t2 = std::sqrt(x2 + y2);
386  const double _K = 1.0/(t2*square(out_range));
387 
388  double vals[3*3]= {
389  local.x*_r, local.y*_r, local.z*_r,
390  -local.y/(x2*(y2/x2 + 1)), 1.0/(local.x*(y2/x2 + 1)), 0,
391  (local.x*local.z) * _K, (local.y*local.z) * _K, -t2/square(out_range)
392  };
393 
394  const CMatrixDouble33 dryp_dlocalpoint(vals);
395  if (out_jacob_dryp_dpoint)
396  out_jacob_dryp_dpoint->multiply(dryp_dlocalpoint,jacob_dinv_dpoint);
397  if (out_jacob_dryp_dpose)
398  out_jacob_dryp_dpose->multiply(dryp_dlocalpoint,jacob_dinv_dpose);
399  }
400 }
401 
402 
403 /** Textual output stream function.
404  */
405 std::ostream& mrpt::poses::operator << (std::ostream& o, const CPose3DQuat& p)
406 {
407  const std::streamsize old_pre = o.precision();
408  const ios_base::fmtflags old_flags = o.flags();
409  o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
410  << p.m_coords[0] << ","
411  << p.m_coords[1] << ","
412  << p.m_coords[2] << ","
413  << p.quat()[0] << ","
414  << p.quat()[1] << ","
415  << p.quat()[2] << ","
416  << p.quat()[3] << ")";
417  o.flags(old_flags);
418  o.precision(old_pre);
419  return o;
420 }
421 
422 
423 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with all its arguments multiplied by "-1") */
425 {
426  CPose3DQuat ret = p;
427  ret.inverse();
428  return ret;
429 }
430 
431 /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
433 {
434  // Invert translation:
435  this->inverseComposePoint(
436  0,0,0,
437  m_coords[0],m_coords[1],m_coords[2]);
438 
439  // Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
440  m_quat[1] = -m_quat[1];
441  m_quat[2] = -m_quat[2];
442  m_quat[3] = -m_quat[3];
443 }
444 
446 {
447  for (int i=0;i<3;i++)
448  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
449 
450  for (int i=0;i<4;i++)
451  quat()[i] = std::numeric_limits<double>::quiet_NaN();
452 }
453 
455 {
456  return p1.quat()==p2.quat() && p1.x()==p2.x() && p1.y()==p2.y() && p1.z()==p2.z();
457 }
458 
460 {
461  return !(p1==p2);
462 }
463 
465 {
466  CPoint3D L;
467  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
468  return L;
469 }
470 
472 {
474  p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
475  return L;
476 }
477 
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...
#define local
Definition: zutil.h:47
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x7 vector with [x y z qr qx qy qz].
Definition: CPose3DQuat.cpp:60
void inverseRotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion...
Definition: CQuaternion.h:203
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacob_dryp_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacob_dryp_dpose=NULL) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
GLdouble GLdouble z
Definition: glext.h:3734
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point G such as .
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
#define THROW_EXCEPTION(msg)
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
Definition: CQuaternion.h:288
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
STL namespace.
double z
X,Y,Z coordinates.
GLdouble s
Definition: glext.h:3602
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
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
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...
Definition: CQuaternion.h:179
void composeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A + B;" since it avoids the temporary objec...
Definition: CPose3DQuat.cpp:75
#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 inverse()
Convert this pose into its inverse, saving the result in itself.
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, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point L such as .
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 rotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.
Definition: CQuaternion.h:192
virtual void operator*=(const double s)
Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
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
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:47
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3DQuat.cpp:49
GLuint in
Definition: glext.h:6301
void inverseComposeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
Definition: CPose3DQuat.cpp:92
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:48
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.
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
GLfloat GLfloat p
Definition: glext.h:5587
#define MRPT_ALIGN16
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