ecl_geometry: pose2d_eigen3.hpp Source File

Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Ifdefs
00012 *****************************************************************************/
00013 
00014 #ifndef ECL_GEOMETRY_POSE2D_EIGEN3_HPP_
00015 #define ECL_GEOMETRY_POSE2D_EIGEN3_HPP_
00016 
00017 /*****************************************************************************
00018 ** Include
00019 *****************************************************************************/
00020 
00021 #include <ecl/config/macros.hpp>
00022 #include <ecl/formatters.hpp>
00023 #include <ecl/linear_algebra.hpp>
00024 #include <ecl/math/constants.hpp>
00025 #include <ecl/exceptions/standard_exception.hpp>
00026 #include <ecl/mpl/enable_if.hpp>
00027 #include <ecl/type_traits/traits.hpp>
00028 #include <ecl/type_traits/fundamental_types.hpp>
00029 #include "angle.hpp"
00030 
00031 /*****************************************************************************
00032 ** Namespaces
00033 *****************************************************************************/
00034 
00035 namespace ecl {
00036 
00037 /*****************************************************************************
00038 ** Enums
00039 *****************************************************************************/
00043 enum Pose2DStorageType {
00044         RotationAngleStorage, 
00045         RotationMatrixStorage,
00046 };
00047 
00048 /*****************************************************************************
00049 ** Forward declarations
00050 *****************************************************************************/
00051 
00052 template <typename Float, enum Pose2DStorageType Storage, typename Enable> class Pose2D;
00053 
00054 /*****************************************************************************
00055 ** Traits
00056 *****************************************************************************/
00060 template <typename Float, enum Pose2DStorageType Storage, typename Enable>
00061 class ecl_traits< Pose2D<Float, Storage, Enable> > {};
00062 
00066 template <typename Float, typename Enable>
00067 class ecl_traits< Pose2D<Float, RotationMatrixStorage, Enable> > {
00068 public:
00069         typedef Float Scalar; 
00070         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationType;  
00071 };
00072 
00076 template <typename Float, typename Enable>
00077 class ecl_traits< Pose2D<Float, RotationAngleStorage, Enable> > {
00078 public:
00079         typedef Float Scalar; 
00080         typedef Angle<Float> RotationType;  
00081 };
00082 
00083 namespace geometry {
00084 
00085 /*****************************************************************************
00086 ** Operations
00087 *****************************************************************************/
00091 template <typename Float, enum Pose2DStorageType Storage> class Pose2DMath {};
00099 template <typename Float>
00100 class ECL_PUBLIC Pose2DMath<Float,RotationMatrixStorage> {
00101 public:
00102         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
00104         static RotationMatrixType Identity() { return RotationMatrixType::Identity(); } 
00105         static RotationMatrixType Rotation(const RotationMatrixType &rotation) { return rotation; } 
00106         static RotationMatrixType Rotation(const Angle<Float>& angle) { return angle.rotationMatrix(); }  
00107         static Angle<Float> Heading(const RotationMatrixType &rotation) { return Angle<Float>(rotation); }  
00108         static RotationMatrixType Product(const RotationMatrixType &rot1, const RotationMatrixType &rot2) { return rot1*rot2; } 
00109         static RotationMatrixType Product(const RotationMatrixType &rotation, const Angle<Float> &angle) { return rotation*angle.rotationMatrix(); } 
00110         static RotationMatrixType Inverse(const RotationMatrixType &rotation) { return rotation.transpose(); } 
00111 };
00112 
00120 template <typename Float>
00121 class ECL_PUBLIC Pose2DMath<Float,RotationAngleStorage> {
00122 public:
00123         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
00124         static Angle<Float> Identity() { return Angle<Float>(0.0); } 
00125         static Angle<Float> Rotation(const Angle<Float>& radians) { return Angle<Float>(radians); }  
00126         static Angle<Float> Rotation(const RotationMatrixType &rotation) { return Pose2DMath<Float,RotationMatrixStorage>::Heading(rotation); } 
00127         static Angle<Float> Heading(const Angle<Float> &angle) { return angle; }  
00128         static Angle<Float> Product(const Angle<Float> &angle1, const Angle<Float> &angle2) { return angle1+angle2; } 
00129         static Angle<Float> Product(const Angle<Float> &angle, RotationMatrixType &rotation) { return angle + Angle<Float>(rotation); } 
00130         static Angle<Float> Inverse(const Angle<Float> &angle) { return Angle<Float>(-1*angle); } 
00131 };
00132 
00133 } // namespace geometry
00134 
00135 
00136 /*****************************************************************************
00137 ** Pose2D
00138 *****************************************************************************/
00144 template <class Float, enum Pose2DStorageType Storage = RotationMatrixStorage, typename Enable = void>
00145 class ECL_PUBLIC Pose2D {
00146         typedef Float Scalar;
00147 private:
00148         Pose2D() {}; 
00149 };
00150 
00167 template<typename Float, enum Pose2DStorageType Storage>
00168 class ECL_PUBLIC Pose2D<Float, Storage, typename enable_if<is_float<Float> >::type> {
00169 public:
00170         /******************************************
00171         ** Eigen Alignment
00172         *******************************************/
00173         EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00174 
00175         /******************************************
00176         ** Typedef
00177         *******************************************/
00178         typedef Float Scalar; 
00179         typedef geometry::Pose2DMath<Float,Storage> RotationMath; 
00180         typedef geometry::Pose2DMath<Float,RotationMatrixStorage> RotationMatrixMath; 
00181         typedef geometry::Pose2DMath<Float,RotationAngleStorage> RotationAngleMath; 
00182         typedef typename ecl_traits< Pose2D<Float,Storage,typename enable_if<is_float<Float> >::type> >::RotationType RotationType;  
00183         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrix; 
00184         typedef ecl::linear_algebra::Matrix<Float,2,1> Translation;  
00186         /******************************************
00187         ** Constructors
00188         *******************************************/
00197         Pose2D() : rot( RotationMath::Identity()), trans(Translation::Zero()) {}
00198 
00205         Pose2D(const Float &x, const Float &y, const Angle<Float> &angle) :
00206                 rot( RotationMath::Rotation(angle)),
00207                 trans( (Translation() << x,y).finished() )
00208         {}
00209 
00221         template<typename Rot, typename Trans>
00222         Pose2D(const ecl::linear_algebra::MatrixBase<Rot>& R, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00223                 rot( RotationMath::Rotation(R) ),
00224                 trans(T)
00225         {}
00236         template<typename Trans>
00237         Pose2D(const Angle<Float>& angle, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00238                 rot( RotationMath::Rotation(angle) ),
00239                 trans(T)
00240         {}
00248         template <enum Pose2DStorageType Storage_>
00249         Pose2D(const Pose2D<Float,Storage_>& pose) :
00250                 rot(RotationMath::Rotation(pose.rotation())),
00251                 trans(pose.translation())
00252         {}
00253 
00254         virtual ~Pose2D() {}
00255 
00256         /******************************************
00257         ** Assignment
00258         *******************************************/
00264         template <enum Pose2DStorageType Storage_>
00265         Pose2D<Float,Storage>& operator=(const Pose2D<Float,Storage_>& pose) {
00266                 trans = pose.translation();
00267                 rot = RotationMath::Rotation(pose.rotation());
00268                 return *this;
00269         }
00270 
00271         /******************************************
00272         ** Eigen Style Setters
00273         *******************************************/
00282         void rotation(const Angle<Float> &heading) {
00283                 rot = RotationMath::Rotation(heading);
00284         }
00293         void rotation(const RotationMatrix &rotation_matrix) {
00294                 rot = RotationMath::Rotation(rotation_matrix);
00295         }
00304         void translation(const Float &x, const Float &y) {
00305                 this->trans << x, y;
00306         }
00316         template <typename Trans>
00317         void translation(const ecl::linear_algebra::MatrixBase<Trans>& T) {
00318                 this->trans = T;
00319         }
00320 
00324         void setIdentity() {
00325                 rot = RotationMath::Identity();
00326                 trans << 0.0, 0.0;
00327         }
00328 
00334         static Pose2D<Float,Storage> Identity() {
00335                 return Pose2D<Float,Storage>();
00336         }
00337 
00338         /******************************************
00339         ** Convenience Setters
00340         *******************************************/
00341         void setPose(const Float& x, const Float& y, const Angle<Float>& heading) { trans << x,y; rot = RotationMath::Rotation(heading); } 
00342         void x(const Float& value) { trans[0] = value; }        
00343         void y(const Float& value) { trans[1] = value; }        
00344         void heading(const Angle<Float>& value) { rot = RotationMath::Rotation(value); } 
00345 
00355         template <typename Rot>
00356         void rotationMatrix(const ecl::linear_algebra::MatrixBase<Rot>& rotation_matrix) {
00357                 rot = RotationMath::Rotation(rotation_matrix);
00358         }
00359 
00360         /******************************************
00361         ** Eigen Style Accessors
00362         *******************************************/
00363         RotationType& rotation() { return rot; } 
00364         Translation& translation() { return trans; } 
00365         const RotationType& rotation() const { return rot; } 
00366         const Translation& translation() const { return trans; }  
00368         /******************************************
00369         ** Convenience Accessors
00370         *******************************************/
00371         Float x() const { return trans[0]; }            
00372         Float y() const { return trans[1]; }            
00373         Angle<Float> heading() const {  return RotationMath::Heading(rot); }    
00374         RotationMatrix rotationMatrix() const { return RotationMatrixMath::Rotation(rot); } 
00376         /******************************************
00377         ** Operators
00378         *******************************************/
00386     Pose2D<Float,Storage> inverse() const {
00387         Pose2D<Float,Storage> inverse;
00388         inverse.rotation(RotationMath::Inverse(rot));
00389         inverse.translation(-1*(RotationMatrixMath::Rotation(inverse.rot)*trans));
00390         return inverse;
00391     }
00392 
00398         template <enum Pose2DStorageType Storage_>
00399     Pose2D<Float,Storage> operator*(const Pose2D<Float,Storage_> &pose) const {
00400                 return Pose2D<Float,Storage>(RotationMath::Product(rot,pose.rot),trans + rotationMatrix()*pose.trans);
00401     }
00407     Pose2D<Float>& operator*=(const Pose2D<Float> &pose) {
00408         // This probably needs checking...could also be (marginally) sped up.
00409         *this = (*this)*pose;
00410         return (*this);
00411     }
00426         template <enum Pose2DStorageType Storage_>
00427     Pose2D<Float,Storage> relative(const Pose2D<Float,Storage_> &pose) const {
00428                 return pose.inverse()*(*this);
00429     }
00430 
00431     /*********************
00432         ** Streaming
00433         **********************/
00434         template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
00435         friend OutputStream& operator<<(OutputStream &ostream , const Pose2D<Float_, Storage_> &pose);
00436 
00437 private:
00438         RotationType rot;
00439         Translation trans;
00440 };
00441 
00442 /*****************************************************************************
00443 ** Insertion Operators
00444 *****************************************************************************/
00454 template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
00455 OutputStream& operator<<(OutputStream &ostream , const Pose2D<Float_,Storage_> &pose) {
00456         ecl::Format<Float_> format;
00457         format.width(6);
00458         format.precision(3);
00459         ostream << "[ ";
00460         ostream << format(pose.x()) << " ";
00461         ostream << format(pose.y()) << " ";
00462         ostream << format(pose.heading());
00463         ostream << " ]";
00464         ostream.flush();
00465   return ostream;
00466 }
00467 
00468 
00469 } // namespace ecl
00470 
00471 // This is more convenient and less bughuntish than always assigning allocators with your vectors
00472 // but currently fails on oneiric with gcc 4.6 (bugfixed in eigen, but not yet out in oneiric).
00473 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose2D<float,ecl::RotationAngleStorage>)
00474 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose2D<float,ecl::RotationMatrixStorage>)
00475 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose2D<double,ecl::RotationAngleStorage>)
00476 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose2D<double,ecl::RotationMatrixStorage>)
00477 
00478 
00479 #endif /* ECL_GEOMETRY_POSE2D_EIGEN3_HPP_ */