MRPT  1.9.9
CPose3DPDF.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TPose3D.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DPDF.h>
23 #include <mrpt/poses/CPosePDFSOG.h>
25 #include <Eigen/Dense>
26 
27 using namespace mrpt::poses;
28 using namespace mrpt::math;
29 using namespace std;
30 
32 
33 // copy covariances: x, y
34 // Move covariances: phi -> yaw
35 template <class MAT33, class MAT66>
36 void cov2to3(const MAT33& c2d, MAT66& c3d)
37 {
38  c3d.setZero();
39  c3d.asEigen().template block<2, 2>(0, 0) =
40  c2d.asEigen().template block<2, 2>(0, 0);
41  c3d(3, 3) = c2d(2, 2);
42  c3d(0, 3) = c3d(3, 0) = c2d(0, 2);
43  c3d(1, 3) = c3d(3, 1) = c2d(1, 2);
44 }
45 
46 /*---------------------------------------------------------------
47  copyFrom2D
48  ---------------------------------------------------------------*/
50 {
52 
54  {
55  auto* newObj = new CPose3DPDFGaussian();
56  const auto* obj = dynamic_cast<const CPosePDFGaussian*>(&o);
57  ASSERT_(obj != nullptr);
58  newObj->mean = CPose3D(obj->mean);
59  cov2to3(obj->cov, newObj->cov);
60  return newObj;
61  }
63  {
64  auto* newObj = new CPose3DPDFGaussianInf();
65  const auto* obj = dynamic_cast<const CPosePDFGaussianInf*>(&o);
66  ASSERT_(obj != nullptr);
67 
68  newObj->mean = CPose3D(obj->mean);
69  cov2to3(obj->cov_inv, newObj->cov_inv);
70  return newObj;
71  }
73  {
74  const auto* obj = dynamic_cast<const CPosePDFParticles*>(&o);
75  ASSERT_(obj != nullptr);
76  auto* newObj = new CPose3DPDFParticles(obj->size());
77 
78  CPosePDFParticles::CParticleList::const_iterator it1;
79  CPose3DPDFParticles::CParticleList::iterator it2;
80  for (it1 = obj->m_particles.begin(), it2 = newObj->m_particles.begin();
81  it1 != obj->m_particles.end(); ++it1, ++it2)
82  {
83  it2->log_w = it1->log_w;
84  it2->d = TPose3D(it1->d);
85  }
86 
87  return newObj;
88  }
89  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFSOG))
90  {
91  const auto* obj = dynamic_cast<const CPosePDFSOG*>(&o);
92  ASSERT_(obj != nullptr);
93  auto* newObj = new CPose3DPDFSOG(obj->size());
94 
97 
98  for (it1 = obj->begin(), it2 = newObj->begin(); it1 != obj->end();
99  ++it1, ++it2)
100  {
101  it2->log_w = it1->log_w;
102  it2->val.mean.setFromValues(
103  it1->mean.x(), it1->mean.y(), 0, it1->mean.phi(), 0, 0);
104 
105  it2->val.cov.setZero();
106 
107  it2->val.cov(0, 0) = it1->cov(0, 0);
108  it2->val.cov(1, 1) = it1->cov(1, 1);
109  it2->val.cov(3, 3) = it1->cov(2, 2); // yaw <- phi
110 
111  it2->val.cov(0, 1) = it2->val.cov(1, 0) = it1->cov(0, 1);
112 
113  it2->val.cov(0, 3) = it2->val.cov(3, 0) = it1->cov(0, 2);
114 
115  it2->val.cov(1, 3) = it2->val.cov(3, 1) = it1->cov(1, 2);
116  }
117 
118  return newObj;
119  }
120  else
121  THROW_EXCEPTION("Class of object not supported by this method!");
122 
123  MRPT_END
124 }
125 
126 /*---------------------------------------------------------------
127  jacobiansPoseComposition
128  ---------------------------------------------------------------*/
130  const CPose3D& x, const CPose3D& u, CMatrixDouble66& df_dx,
131  CMatrixDouble66& df_du)
132 {
133  // See this techical report:
134  // https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
135 
136  // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
137  // Make a way around them and consider instead this path:
138  //
139  // X(6D) U(6D)
140  // | |
141  // v v
142  // X(7D) U(7D)
143  // | |
144  // +--- (+) ---+
145  // |
146  // v
147  // RES(7D)
148  // |
149  // v
150  // RES(6D)
151  //
152 
153  // FUNCTION = f_quat2eul( f_quat_comp( f_eul2quat(x) , f_eul2quat(u) ) )
154  // Jacobians chain rule:
155  //
156  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
157  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
158 
159  // J_E2Q_dx:
160  CMatrixFixed<double, 7, 6> J_E2Q_dx; // Init to zeros
161  {
164  x.getAsQuaternion(q_dumm, dq_dr_sub);
165  J_E2Q_dx(0, 0) = J_E2Q_dx(1, 1) = J_E2Q_dx(2, 2) = 1;
166  J_E2Q_dx.insertMatrix(3, 3, dq_dr_sub);
167  }
168 
169  // J_E2Q_du:
170  CMatrixFixed<double, 7, 6> J_E2Q_du; // Init to zeros
171  {
174  u.getAsQuaternion(q_dumm, dq_dr_sub);
175  J_E2Q_du(0, 0) = J_E2Q_du(1, 1) = J_E2Q_du(2, 2) = 1;
176  J_E2Q_du.insertMatrix(3, 3, dq_dr_sub);
177  }
178 
179  // quat_df_dx & quat_df_du
181  quat_df_du(UNINITIALIZED_MATRIX);
182  const CPose3DQuat quat_x(x);
183  const CPose3DQuat quat_u(u);
184 
186  quat_x, // x
187  quat_u, // u
188  quat_df_dx, quat_df_du);
189 
190  // And now J_Q2E:
191  // [ I_3 | 0 ]
192  // J_Q2E = [ -------+------------- ]
193  // [ 0 | dr_dq_angles ]
194  //
195  CMatrixFixed<double, 6, 7> J_Q2E; // Init to zeros
196  J_Q2E(0, 0) = J_Q2E(1, 1) = J_Q2E(2, 2) = 1;
197  {
198  // The end result of the pose composition, as a quaternion:
200  q_xu.crossProduct(quat_x.quat(), quat_u.quat());
201 
202  // Compute the jacobian:
204  double yaw, pitch, roll;
205  q_xu.rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);
206 
208  q_xu.normalizationJacobian(dnorm_dq);
209 
211  dr_dq_sub.asEigen() = dr_dq_sub_aux * dnorm_dq;
212 
213  J_Q2E.insertMatrix(3, 3, dr_dq_sub);
214  }
215 
216  // And finally:
217  // JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
218  // JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
219  df_dx.asEigen() =
220  J_Q2E.asEigen() * quat_df_dx.asEigen() * J_E2Q_dx.asEigen();
221  df_du.asEigen() =
222  J_Q2E.asEigen() * quat_df_du.asEigen() * J_E2Q_du.asEigen();
223 }
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:449
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:66
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:129
static CPose3DPDF * createFrom2D(const CPosePDF &o)
This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (...
Definition: CPose3DPDF.cpp:49
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
STL namespace.
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:501
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:102
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:231
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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:45
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:313
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:52
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void cov2to3(const MAT33 &c2d, MAT66 &c3d)
Definition: CPose3DPDF.cpp:36
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$.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020