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