Main MRPT website > C++ reference for MRPT 1.5.6
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-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/utils/CStream.h>
16 
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 IMPLEMENTS_VIRTUAL_SERIALIZABLE( CPose3DQuatPDF, CSerializable, mrpt::poses )
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 /*---------------------------------------------------------------
40  jacobiansPoseComposition
41  ---------------------------------------------------------------*/
43  const CPose3DQuat &x,
44  const CPose3DQuat &u,
45  CMatrixDouble77 &df_dx,
46  CMatrixDouble77 &df_du,
47  CPose3DQuat *out_x_oplus_u
48  )
49 {
50  // For the derivation of the formulas, see the tech. report cited in the header file.
51  const double qr = x.quat().r();
52  const double qx = x.quat().x(); const double qx2 = square(qx);
53  const double qy = x.quat().y(); const double qy2 = square(qy);
54  const double qz = x.quat().z(); 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.zeros();
73 
74  // first part 3x7: df_{qr} / dp
75  df_dx.set_unsafe(0,0, 1);
76  df_dx.set_unsafe(1,1, 1);
77  df_dx.set_unsafe(2,2, 1);
78 
79  MRPT_ALIGN16 const double vals2[3*4] = {
80  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 
96  // df_dx(0:3,3:7) = vals2 * NORM_JACOB
97  df_dx.block(0,3, 3,4).noalias() = (CMatrixFixedNumeric<double,3,4>(vals2) * norm_jacob_x).eval();
98 
99  // second part:
100  {
101  MRPT_ALIGN16 const double aux44_data[4*4] = {
102  q2r,-q2x,-q2y,-q2z,
103  q2x, q2r, q2z,-q2y,
104  q2y,-q2z, q2r, q2x,
105  q2z, q2y,-q2x, q2r };
106 
107  df_dx.block(3,3, 4,4).noalias() = (norm_jacob * CMatrixFixedNumeric<double,4,4>(aux44_data)).eval();
108  }
109 
110  // df_du ===================================================
111  df_du.zeros();
112 
113  // first part 3x3: df_{qr} / da
114  df_du.set_unsafe(0,0, 1-2*(qy2+qz2) );
115  df_du.set_unsafe(0,1, 2*(qx*qy - qr*qz ) );
116  df_du.set_unsafe(0,2, 2*(qr*qy + qx*qz ) );
117 
118  df_du.set_unsafe(1,0, 2*(qr*qz + qx*qy ) );
119  df_du.set_unsafe(1,1, 1 - 2*( qx2+qz2) );
120  df_du.set_unsafe(1,2, 2*(qy*qz - qr*qx ) );
121 
122  df_du.set_unsafe(2,0, 2*(qx*qz - qr*qy ) );
123  df_du.set_unsafe(2,1, 2*(qr*qx + qy*qz ) );
124  df_du.set_unsafe(2,2, 1-2*(qx2+qy2) );
125 
126  // Second part:
127  {
128  MRPT_ALIGN16 const double aux44_data[4*4] = {
129  qr,-qx,-qy,-qz,
130  qx, qr,-qz, qy,
131  qy, qz, qr,-qx,
132  qz,-qy, qx, qr };
133 
134  df_du.block(3,3, 4,4).noalias() = (norm_jacob * CMatrixFixedNumeric<double,4,4>(aux44_data)).eval();
135  }
136 
137  if (out_x_oplus_u)
138  *out_x_oplus_u = x_plus_u;
139 }
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
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.
Definition: bits.h:52
A numeric matrix of compile-time fixed size.
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_END
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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
#define MRPT_START
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
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 ...
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
#define MRPT_ALIGN16



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018