pose2d_eigen2.hpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Ifdefs
00012 *****************************************************************************/
00013 
00014 #ifndef ECL_GEOMETRY_POSE2D_EIGEN2_HPP_
00015 #define ECL_GEOMETRY_POSE2D_EIGEN2_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_LOCAL 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_LOCAL 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 ** Pose2D
00137 *****************************************************************************/
00143 template <class Float, enum Pose2DStorageType Storage = RotationMatrixStorage, typename Enable = void>
00144 class ECL_LOCAL Pose2D {
00145         typedef Float Scalar;
00146 private:
00147         Pose2D() {}; 
00148 };
00149 
00166 template<typename Float, enum Pose2DStorageType Storage>
00167 class ECL_PUBLIC Pose2D<Float, Storage, typename enable_if<is_float<Float> >::type> {
00168 public:
00169         /******************************************
00170         ** Eigen Alignment
00171         *******************************************/
00172         EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00173 
00174         /******************************************
00175         ** Typedef
00176         *******************************************/
00177         typedef Float Scalar; 
00178         typedef geometry::Pose2DMath<Float,Storage> RotationMath; 
00179         typedef geometry::Pose2DMath<Float,RotationMatrixStorage> RotationMatrixMath; 
00180         typedef geometry::Pose2DMath<Float,RotationAngleStorage> RotationAngleMath; 
00181         typedef typename ecl_traits< Pose2D<Float,Storage,typename enable_if<is_float<Float> >::type> >::RotationType RotationType;  
00182         typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrix; 
00183         typedef ecl::linear_algebra::Matrix<Float,2,1> Translation;  
00185         /******************************************
00186         ** Constructors
00187         *******************************************/
00196         Pose2D() : rot( RotationMath::Identity()), trans(Translation::Zero()) {}
00197 
00204         Pose2D(const Float &x, const Float &y, const Angle<Float> &angle) :
00205                 rot( RotationMath::Rotation(angle)),
00206                 trans( (Translation() << x,y).finished() )
00207         {}
00208 
00220         template<typename Rot, typename Trans>
00221         Pose2D(const ecl::linear_algebra::MatrixBase<Rot>& R, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00222                 rot( RotationMath::Rotation(R) ),
00223                 trans(T)
00224         {}
00235         template<typename Trans>
00236         Pose2D(const Angle<Float>& angle, const ecl::linear_algebra::MatrixBase<Trans>& T) :
00237                 rot( RotationMath::Rotation(angle) ),
00238                 trans(T)
00239         {}
00247         template <enum Pose2DStorageType Storage_>
00248         Pose2D(const Pose2D<Float,Storage_>& pose) :
00249                 rot(RotationMath::Rotation(pose.rotation())),
00250                 trans(pose.translation())
00251         {}
00252 
00253         virtual ~Pose2D() {}
00254 
00255         /******************************************
00256         ** Assignment
00257         *******************************************/
00263         template <enum Pose2DStorageType Storage_>
00264         Pose2D<Float,Storage>& operator=(const Pose2D<Float,Storage_>& pose) {
00265                 trans = pose.translation();
00266                 rot = RotationMath::Rotation(pose.rotation());
00267                 return *this;
00268         }
00269 
00270         /******************************************
00271         ** Eigen Style Setters
00272         *******************************************/
00281         void rotation(const Angle<Float> &heading) {
00282                 rot = RotationMath::Rotation(heading);
00283         }
00292         void rotation(const RotationMatrix &rotation_matrix) {
00293                 rot = RotationMath::Rotation(rotation_matrix);
00294         }
00303         void translation(const Float &x, const Float &y) {
00304                 this->trans << x, y;
00305         }
00315         template <typename Trans>
00316         void translation(const ecl::linear_algebra::MatrixBase<Trans>& T) {
00317                 this->trans = T;
00318         }
00319 
00323         void setIdentity() {
00324                 rot = RotationMath::Identity();
00325                 trans << 0.0, 0.0;
00326         }
00327 
00333         static Pose2D<Float,Storage> Identity() {
00334                 return Pose2D<Float,Storage>();
00335         }
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); }
00375 
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 } // namespace ecl
00469 
00470 
00471 #endif /* ECL_GEOMETRY_POSE2D_EIGEN2_HPP_ */


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Wed Aug 26 2015 11:27:46