Main MRPT website > C++ reference for MRPT 1.5.7
jacobians.h
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 #ifndef mrpt_math_jacobians_H
10 #define mrpt_math_jacobians_H
11 
12 #include <mrpt/math/num_jacobian.h>
13 #include <mrpt/math/CQuaternion.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPosePDF.h>
17 #include <mrpt/poses/CPose3DPDF.h>
18 
19 namespace mrpt
20 {
21  namespace math
22  {
23  /** A collection of functions to compute jacobians of diverse transformations, etc (some functions are redirections to existing methods elsewhere, so this namespace is actually used with grouping purposes).
24  * Since most functions in this namespace are inline, their use implies no execution time overload and the code may be more clear to read, so it's recommended to use them where needed.
25  * \ingroup mrpt_base_grp
26  */
27  namespace jacobians
28  {
29  /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
30  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right) \f]
31  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
32  * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
33  */
36  const double yaw,
37  const double pitch,
38  const double roll
39  )
40  {
42  mrpt::poses::CPose3D p(0,0,0,yaw,pitch,roll);
43  p.getAsQuaternion(q,&out_dq_dr);
44  }
45 
46  /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
47  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right) \f]
48  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
49  * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
50  */
53  const mrpt::poses::CPose3D &in_pose
54  )
55  {
57  in_pose.getAsQuaternion(q,&out_dq_dr);
58  }
59 
60 
61  /** Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll).
62  * \sa jacob_quat_from_yawpitchroll
63  */
66  )
67  {
68  MRPT_UNUSED_PARAM(out_dr_dq);
69  THROW_EXCEPTION("TO DO")
70  }
71 
72  /** Compute the Jacobian of the rotation composition operation \f$ p = f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$ \frac{\partial f}{\partial q_{this} } \f$.
73  * The output matrix can be a dynamic or fixed size (4x4) matrix. The input quaternion can be mrpt::math::CQuaternionFloat or mrpt::math::CQuaternionDouble.
74  */
75  template <class QUATERNION,class MATRIXLIKE>
76  inline void jacob_quat_rotation(const QUATERNION& quaternion, MATRIXLIKE &out_mat4x4)
77  {
78  quaternion.rotationJacobian(out_mat4x4);
79  }
80 
81  /** Given the 3D(6D) pose composition \f$ f(x,u) = x \oplus u \f$, compute the two 6x6 Jacobians \f$ \frac{\partial f}{\partial x} \f$ and \f$ \frac{\partial f}{\partial u} \f$.
82  * For the equations, see CPose3DPDF::jacobiansPoseComposition
83  */
84  inline void jacobs_6D_pose_comp(
85  const mrpt::poses::CPose3D &x,
86  const mrpt::poses::CPose3D &u,
87  CMatrixDouble66 &out_df_dx,
88  CMatrixDouble66 &out_df_du)
89  {
91  }
92 
93  /** Given the 3D(6D) pose composition \f$ f(x,u) = x \oplus u \f$, compute the two 6x6 Jacobians \f$ \frac{\partial f}{\partial x} \f$ and \f$ \frac{\partial f}{\partial u} \f$.
94  * For the equations, see CPose3DQuatPDF::jacobiansPoseComposition
95  */
96  inline void jacobs_6D_pose_comp(
98  const mrpt::poses::CPose3DQuat &u,
99  CMatrixDouble77 &out_df_dx,
100  CMatrixDouble77 &out_df_du)
101  {
103  }
104 
105  /** Given the 2D pose composition \f$ f(x,u) = x \oplus u \f$, compute the two 3x3 Jacobians \f$ \frac{\partial f}{\partial x} \f$ and \f$ \frac{\partial f}{\partial u} \f$.
106  * For the equations, see CPosePDF::jacobiansPoseComposition
107  */
108  inline void jacobs_2D_pose_comp(
111  CMatrixDouble33 &out_df_dx,
112  CMatrixDouble33 &out_df_du)
113  {
114  mrpt::poses::CPosePDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
115  }
116 
117  /** Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation. */
118  template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
120  const VECTORLIKE &x,
121  void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
122  const VECTORLIKE2 &increments,
123  const USERPARAM &userParam,
124  MATRIXLIKE &out_Jacobian )
125  {
126  mrpt::math::estimateJacobian(x,functor,increments,userParam,out_Jacobian);
127  }
128 
129 
130  } // End of jacobians namespace
131 
132  } // End of MATH namespace
133 
134 } // End of namespace
135 
136 #endif
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:133
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
A numeric matrix of compile-time fixed size.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void jacob_quat_from_yawpitchroll(mrpt::math::CMatrixFixedNumeric< double, 4, 3 > &out_dq_dr, const double yaw, const double pitch, const double roll)
Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quatern...
Definition: jacobians.h:34
void jacobs_6D_pose_comp(const mrpt::poses::CPose3D &x, const mrpt::poses::CPose3D &u, CMatrixDouble66 &out_df_dx, CMatrixDouble66 &out_df_du)
Given the 3D(6D) pose composition , compute the two 6x6 Jacobians and .
Definition: jacobians.h:84
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static void jacobiansPoseComposition(const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true)
This static method computes the pose composition Jacobians, with these formulas:
Definition: CPosePDF.cpp:35
void jacob_quat_rotation(const QUATERNION &quaternion, MATRIXLIKE &out_mat4x4)
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...
Definition: jacobians.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$.
GLenum GLint GLint y
Definition: glext.h:3516
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
Definition: jacobians.h:119
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:26
void jacob_yawpitchroll_from_quat(mrpt::math::CMatrixFixedNumeric< double, 3, 4 > &out_dr_dq)
Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (ya...
Definition: jacobians.h:64
GLenum GLint x
Definition: glext.h:3516
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
void jacobs_2D_pose_comp(const mrpt::poses::CPosePDFGaussian &x, const mrpt::poses::CPosePDFGaussian &u, CMatrixDouble33 &out_df_dx, CMatrixDouble33 &out_df_du)
Given the 2D pose composition , compute the two 3x3 Jacobians and .
Definition: jacobians.h:108
GLfloat GLfloat p
Definition: glext.h:5587
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:553



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019