Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DPDF.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/CPose3DPDF.h>
21 #include <mrpt/poses/CPosePDFSOG.h>
23 #include <mrpt/utils/CStream.h>
24 
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace std;
28 
30 
31 /*---------------------------------------------------------------
32  copyFrom2D
33  ---------------------------------------------------------------*/
34 CPose3DPDF* CPose3DPDF::createFrom2D(const CPosePDF &o)
35 {
37 
38  if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
39  {
40  CPose3DPDFGaussian *newObj = new CPose3DPDFGaussian();
41  const CPosePDFGaussian *obj = static_cast<const CPosePDFGaussian *>( &o );
42 
43  newObj->mean = CPose3D( obj->mean );
44  CMatrixDouble COV = CMatrixDouble(obj->cov);
45  COV.setSize(6,6);
46 
47  // Move covariances: phi<->z
48  COV(3,3)=COV(2,2); COV(2,2)=0;
49  COV(0,3)=COV(3,0) = COV(0,2); COV(0,2)=COV(2,0)=0;
50  COV(1,3)=COV(3,1) = COV(1,2); COV(1,2)=COV(2,1)=0;
51 
52  newObj->cov= CMatrixDouble66(COV);
53 
54  return newObj;
55  }
56  else if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussianInf))
57  {
59  const CPosePDFGaussianInf *obj = static_cast<const CPosePDFGaussianInf *>( &o );
60 
61  newObj->mean = CPose3D( obj->mean );
62  CMatrixDouble COVINV = CMatrixDouble(obj->cov_inv);
63 
64  COVINV.setSize(6,6);
65  // Move covariances: phi<->z
66  COVINV(3,3)=COVINV(2,2); COVINV(2,2)=0;
67  COVINV(0,3)=COVINV(3,0) = COVINV(0,2); COVINV(0,2)=COVINV(2,0)=0;
68  COVINV(1,3)=COVINV(3,1) = COVINV(1,2); COVINV(1,2)=COVINV(2,1)=0;
69 
70  newObj->cov_inv= CMatrixDouble66(COVINV);
71 
72  return newObj;
73 
74  }
75  else
76  if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticles))
77  {
78  const CPosePDFParticles *obj = static_cast<const CPosePDFParticles*>( &o );
79  CPose3DPDFParticles *newObj = new CPose3DPDFParticles(obj->size());
80 
83  for (it1=obj->m_particles.begin(),it2=newObj->m_particles.begin();it1!=obj->m_particles.end();++it1,++it2)
84  {
85  it2->log_w = it1->log_w;
86  (*it2->d) = CPose3D(*it1->d);
87  }
88 
89  return newObj;
90  }
91  else
92  if (o.GetRuntimeClass()==CLASS_ID(CPosePDFSOG))
93  {
94  const CPosePDFSOG *obj = static_cast<const CPosePDFSOG*>( &o );
95  CPose3DPDFSOG *newObj = new CPose3DPDFSOG(obj->size());
96 
99 
100  for (it1=obj->begin(),it2=newObj->begin();it1!=obj->end();++it1,++it2)
101  {
102  it2->log_w = it1->log_w;
103  it2->val.mean.setFromValues( it1->mean.x(),it1->mean.y(),0, it1->mean.phi(),0,0 );
104 
105  it2->val.cov.zeros();
106 
107  it2->val.cov.get_unsafe(0,0) = it1->cov.get_unsafe(0,0);
108  it2->val.cov.get_unsafe(1,1) = it1->cov.get_unsafe(1,1);
109  it2->val.cov.get_unsafe(3,3) = it1->cov.get_unsafe(2,2); // yaw <- phi
110 
111  it2->val.cov.get_unsafe(0,1) =
112  it2->val.cov.get_unsafe(1,0) = it1->cov.get_unsafe(0,1);
113 
114  it2->val.cov.get_unsafe(0,3) =
115  it2->val.cov.get_unsafe(3,0) = it1->cov.get_unsafe(0,2);
116 
117  it2->val.cov.get_unsafe(1,3) =
118  it2->val.cov.get_unsafe(3,1) = it1->cov.get_unsafe(1,2);
119  }
120 
121  return newObj;
122  }
123  else
124  THROW_EXCEPTION("Class of object not supported by this method!");
125 
126  MRPT_END
127 }
128 
129 
130 /*---------------------------------------------------------------
131  jacobiansPoseComposition
132  ---------------------------------------------------------------*/
134  const CPose3D &x,
135  const CPose3D &u,
136  CMatrixDouble66 &df_dx,
137  CMatrixDouble66 &df_du)
138 {
139  // See this techical report: http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
140 
141  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
142  // Make a way around them and consider instead this path:
143  //
144  // X(6D) U(6D)
145  // | |
146  // v v
147  // X(7D) U(7D)
148  // | |
149  // +--- (+) ---+
150  // |
151  // v
152  // RES(7D)
153  // |
154  // v
155  // RES(6D)
156  //
157 
158  // FUNCTION = f_quat2eul( f_quat_comp( f_eul2quat(x) , f_eul2quat(u) ) )
159  // Jacobians chain rule:
160  //
161  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
162  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
163 
164  // J_E2Q_dx:
165  CMatrixFixedNumeric<double,7,6> J_E2Q_dx; // Init to zeros
166  {
169  x.getAsQuaternion(q_dumm, &dq_dr_sub);
170  J_E2Q_dx.get_unsafe(0,0)=J_E2Q_dx.get_unsafe(1,1)=J_E2Q_dx.get_unsafe(2,2)=1;
171  J_E2Q_dx.insertMatrix(3,3,dq_dr_sub);
172  }
173 
174  // J_E2Q_du:
175  CMatrixFixedNumeric<double,7,6> J_E2Q_du; // Init to zeros
176  {
179  u.getAsQuaternion(q_dumm, &dq_dr_sub);
180  J_E2Q_du.get_unsafe(0,0)=J_E2Q_du.get_unsafe(1,1)=J_E2Q_du.get_unsafe(2,2)=1;
181  J_E2Q_du.insertMatrix(3,3,dq_dr_sub);
182  }
183 
184  // quat_df_dx & quat_df_du
186  const CPose3DQuat quat_x(x);
187  const CPose3DQuat quat_u(u);
188 
190  quat_x, // x
191  quat_u, // u
192  quat_df_dx,
193  quat_df_du );
194 
195  // And now J_Q2E:
196  // [ I_3 | 0 ]
197  // J_Q2E = [ -------+------------- ]
198  // [ 0 | dr_dq_angles ]
199  //
200  CMatrixFixedNumeric<double,6,7> J_Q2E; // Init to zeros
201  J_Q2E.get_unsafe(0,0) = J_Q2E.get_unsafe(1,1) = J_Q2E.get_unsafe(2,2) = 1;
202  {
203  // The end result of the pose composition, as a quaternion:
205  q_xu.crossProduct(quat_x.quat(),quat_u.quat());
206 
207  // Compute the jacobian:
209  double yaw,pitch,roll;
210  q_xu.rpy_and_jacobian(roll,pitch,yaw,&dr_dq_sub_aux,false);
211 
213  q_xu.normalizationJacobian(dnorm_dq);
214 
216  dr_dq_sub.multiply(dr_dq_sub_aux,dnorm_dq);
217 
218  J_Q2E.insertMatrix(3,3,dr_dq_sub);
219  }
220 
221  // And finally:
222  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
223  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
224  df_dx.multiply_ABC(J_Q2E, quat_df_dx, J_E2Q_dx);
225  df_du.multiply_ABC(J_Q2E, quat_df_du, J_E2Q_du);
226 }
227 
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
CPose3D mean
The mean value.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:58
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
Definition: CPose3DPDF.cpp:133
#define THROW_EXCEPTION(msg)
Scalar * iterator
Definition: eigen_plugins.h:23
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
CParticleList m_particles
The array of particles.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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
#define MRPT_END
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
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
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
#define MRPT_START
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=NULL)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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 rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=NULL, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
Definition: CQuaternion.h:328
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:50
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
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
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:72
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



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