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 |