Main MRPT website > C++ reference
MRPT logo

jacobians.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  mrpt_math_jacobians_H
00029 #define  mrpt_math_jacobians_H
00030 
00031 #include <mrpt/math/CQuaternion.h>
00032 #include <mrpt/math/utils.h>
00033 #include <mrpt/poses/CPose3D.h>
00034 #include <mrpt/poses/CPose3DPDFGaussian.h>
00035 #include <mrpt/poses/CPosePDFGaussian.h>
00036 
00037 namespace mrpt
00038 {
00039         namespace math
00040         {
00041                 /** 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).
00042                   *  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.
00043                   */
00044                 namespace jacobians
00045                 {
00046                         using namespace mrpt::utils;
00047                         using namespace mrpt::poses;
00048                         using namespace mrpt::math;
00049 
00050                         /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
00051                           * \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]
00052                           * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
00053                           * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
00054                           */
00055                         inline void jacob_quat_from_yawpitchroll(
00056                                 CMatrixFixedNumeric<double,4,3>  &out_dq_dr,
00057                                 const double yaw,
00058                                 const double pitch,
00059                                 const double roll
00060                                 )
00061                         {
00062                                 CQuaternionDouble q(UNINITIALIZED_QUATERNION);
00063                                 CPose3D  p(0,0,0,yaw,pitch,roll);
00064                                 p.getAsQuaternion(q,&out_dq_dr);
00065                         }
00066 
00067                         /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of:
00068                           * \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]
00069                           * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
00070                           * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion
00071                           */
00072                         inline void jacob_quat_from_yawpitchroll(
00073                                 CMatrixFixedNumeric<double,4,3> &out_dq_dr,
00074                                 const CPose3D  &in_pose
00075                                 )
00076                         {
00077                                 CQuaternionDouble q(UNINITIALIZED_QUATERNION);
00078                                 in_pose.getAsQuaternion(q,&out_dq_dr);
00079                         }
00080 
00081 
00082                         /** Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll).
00083                           * \sa jacob_quat_from_yawpitchroll
00084                           */
00085                         inline void jacob_yawpitchroll_from_quat(
00086                                 CMatrixFixedNumeric<double,3,4>  &out_dr_dq
00087                                 )
00088                         {
00089                                 THROW_EXCEPTION("TO DO")
00090                         }
00091 
00092                         /** 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$.
00093                           *  The output matrix can be a dynamic or fixed size (4x4) matrix. The input quaternion can be mrpt::math::CQuaternionFloat or mrpt::math::CQuaternionDouble.
00094                           */
00095                         template <class QUATERNION,class MATRIXLIKE>
00096                         inline void jacob_quat_rotation(const QUATERNION& quaternion, MATRIXLIKE &out_mat4x4)
00097                         {
00098                                 quaternion.rotationJacobian(out_mat4x4);
00099                         }
00100 
00101                         /** 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$.
00102                           * For the equations, see CPose3DPDFGaussian::jacobiansPoseComposition
00103                           */
00104                         inline void jacobs_6D_pose_comp(
00105                                 const CPose3D   &x,
00106                                 const CPose3D   &u,
00107                                 CMatrixDouble66         &out_df_dx,
00108                                 CMatrixDouble66         &out_df_du)
00109                         {
00110                                 CPose3DPDFGaussian::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
00111                         }
00112 
00113                         /** 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$.
00114                           * For the equations, see CPosePDFGaussian::jacobiansPoseComposition
00115                           */
00116                         inline void jacobs_2D_pose_comp(
00117                                 const CPosePDFGaussian &x,
00118                                 const CPosePDFGaussian &u,
00119                                 CMatrixDouble33                  &out_df_dx,
00120                                 CMatrixDouble33                  &out_df_du)
00121                         {
00122                                 CPosePDFGaussian::jacobiansPoseComposition(x,u,out_df_dx,out_df_du);
00123                         }
00124 
00125                         /** Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation. */
00126                         template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
00127                         inline void jacob_numeric_estimate(
00128                                 const VECTORLIKE        &x,
00129                                 void                            (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3  &out),
00130                                 const VECTORLIKE2       &increments,
00131                                 const USERPARAM         &userParam,
00132                                 MATRIXLIKE                      &out_Jacobian )
00133                         {
00134                                 mrpt::math::estimateJacobian(x,functor,increments,userParam,out_Jacobian);
00135                         }
00136 
00137 
00138                 } // End of jacobians namespace
00139 
00140         } // End of MATH namespace
00141 
00142 } // End of namespace
00143 
00144 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011