Main MRPT website > C++ reference
MRPT logo
jacobians.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef mrpt_math_jacobians_H
29 #define mrpt_math_jacobians_H
30 
31 #include <mrpt/math/CQuaternion.h>
32 #include <mrpt/math/utils.h>
33 #include <mrpt/poses/CPose3D.h>
34 #include <mrpt/poses/CPosePDF.h>
35 #include <mrpt/poses/CPose3DQuatPDF.h>
36 #include <mrpt/poses/CPose3DPDF.h>
37 
38 namespace mrpt
39 {
40  namespace math
41  {
42  /** 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).
43  * 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.
44  * \ingroup mrpt_base_grp
45  */
46  namespace jacobians
47  {
48  using namespace mrpt::utils;
49  using namespace mrpt::poses;
50  using namespace mrpt::math;
51 
52  /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
53  * \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]
54  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
55  * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
56  */
57  inline void jacob_quat_from_yawpitchroll(
58  CMatrixFixedNumeric<double,4,3> &out_dq_dr,
59  const double yaw,
60  const double pitch,
61  const double roll
62  )
63  {
64  CQuaternionDouble q(UNINITIALIZED_QUATERNION);
65  CPose3D p(0,0,0,yaw,pitch,roll);
66  p.getAsQuaternion(q,&out_dq_dr);
67  }
68 
69  /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
70  * \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]
71  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
72  * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
73  */
74  inline void jacob_quat_from_yawpitchroll(
75  CMatrixFixedNumeric<double,4,3> &out_dq_dr,
76  const CPose3D &in_pose
77  )
78  {
79  CQuaternionDouble q(UNINITIALIZED_QUATERNION);
80  in_pose.getAsQuaternion(q,&out_dq_dr);
81  }
82 
83 
84  /** Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll).
85  * \sa jacob_quat_from_yawpitchroll
86  */
87  inline void jacob_yawpitchroll_from_quat(
88  CMatrixFixedNumeric<double,3,4> &out_dr_dq
89  )
90  {
91  THROW_EXCEPTION("TO DO")
92  }
93 
94  /** 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$.
95  * The output matrix can be a dynamic or fixed size (4x4) matrix. The input quaternion can be mrpt::math::CQuaternionFloat or mrpt::math::CQuaternionDouble.
96  */
97  template <class QUATERNION,class MATRIXLIKE>
98  inline void jacob_quat_rotation(const QUATERNION& quaternion, MATRIXLIKE &out_mat4x4)
99  {
100  quaternion.rotationJacobian(out_mat4x4);
101  }
102 
103  /** 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$.
104  * For the equations, see CPose3DPDF::jacobiansPoseComposition
105  */
106  inline void jacobs_6D_pose_comp(
107  const CPose3D &x,
108  const CPose3D &u,
109  CMatrixDouble66 &out_df_dx,
110  CMatrixDouble66 &out_df_du)
111  {
112  CPose3DPDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
113  }
114 
115  /** 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$.
116  * For the equations, see CPose3DQuatPDF::jacobiansPoseComposition
117  */
118  inline void jacobs_6D_pose_comp(
119  const CPose3DQuat &x,
120  const CPose3DQuat &u,
121  CMatrixDouble77 &out_df_dx,
122  CMatrixDouble77 &out_df_du)
123  {
124  CPose3DQuatPDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
125  }
126 
127  /** 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$.
128  * For the equations, see CPosePDF::jacobiansPoseComposition
129  */
130  inline void jacobs_2D_pose_comp(
131  const CPosePDFGaussian &x,
132  const CPosePDFGaussian &u,
133  CMatrixDouble33 &out_df_dx,
134  CMatrixDouble33 &out_df_du)
135  {
136  CPosePDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
137  }
138 
139  /** Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation. */
140  template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
141  inline void jacob_numeric_estimate(
142  const VECTORLIKE &x,
143  void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
144  const VECTORLIKE2 &increments,
145  const USERPARAM &userParam,
146  MATRIXLIKE &out_Jacobian )
147  {
148  mrpt::math::estimateJacobian(x,functor,increments,userParam,out_Jacobian);
149  }
150 
151 
152  } // End of jacobians namespace
153 
154  } // End of MATH namespace
155 
156 } // End of namespace
157 
158 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013