ecl_geometry: legacy_pose2d.hpp Source File

Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Ifdefs
00012 *****************************************************************************/
00013 
00014 #ifndef ECL_GEOMETRY_LEGACY_POSE2D_HPP_
00015 #define ECL_GEOMETRY_LEGACY_POSE2D_HPP_
00016 
00017 /*****************************************************************************
00018 ** Includes
00019 *****************************************************************************/
00020 
00021 #include <ecl/linear_algebra.hpp>
00022 
00023 #include <ecl/config/macros.hpp>
00024 #include <ecl/formatters.hpp>
00025 #include <ecl/math/constants.hpp>
00026 #include <ecl/exceptions/standard_exception.hpp>
00027 #include <ecl/mpl/enable_if.hpp>
00028 #include <ecl/type_traits/traits.hpp>
00029 #include <ecl/type_traits/fundamental_types.hpp>
00030 #include "angle.hpp"
00031 
00032 /*****************************************************************************
00033 ** Enums
00034 *****************************************************************************/
00035 
00036 namespace ecl {
00037 
00041 enum Pose2DStorageType {
00042         RotationAngleStorage, 
00043         RotationMatrixStorage,
00044 };
00045 
00046 } // namespace ecl
00047 
00048 /*****************************************************************************
00049 ** Forward declarations
00050 *****************************************************************************/
00051 
00052 namespace ecl {
00053 
00054 template <typename Float, enum Pose2DStorageType Storage, typename Enable> class LegacyPose2D;
00055 
00056 } // namespace ecl
00057 
00058 /*****************************************************************************
00059 ** Traits
00060 *****************************************************************************/
00061 
00062 namespace ecl {
00063 
00067 template <typename Float, enum Pose2DStorageType Storage, typename Enable>
00068 class ecl_traits< LegacyPose2D<Float, Storage, Enable> > {};
00069 
00073 template <typename Float, typename Enable>
00074 class ecl_traits< LegacyPose2D<Float, RotationMatrixStorage, Enable> > {
00075 public:
00076         typedef Float Scalar; 
00077         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationType;  
00078 };
00079 
00083 template <typename Float, typename Enable>
00084 class ecl_traits< LegacyPose2D<Float, RotationAngleStorage, Enable> > {
00085 public:
00086         typedef Float Scalar; 
00087         typedef Angle<Float> RotationType;  
00088 };
00089 
00090 } // namespace ecl
00091 
00092 /*****************************************************************************
00093 ** Operations
00094 *****************************************************************************/
00095 
00096 namespace ecl {
00097 namespace geometry {
00098 
00102 template <typename Float, enum Pose2DStorageType Storage> class Pose2DMath {};
00110 template <typename Float>
00111 class ECL_PUBLIC Pose2DMath<Float,RotationMatrixStorage> {
00112 public:
00113         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
00115         static RotationMatrixType Identity() { return RotationMatrixType::Identity(); } 
00116         static RotationMatrixType Rotation(const RotationMatrixType &rotation) { return rotation; } 
00117         static RotationMatrixType Rotation(const Angle<Float>& angle) { return angle.rotationMatrix(); }  
00118         static Angle<Float> Heading(const RotationMatrixType &rotation) { return Angle<Float>(rotation); }  
00119         static RotationMatrixType Product(const RotationMatrixType &rot1, const RotationMatrixType &rot2) { return rot1*rot2; } 
00120         static RotationMatrixType Product(const RotationMatrixType &rotation, const Angle<Float> &angle) { return rotation*angle.rotationMatrix(); } 
00121         static RotationMatrixType Inverse(const RotationMatrixType &rotation) { return rotation.transpose(); } 
00122 };
00123 
00131 template <typename Float>
00132 class ECL_PUBLIC Pose2DMath<Float,RotationAngleStorage> {
00133 public:
00134         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
00135         static Angle<Float> Identity() { return Angle<Float>(0.0); } 
00136         static Angle<Float> Rotation(const Angle<Float>& radians) { return Angle<Float>(radians); }  
00137         static Angle<Float> Rotation(const RotationMatrixType &rotation) { return Pose2DMath<Float,RotationMatrixStorage>::Heading(rotation); } 
00138         static Angle<Float> Heading(const Angle<Float> &angle) { return angle; }  
00139         static Angle<Float> Product(const Angle<Float> &angle1, const Angle<Float> &angle2) { return angle1+angle2; } 
00140         static Angle<Float> Product(const Angle<Float> &angle, RotationMatrixType &rotation) { return angle + Angle<Float>(rotation); } 
00141         static Angle<Float> Inverse(const Angle<Float> &angle) { return Angle<Float>(-1*angle); } 
00142 };
00143 
00144 } // namespace geometry
00145 } // namespace ecl
00146 
00147 
00148 /*****************************************************************************
00149 ** Pose2D
00150 *****************************************************************************/
00151 
00152 namespace ecl {
00153 
00159 template <class Float, enum Pose2DStorageType Storage = RotationMatrixStorage, typename Enable = void>
00160 class ECL_PUBLIC LegacyPose2D {
00161         typedef Float Scalar;
00162 private:
00163         LegacyPose2D() {}; 
00164 };
00165 
00182 template<typename Float, enum Pose2DStorageType Storage>
00183 class ECL_PUBLIC LegacyPose2D<Float, Storage, typename enable_if<is_float<Float> >::type> {
00184 public:
00185         /******************************************
00186         ** Eigen Alignment
00187         *******************************************/
00188         EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00189 
00190         /******************************************
00191         ** Typedef
00192         *******************************************/
00193         typedef Float Scalar; 
00194         typedef geometry::Pose2DMath<Float,Storage> RotationMath; 
00195         typedef geometry::Pose2DMath<Float,RotationMatrixStorage> RotationMatrixMath; 
00196         typedef geometry::Pose2DMath<Float,RotationAngleStorage> RotationAngleMath; 
00197         typedef typename ecl_traits< LegacyPose2D<Float,Storage,typename enable_if<is_float<Float> >::type> >::RotationType RotationType;  
00198         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrix; 
00199         typedef ecl::linear_algebra::Matrix<Float,2,1> Translation;  
00201         /******************************************
00202         ** Constructors
00203         *******************************************/
00212         LegacyPose2D() : rot( RotationMath::Identity()), trans(Translation::Zero()) {}
00213 
00220         LegacyPose2D(const Float &x, const Float &y, const Angle<Float> &angle) :
00221                 rot( RotationMath::Rotation(angle)),
00222                 trans( (Translation() << x,y).finished() )
00223         {}
00224 
00236         template<typename Rot, typename Trans>
00237         LegacyPose2D(const ecl::linear_algebra::MatrixBase<Rot>& R, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00238                 rot( RotationMath::Rotation(R) ),
00239                 trans(T)
00240         {}
00251         template<typename Trans>
00252         LegacyPose2D(const Angle<Float>& angle, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00253                 rot( RotationMath::Rotation(angle) ),
00254                 trans(T)
00255         {}
00263         template <enum Pose2DStorageType Storage_>
00264         LegacyPose2D(const LegacyPose2D<Float,Storage_>& pose) :
00265                 rot(RotationMath::Rotation(pose.rotation())),
00266                 trans(pose.translation())
00267         {}
00268 
00269         virtual ~LegacyPose2D() {}
00270 
00271         /******************************************
00272         ** Assignment
00273         *******************************************/
00279         template <enum Pose2DStorageType Storage_>
00280         LegacyPose2D<Float,Storage>& operator=(const LegacyPose2D<Float,Storage_>& pose) {
00281                 trans = pose.translation();
00282                 rot = RotationMath::Rotation(pose.rotation());
00283                 return *this;
00284         }
00285 
00286         /******************************************
00287         ** Eigen Style Setters
00288         *******************************************/
00297         void rotation(const Angle<Float> &heading) {
00298                 rot = RotationMath::Rotation(heading);
00299         }
00308         void rotation(const RotationMatrix &rotation_matrix) {
00309                 rot = RotationMath::Rotation(rotation_matrix);
00310         }
00319         void translation(const Float &x, const Float &y) {
00320                 this->trans << x, y;
00321         }
00331         template <typename Trans>
00332         void translation(const ecl::linear_algebra::MatrixBase<Trans>& T) {
00333                 this->trans = T;
00334         }
00335 
00339         void setIdentity() {
00340                 rot = RotationMath::Identity();
00341                 trans << 0.0, 0.0;
00342         }
00343 
00349         static LegacyPose2D<Float,Storage> Identity() {
00350                 return LegacyPose2D<Float,Storage>();
00351         }
00352 
00353         /******************************************
00354         ** Convenience Setters
00355         *******************************************/
00356         void setPose(const Float& x, const Float& y, const Angle<Float>& heading) { trans << x,y; rot = RotationMath::Rotation(heading); } 
00357         void x(const Float& value) { trans[0] = value; }        
00358         void y(const Float& value) { trans[1] = value; }        
00359         void heading(const Angle<Float>& value) { rot = RotationMath::Rotation(value); } 
00360 
00370         template <typename Rot>
00371         void rotationMatrix(const ecl::linear_algebra::MatrixBase<Rot>& rotation_matrix) {
00372                 rot = RotationMath::Rotation(rotation_matrix);
00373         }
00374 
00375         /******************************************
00376         ** Eigen Style Accessors
00377         *******************************************/
00378         RotationType& rotation() { return rot; } 
00379         Translation& translation() { return trans; } 
00380         const RotationType& rotation() const { return rot; } 
00381         const Translation& translation() const { return trans; }  
00383         /******************************************
00384         ** Convenience Accessors
00385         *******************************************/
00386         Float x() const { return trans[0]; }            
00387         Float y() const { return trans[1]; }            
00388         Angle<Float> heading() const {  return RotationMath::Heading(rot); }    
00389         RotationMatrix rotationMatrix() const { return RotationMatrixMath::Rotation(rot); } 
00391         /******************************************
00392         ** Operators
00393         *******************************************/
00401     LegacyPose2D<Float,Storage> inverse() const {
00402         LegacyPose2D<Float,Storage> inverse;
00403         inverse.rotation(RotationMath::Inverse(rot));
00404         inverse.translation(-1*(RotationMatrixMath::Rotation(inverse.rot)*trans));
00405         return inverse;
00406     }
00407 
00413         template <enum Pose2DStorageType Storage_>
00414     LegacyPose2D<Float,Storage> operator*(const LegacyPose2D<Float,Storage_> &pose) const {
00415                 return LegacyPose2D<Float,Storage>(RotationMath::Product(rot,pose.rot),trans + rotationMatrix()*pose.trans);
00416     }
00422     LegacyPose2D<Float>& operator*=(const LegacyPose2D<Float> &pose) {
00423         // This probably needs checking...could also be (marginally) sped up.
00424         *this = (*this)*pose;
00425         return (*this);
00426     }
00441         template <enum Pose2DStorageType Storage_>
00442     LegacyPose2D<Float,Storage> relative(const LegacyPose2D<Float,Storage_> &pose) const {
00443                 return pose.inverse()*(*this);
00444     }
00445 
00446     /*********************
00447         ** Streaming
00448         **********************/
00449         template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
00450         friend OutputStream& operator<<(OutputStream &ostream , const LegacyPose2D<Float_, Storage_> &pose);
00451 
00452 private:
00453         RotationType rot;
00454         Translation trans;
00455 };
00456 
00457 } // namespace ecl
00458 
00459 /*****************************************************************************
00460 ** Insertion Operators
00461 *****************************************************************************/
00462 
00463 namespace ecl {
00464 
00474 template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
00475 OutputStream& operator<<(OutputStream &ostream , const LegacyPose2D<Float_,Storage_> &pose) {
00476         ecl::Format<Float_> format;
00477         format.width(6);
00478         format.precision(3);
00479         ostream << "[ ";
00480         ostream << format(pose.x()) << " ";
00481         ostream << format(pose.y()) << " ";
00482         ostream << format(pose.heading());
00483         ostream << " ]";
00484         ostream.flush();
00485   return ostream;
00486 }
00487 
00488 
00489 } // namespace ecl
00490 
00491 // This is more convenient and less bughuntish than always assigning allocators with your vectors
00492 // but currently fails on oneiric with gcc 4.6 (bugfixed in eigen, but not yet out in oneiric).
00493 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<float,ecl::RotationAngleStorage>)
00494 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<float,ecl::RotationMatrixStorage>)
00495 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<double,ecl::RotationAngleStorage>)
00496 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<double,ecl::RotationMatrixStorage>)
00497 
00498 #endif /* ECL_GEOMETRY_LEGACY_POSE2D_HPP_ */