MRPT  1.9.9
CPose3DQuatPDF.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 |
8  +------------------------------------------------------------------------+ */
9
10 #include "poses-precomp.h" // Precompiled headers
11
16
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace std;
20
22
23 /*---------------------------------------------------------------
24  copyFrom2D
25  ---------------------------------------------------------------*/
26 CPose3DQuatPDF* CPose3DQuatPDF::createFrom2D(const CPosePDF& o)
27 {
29
31  q.copyFrom(o);
32
33  return new CPose3DQuatPDFGaussian(q);
34
35  MRPT_END
36 }
37
38 /*---------------------------------------------------------------
39  jacobiansPoseComposition
40  ---------------------------------------------------------------*/
42  const CPose3DQuat& x, const CPose3DQuat& u, CMatrixDouble77& df_dx,
43  CMatrixDouble77& df_du, CPose3DQuat* out_x_oplus_u)
44 {
45  // For the derivation of the formulas, see the tech. report cited in the
47  const double qr = x.quat().r();
48  const double qx = x.quat().x();
49  const double qx2 = square(qx);
50  const double qy = x.quat().y();
51  const double qy2 = square(qy);
52  const double qz = x.quat().z();
53  const double qz2 = square(qz);
54
55  const double ax = u.x();
56  const double ay = u.y();
57  const double az = u.z();
58  const double q2r = u.quat().r();
59  const double q2x = u.quat().x();
60  const double q2y = u.quat().y();
61  const double q2z = u.quat().z();
62
63  CPose3DQuat x_plus_u = x + u; // needed for the normalization Jacobian:
65  x_plus_u.quat().normalizationJacobian(norm_jacob);
66
68  x.quat().normalizationJacobian(norm_jacob_x);
69
70  // df_dx ===================================================
71  df_dx.zeros();
72
73  // first part 3x7: df_{qr} / dp
74  df_dx.set_unsafe(0, 0, 1);
75  df_dx.set_unsafe(1, 1, 1);
76  df_dx.set_unsafe(2, 2, 1);
77
78  alignas(MRPT_MAX_ALIGN_BYTES)
79  const double vals2[3 * 4] = {2 * (-qz * ay + qy * az),
80  2 * (qy * ay + qz * az),
81  2 * (-2 * qy * ax + qx * ay + qr * az),
82  2 * (-2 * qz * ax - qr * ay + qx * az),
83
84  2 * (qz * ax - qx * az),
85  2 * (qy * ax - 2 * qx * ay - qr * az),
86  2 * (qx * ax + qz * az),
87  2 * (qr * ax - 2 * qz * ay + qy * az),
88
89  2 * (-qy * ax + qx * ay),
90  2 * (qz * ax + qr * ay - 2 * qx * az),
91  2 * (-qr * ax + qz * ay - 2 * qy * az),
92  2 * (qx * ax + qy * ay)};
93
94  // df_dx(0:3,3:7) = vals2 * NORM_JACOB
95  df_dx.block(0, 3, 3, 4).noalias() =
96  (CMatrixFixedNumeric<double, 3, 4>(vals2) * norm_jacob_x).eval();
97
98  // second part:
99  {
100  alignas(MRPT_MAX_ALIGN_BYTES) const double aux44_data[4 * 4] = {
101  q2r, -q2x, -q2y, -q2z, q2x, q2r, q2z, -q2y,
102  q2y, -q2z, q2r, q2x, q2z, q2y, -q2x, q2r};
103
104  df_dx.block(3, 3, 4, 4).noalias() =
105  (norm_jacob * CMatrixFixedNumeric<double, 4, 4>(aux44_data)).eval();
106  }
107
108  // df_du ===================================================
109  df_du.zeros();
110
111  // first part 3x3: df_{qr} / da
112  df_du.set_unsafe(0, 0, 1 - 2 * (qy2 + qz2));
113  df_du.set_unsafe(0, 1, 2 * (qx * qy - qr * qz));
114  df_du.set_unsafe(0, 2, 2 * (qr * qy + qx * qz));
115
116  df_du.set_unsafe(1, 0, 2 * (qr * qz + qx * qy));
117  df_du.set_unsafe(1, 1, 1 - 2 * (qx2 + qz2));
118  df_du.set_unsafe(1, 2, 2 * (qy * qz - qr * qx));
119
120  df_du.set_unsafe(2, 0, 2 * (qx * qz - qr * qy));
121  df_du.set_unsafe(2, 1, 2 * (qr * qx + qy * qz));
122  df_du.set_unsafe(2, 2, 1 - 2 * (qx2 + qy2));
123
124  // Second part:
125  {
126  alignas(MRPT_MAX_ALIGN_BYTES) const double aux44_data[4 * 4] = {
127  qr, -qx, -qy, -qz, qx, qr, -qz, qy,
128  qy, qz, qr, -qx, qz, -qy, qx, qr};
129
130  df_du.block(3, 3, 4, 4).noalias() =
131  (norm_jacob * CMatrixFixedNumeric<double, 4, 4>(aux44_data)).eval();
132  }
133
134  if (out_x_oplus_u) *out_x_oplus_u = x_plus_u;
135 }
mrpt::math::CQuaternionDouble & quat()
Definition: CPose3DQuat.h:59
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_MAX_ALIGN_BYTES
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:89
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
STL namespace.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
T square(const T x)
Inline function for the square of a number.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:85
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...
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:87
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:281
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
#define MRPT_END
Definition: exceptions.h:266
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint x
Definition: glext.h:3538
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:91
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE( class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...

 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