00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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/CPosePDF.h> 00035 #include <mrpt/poses/CPose3DQuatPDF.h> 00036 #include <mrpt/poses/CPose3DPDF.h> 00037 00038 namespace mrpt 00039 { 00040 namespace math 00041 { 00042 /** 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). 00043 * 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. 00044 * \ingroup mrpt_base_grp 00045 */ 00046 namespace jacobians 00047 { 00048 using namespace mrpt::utils; 00049 using namespace mrpt::poses; 00050 using namespace mrpt::math; 00051 00052 /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of: 00053 * \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] 00054 * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$. 00055 * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion 00056 */ 00057 inline void jacob_quat_from_yawpitchroll( 00058 CMatrixFixedNumeric<double,4,3> &out_dq_dr, 00059 const double yaw, 00060 const double pitch, 00061 const double roll 00062 ) 00063 { 00064 CQuaternionDouble q(UNINITIALIZED_QUATERNION); 00065 CPose3D p(0,0,0,yaw,pitch,roll); 00066 p.getAsQuaternion(q,&out_dq_dr); 00067 } 00068 00069 /** Computes the 4x3 Jacobian of the transformation from a 3D pose angles (yaw pitch roll) into a Quaternion, that is, the Jacobian of: 00070 * \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] 00071 * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$. 00072 * \sa jacob_yawpitchroll_from_quat, mrpt::poses::CPose3D::getAsQuaternion 00073 */ 00074 inline void jacob_quat_from_yawpitchroll( 00075 CMatrixFixedNumeric<double,4,3> &out_dq_dr, 00076 const CPose3D &in_pose 00077 ) 00078 { 00079 CQuaternionDouble q(UNINITIALIZED_QUATERNION); 00080 in_pose.getAsQuaternion(q,&out_dq_dr); 00081 } 00082 00083 00084 /** Computes the 3x4 Jacobian of the transformation from a quaternion (qr qx qy qz) to 3D pose angles (yaw pitch roll). 00085 * \sa jacob_quat_from_yawpitchroll 00086 */ 00087 inline void jacob_yawpitchroll_from_quat( 00088 CMatrixFixedNumeric<double,3,4> &out_dr_dq 00089 ) 00090 { 00091 THROW_EXCEPTION("TO DO") 00092 } 00093 00094 /** 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$. 00095 * The output matrix can be a dynamic or fixed size (4x4) matrix. The input quaternion can be mrpt::math::CQuaternionFloat or mrpt::math::CQuaternionDouble. 00096 */ 00097 template <class QUATERNION,class MATRIXLIKE> 00098 inline void jacob_quat_rotation(const QUATERNION& quaternion, MATRIXLIKE &out_mat4x4) 00099 { 00100 quaternion.rotationJacobian(out_mat4x4); 00101 } 00102 00103 /** 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$. 00104 * For the equations, see CPose3DPDF::jacobiansPoseComposition 00105 */ 00106 inline void jacobs_6D_pose_comp( 00107 const CPose3D &x, 00108 const CPose3D &u, 00109 CMatrixDouble66 &out_df_dx, 00110 CMatrixDouble66 &out_df_du) 00111 { 00112 CPose3DPDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du); 00113 } 00114 00115 /** 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$. 00116 * For the equations, see CPose3DQuatPDF::jacobiansPoseComposition 00117 */ 00118 inline void jacobs_6D_pose_comp( 00119 const CPose3DQuat &x, 00120 const CPose3DQuat &u, 00121 CMatrixDouble77 &out_df_dx, 00122 CMatrixDouble77 &out_df_du) 00123 { 00124 CPose3DQuatPDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du); 00125 } 00126 00127 /** 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$. 00128 * For the equations, see CPosePDF::jacobiansPoseComposition 00129 */ 00130 inline void jacobs_2D_pose_comp( 00131 const CPosePDFGaussian &x, 00132 const CPosePDFGaussian &u, 00133 CMatrixDouble33 &out_df_dx, 00134 CMatrixDouble33 &out_df_du) 00135 { 00136 CPosePDF::jacobiansPoseComposition(x,u,out_df_dx,out_df_du); 00137 } 00138 00139 /** Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::math::estimateJacobian, see that function for documentation. */ 00140 template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM > 00141 inline void jacob_numeric_estimate( 00142 const VECTORLIKE &x, 00143 void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out), 00144 const VECTORLIKE2 &increments, 00145 const USERPARAM &userParam, 00146 MATRIXLIKE &out_Jacobian ) 00147 { 00148 mrpt::math::estimateJacobian(x,functor,increments,userParam,out_Jacobian); 00149 } 00150 00151 00152 } // End of jacobians namespace 00153 00154 } // End of MATH namespace 00155 00156 } // End of namespace 00157 00158 #endif
| Page generated by Doxygen 1.7.4 for MRPT 0.9.5 SVN:2717 at Sun Oct 16 16:08:03 PDT 2011 | | Hosted on: |