Main MRPT website > C++ reference
MRPT logo

SE_traits.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_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.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011