MRPT  2.0.1
CPose3DQuatPDF.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 
16 #include <Eigen/Dense>
17 
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
23 
24 /*---------------------------------------------------------------
25  copyFrom2D
26  ---------------------------------------------------------------*/
27 CPose3DQuatPDF* CPose3DQuatPDF::createFrom2D(const CPosePDF& o)
28 {
30 
32  q.copyFrom(o);
33 
34  return new CPose3DQuatPDFGaussian(q);
35 
36  MRPT_END
37 }
38 
39 /*---------------------------------------------------------------
40  jacobiansPoseComposition
41  ---------------------------------------------------------------*/
43  const CPose3DQuat& x, const CPose3DQuat& u, CMatrixDouble77& df_dx,
44  CMatrixDouble77& df_du, CPose3DQuat* out_x_oplus_u)
45 {
46  // For the derivation of the formulas, see the tech. report cited in the
47  // header file.
48  const double qr = x.quat().r();
49  const double qx = x.quat().x();
50  const double qx2 = square(qx);
51  const double qy = x.quat().y();
52  const double qy2 = square(qy);
53  const double qz = x.quat().z();
54  const double qz2 = square(qz);
55 
56  const double ax = u.x();
57  const double ay = u.y();
58  const double az = u.z();
59  const double q2r = u.quat().r();
60  const double q2x = u.quat().x();
61  const double q2y = u.quat().y();
62  const double q2z = u.quat().z();
63 
64  CPose3DQuat x_plus_u = x + u; // needed for the normalization Jacobian:
66  x_plus_u.quat().normalizationJacobian(norm_jacob);
67 
69  x.quat().normalizationJacobian(norm_jacob_x);
70 
71  // df_dx ===================================================
72  df_dx.setZero();
73 
74  // first part 3x7: df_{qr} / dp
75  df_dx(0, 0) = 1;
76  df_dx(1, 1) = 1;
77  df_dx(2, 2) = 1;
78 
79  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
80  const double vals2[3 * 4] = {2 * (-qz * ay + qy * az),
81  2 * (qy * ay + qz * az),
82  2 * (-2 * qy * ax + qx * ay + qr * az),
83  2 * (-2 * qz * ax - qr * ay + qx * az),
84 
85  2 * (qz * ax - qx * az),
86  2 * (qy * ax - 2 * qx * ay - qr * az),
87  2 * (qx * ax + qz * az),
88  2 * (qr * ax - 2 * qz * ay + qy * az),
89 
90  2 * (-qy * ax + qx * ay),
91  2 * (qz * ax + qr * ay - 2 * qx * az),
92  2 * (-qr * ax + qz * ay - 2 * qy * az),
93  2 * (qx * ax + qy * ay)};
94 
95  // df_dx(0:3,3:7) = vals2 * NORM_JACOB
96  df_dx.block<3, 4>(0, 3).noalias() =
97  (CMatrixFixed<double, 3, 4>(vals2) * norm_jacob_x).eval();
98 
99  // second part:
100  {
101  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double aux44_data[4 * 4] = {
102  q2r, -q2x, -q2y, -q2z, q2x, q2r, q2z, -q2y,
103  q2y, -q2z, q2r, q2x, q2z, q2y, -q2x, q2r};
104 
105  df_dx.block<4, 4>(3, 3).noalias() =
106  (norm_jacob * CMatrixFixed<double, 4, 4>(aux44_data)).asEigen();
107  }
108 
109  // df_du ===================================================
110  df_du.setZero();
111 
112  // first part 3x3: df_{qr} / da
113  df_du(0, 0) = 1 - 2 * (qy2 + qz2);
114  df_du(0, 1) = 2 * (qx * qy - qr * qz);
115  df_du(0, 2) = 2 * (qr * qy + qx * qz);
116 
117  df_du(1, 0) = 2 * (qr * qz + qx * qy);
118  df_du(1, 1) = 1 - 2 * (qx2 + qz2);
119  df_du(1, 2) = 2 * (qy * qz - qr * qx);
120 
121  df_du(2, 0) = 2 * (qx * qz - qr * qy);
122  df_du(2, 1) = 2 * (qr * qx + qy * qz);
123  df_du(2, 2) = 1 - 2 * (qx2 + qy2);
124 
125  // Second part:
126  {
127  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double aux44_data[4 * 4] = {
128  qr, -qx, -qy, -qz, qx, qr, -qz, qy,
129  qy, qz, qr, -qx, qz, -qy, qx, qr};
130 
131  df_du.block<4, 4>(3, 3).noalias() =
132  (norm_jacob * CMatrixFixed<double, 4, 4>(aux44_data)).asEigen();
133  }
134 
135  if (out_x_oplus_u) *out_x_oplus_u = x_plus_u;
136 }
#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
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
STL namespace.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
This base provides a set of functions for maths stuff.
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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...
return_t square(const num_t x)
Inline function for the square of a number.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
#define MRPT_END
Definition: exceptions.h:245
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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$.
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020