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_SE3_TRAITS_H 00029 #define MRPT_SE3_TRAITS_H 00030 00031 #include <mrpt/poses/CPose3D.h> 00032 #include <mrpt/poses/CPose2D.h> 00033 #include <mrpt/math/CMatrixFixedNumeric.h> 00034 00035 namespace mrpt 00036 { 00037 namespace poses 00038 { 00039 /** \addtogroup poses_grp 00040 * @{ */ 00041 00042 /** A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobians, etc. 00043 * \sa SE_traits<2>, SE_traits<3>, CPose3D, CPose2D 00044 */ 00045 template <size_t DOF> struct BASE_IMPEXP SE_traits; 00046 00047 /** Specialization of SE for 3D poses \sa SE_traits */ 00048 template <> struct BASE_IMPEXP SE_traits<3> 00049 { 00050 enum { VECTOR_SIZE = 6 }; 00051 typedef CArrayDouble<VECTOR_SIZE> array_t; 00052 typedef CMatrixFixedNumeric<double,VECTOR_SIZE,VECTOR_SIZE> matrix_VxV_t; 00053 typedef CPose3D pose_t; 00054 00055 /** Exponential map in SE(3) */ 00056 static inline void exp(const array_t &x, CPose3D &P) { P = CPose3D::exp(x); } 00057 00058 /** Logarithm map in SE(3) */ 00059 static inline void ln(const CPose3D &P, array_t &x) { P.ln(x); } 00060 00061 /** A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal 00062 * SO(3) logarithm is used for the rotation components, but the translation is left unmodified. 00063 */ 00064 static void pseudo_ln(const CPose3D &P, array_t &x); 00065 00066 /** Return one or both of the following 6x6 Jacobians, useful in graph-slam problems: 00067 * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \f] 00068 * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \f] 00069 * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2. 00070 */ 00071 static void jacobian_dP1DP2inv_depsilon( 00072 const CPose3D &P1DP2inv, 00073 matrix_VxV_t *df_de1, 00074 matrix_VxV_t *df_de2); 00075 00076 }; // end SE_traits 00077 00078 /** Specialization of SE for 2D poses \sa SE_traits */ 00079 template <> struct BASE_IMPEXP SE_traits<2> 00080 { 00081 enum { VECTOR_SIZE = 3 }; 00082 typedef CArrayDouble<VECTOR_SIZE> array_t; 00083 typedef CMatrixFixedNumeric<double,VECTOR_SIZE,VECTOR_SIZE> matrix_VxV_t; 00084 typedef CPose2D pose_t; 00085 00086 /** Exponential map in SE(2) */ 00087 static inline void exp(const array_t &x, CPose2D &P) { P.x(x[0]); P.y(x[1]); P.phi(x[2]); } 00088 00089 /** Logarithm map in SE(2) */ 00090 static inline void ln(const CPose2D &P, array_t &x) { x[0] = P.x(); x[1] = P.y(); x[2] = P.phi(); } 00091 00092 /** A pseudo-Logarithm map in SE(2), where the output = [X,Y, Ln(ROT)], that is, the normal 00093 * SO(2) logarithm is used for the rotation components, but the translation is left unmodified. 00094 */ 00095 static inline void pseudo_ln(const CPose2D &P, array_t &x) { ln(P,x); } 00096 00097 /** Return one or both of the following 3x3 Jacobians, useful in graph-slam problems: 00098 * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \f] 00099 * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \f] 00100 * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2. 00101 */ 00102 static void jacobian_dP1DP2inv_depsilon( 00103 const CPose2D &P1DP2inv, 00104 matrix_VxV_t *df_de1, 00105 matrix_VxV_t *df_de2); 00106 00107 }; // end SE_traits 00108 00109 /** @} */ // end of grouping 00110 00111 } // End of namespace 00112 } // End of namespace 00113 00114 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |