MRPT  1.9.9
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-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/CPose3DPDF.h>
21 #include <mrpt/poses/CPosePDFSOG.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);
49  COV(2, 2) = 0;
50  COV(0, 3) = COV(3, 0) = COV(0, 2);
51  COV(0, 2) = COV(2, 0) = 0;
52  COV(1, 3) = COV(3, 1) = COV(1, 2);
53  COV(1, 2) = COV(2, 1) = 0;
54 
55  newObj->cov = CMatrixDouble66(COV);
56 
57  return newObj;
58  }
59  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFGaussianInf))
60  {
62  const CPosePDFGaussianInf* obj =
63  static_cast<const CPosePDFGaussianInf*>(&o);
64 
65  newObj->mean = CPose3D(obj->mean);
66  CMatrixDouble COVINV = CMatrixDouble(obj->cov_inv);
67 
68  COVINV.setSize(6, 6);
69  // Move covariances: phi<->z
70  COVINV(3, 3) = COVINV(2, 2);
71  COVINV(2, 2) = 0;
72  COVINV(0, 3) = COVINV(3, 0) = COVINV(0, 2);
73  COVINV(0, 2) = COVINV(2, 0) = 0;
74  COVINV(1, 3) = COVINV(3, 1) = COVINV(1, 2);
75  COVINV(1, 2) = COVINV(2, 1) = 0;
76 
77  newObj->cov_inv = CMatrixDouble66(COVINV);
78 
79  return newObj;
80  }
81  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFParticles))
82  {
83  const CPosePDFParticles* obj =
84  static_cast<const CPosePDFParticles*>(&o);
85  CPose3DPDFParticles* newObj = new CPose3DPDFParticles(obj->size());
86 
89  for (it1 = obj->m_particles.begin(), it2 = newObj->m_particles.begin();
90  it1 != obj->m_particles.end(); ++it1, ++it2)
91  {
92  it2->log_w = it1->log_w;
93  it2->d = TPose3D(it1->d);
94  }
95 
96  return newObj;
97  }
98  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFSOG))
99  {
100  const CPosePDFSOG* obj = static_cast<const CPosePDFSOG*>(&o);
101  CPose3DPDFSOG* newObj = new CPose3DPDFSOG(obj->size());
102 
105 
106  for (it1 = obj->begin(), it2 = newObj->begin(); it1 != obj->end();
107  ++it1, ++it2)
108  {
109  it2->log_w = it1->log_w;
110  it2->val.mean.setFromValues(
111  it1->mean.x(), it1->mean.y(), 0, it1->mean.phi(), 0, 0);
112 
113  it2->val.cov.zeros();
114 
115  it2->val.cov.get_unsafe(0, 0) = it1->cov.get_unsafe(0, 0);
116  it2->val.cov.get_unsafe(1, 1) = it1->cov.get_unsafe(1, 1);
117  it2->val.cov.get_unsafe(3, 3) =
118  it1->cov.get_unsafe(2, 2); // yaw <- phi
119 
120  it2->val.cov.get_unsafe(0, 1) = it2->val.cov.get_unsafe(1, 0) =
121  it1->cov.get_unsafe(0, 1);
122 
123  it2->val.cov.get_unsafe(0, 3) = it2->val.cov.get_unsafe(3, 0) =
124  it1->cov.get_unsafe(0, 2);
125 
126  it2->val.cov.get_unsafe(1, 3) = it2->val.cov.get_unsafe(3, 1) =
127  it1->cov.get_unsafe(1, 2);
128  }
129 
130  return newObj;
131  }
132  else
133  THROW_EXCEPTION("Class of object not supported by this method!");
134 
135  MRPT_END
136 }
137 
138 /*---------------------------------------------------------------
139  jacobiansPoseComposition
140  ---------------------------------------------------------------*/
142  const CPose3D& x, const CPose3D& u, CMatrixDouble66& df_dx,
143  CMatrixDouble66& df_du)
144 {
145  // See this techical report:
146  // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
147 
148  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
149  // Make a way around them and consider instead this path:
150  //
151  // X(6D) U(6D)
152  // | |
153  // v v
154  // X(7D) U(7D)
155  // | |
156  // +--- (+) ---+
157  // |
158  // v
159  // RES(7D)
160  // |
161  // v
162  // RES(6D)
163  //
164 
165  // FUNCTION = f_quat2eul( f_quat_comp( f_eul2quat(x) , f_eul2quat(u) ) )
166  // Jacobians chain rule:
167  //
168  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
169  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
170 
171  // J_E2Q_dx:
172  CMatrixFixedNumeric<double, 7, 6> J_E2Q_dx; // Init to zeros
173  {
176  x.getAsQuaternion(q_dumm, &dq_dr_sub);
177  J_E2Q_dx.get_unsafe(0, 0) = J_E2Q_dx.get_unsafe(1, 1) =
178  J_E2Q_dx.get_unsafe(2, 2) = 1;
179  J_E2Q_dx.insertMatrix(3, 3, dq_dr_sub);
180  }
181 
182  // J_E2Q_du:
183  CMatrixFixedNumeric<double, 7, 6> J_E2Q_du; // Init to zeros
184  {
187  u.getAsQuaternion(q_dumm, &dq_dr_sub);
188  J_E2Q_du.get_unsafe(0, 0) = J_E2Q_du.get_unsafe(1, 1) =
189  J_E2Q_du.get_unsafe(2, 2) = 1;
190  J_E2Q_du.insertMatrix(3, 3, dq_dr_sub);
191  }
192 
193  // quat_df_dx & quat_df_du
195  quat_df_du(UNINITIALIZED_MATRIX);
196  const CPose3DQuat quat_x(x);
197  const CPose3DQuat quat_u(u);
198 
200  quat_x, // x
201  quat_u, // u
202  quat_df_dx, quat_df_du);
203 
204  // And now J_Q2E:
205  // [ I_3 | 0 ]
206  // J_Q2E = [ -------+------------- ]
207  // [ 0 | dr_dq_angles ]
208  //
209  CMatrixFixedNumeric<double, 6, 7> J_Q2E; // Init to zeros
210  J_Q2E.get_unsafe(0, 0) = J_Q2E.get_unsafe(1, 1) = J_Q2E.get_unsafe(2, 2) =
211  1;
212  {
213  // The end result of the pose composition, as a quaternion:
215  q_xu.crossProduct(quat_x.quat(), quat_u.quat());
216 
217  // Compute the jacobian:
219  double yaw, pitch, roll;
220  q_xu.rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);
221 
223  q_xu.normalizationJacobian(dnorm_dq);
224 
226  dr_dq_sub.multiply(dr_dq_sub_aux, dnorm_dq);
227 
228  J_Q2E.insertMatrix(3, 3, dr_dq_sub);
229  }
230 
231  // And finally:
232  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
233  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
234  df_dx.multiply_ABC(J_Q2E, quat_df_dx, J_E2Q_dx);
235  df_du.multiply_ABC(J_Q2E, quat_df_du, J_E2Q_du);
236 }
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, 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:409
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:59
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
CPose3D mean
The mean value.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:35
CParticleList m_particles
The array of particles.
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:67
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:141
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:59
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:510
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
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:200
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:46
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...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
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:281
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:53
#define MRPT_END
Definition: exceptions.h:266
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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:44
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=nullptr)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
#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 3D pose.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020