pose2d_eigen2.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_GEOMETRY_POSE2D_EIGEN2_HPP_
#define ECL_GEOMETRY_POSE2D_EIGEN2_HPP_

/*****************************************************************************
** Include
*****************************************************************************/

#include <ecl/config/macros.hpp>
#include <ecl/formatters.hpp>
#include <ecl/linear_algebra.hpp>
#include <ecl/math/constants.hpp>
#include <ecl/exceptions/standard_exception.hpp>
#include <ecl/mpl/enable_if.hpp>
#include <ecl/type_traits/traits.hpp>
#include <ecl/type_traits/fundamental_types.hpp>
#include "angle.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Enums
*****************************************************************************/
enum Pose2DStorageType {
        RotationAngleStorage, 
        RotationMatrixStorage,
};

/*****************************************************************************
** Forward declarations
*****************************************************************************/

template <typename Float, enum Pose2DStorageType Storage, typename Enable> class Pose2D;

/*****************************************************************************
** Traits
*****************************************************************************/
template <typename Float, enum Pose2DStorageType Storage, typename Enable>
class ecl_traits< Pose2D<Float, Storage, Enable> > {};

template <typename Float, typename Enable>
class ecl_traits< Pose2D<Float, RotationMatrixStorage, Enable> > {
public:
        typedef Float Scalar; 
        typedef ecl::linear_algebra::Matrix<Float,2,2> RotationType;  
};

template <typename Float, typename Enable>
class ecl_traits< Pose2D<Float, RotationAngleStorage, Enable> > {
public:
        typedef Float Scalar; 
        typedef Angle<Float> RotationType;  
};

namespace geometry {

/*****************************************************************************
** Operations
*****************************************************************************/
template <typename Float, enum Pose2DStorageType Storage> class Pose2DMath {};
template <typename Float>
class ECL_LOCAL Pose2DMath<Float,RotationMatrixStorage> {
public:
        typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
        static RotationMatrixType Identity() { return RotationMatrixType::Identity(); } 
        static RotationMatrixType Rotation(const RotationMatrixType &rotation) { return rotation; } 
        static RotationMatrixType Rotation(const Angle<Float>& angle) { return angle.rotationMatrix(); }  
        static Angle<Float> Heading(const RotationMatrixType &rotation) { return Angle<Float>(rotation); }  
        static RotationMatrixType Product(const RotationMatrixType &rot1, const RotationMatrixType &rot2) { return rot1*rot2; } 
        static RotationMatrixType Product(const RotationMatrixType &rotation, const Angle<Float> &angle) { return rotation*angle.rotationMatrix(); } 
        static RotationMatrixType Inverse(const RotationMatrixType &rotation) { return rotation.transpose(); } 
};

template <typename Float>
class ECL_LOCAL Pose2DMath<Float,RotationAngleStorage> {
public:
        typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType; 
        static Angle<Float> Identity() { return Angle<Float>(0.0); } 
        static Angle<Float> Rotation(const Angle<Float>& radians) { return Angle<Float>(radians); }  
        static Angle<Float> Rotation(const RotationMatrixType &rotation) { return Pose2DMath<Float,RotationMatrixStorage>::Heading(rotation); } 
        static Angle<Float> Heading(const Angle<Float> &angle) { return angle; }  
        static Angle<Float> Product(const Angle<Float> &angle1, const Angle<Float> &angle2) { return angle1+angle2; } 
        static Angle<Float> Product(const Angle<Float> &angle, RotationMatrixType &rotation) { return angle + Angle<Float>(rotation); } 
        static Angle<Float> Inverse(const Angle<Float> &angle) { return Angle<Float>(-1*angle); } 
};

} // namespace geometry

/*****************************************************************************
** Pose2D
*****************************************************************************/
template <class Float, enum Pose2DStorageType Storage = RotationMatrixStorage, typename Enable = void>
class ECL_LOCAL Pose2D {
        typedef Float Scalar;
private:
        Pose2D() {}; 
};

template<typename Float, enum Pose2DStorageType Storage>
class ECL_PUBLIC Pose2D<Float, Storage, typename enable_if<is_float<Float> >::type> {
public:
        /******************************************
        ** Eigen Alignment
        *******************************************/
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html

        /******************************************
        ** Typedef
        *******************************************/
        typedef Float Scalar; 
        typedef geometry::Pose2DMath<Float,Storage> RotationMath; 
        typedef geometry::Pose2DMath<Float,RotationMatrixStorage> RotationMatrixMath; 
        typedef geometry::Pose2DMath<Float,RotationAngleStorage> RotationAngleMath; 
        typedef typename ecl_traits< Pose2D<Float,Storage,typename enable_if<is_float<Float> >::type> >::RotationType RotationType;  
        typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrix; 
        typedef ecl::linear_algebra::Matrix<Float,2,1> Translation;  
        /******************************************
        ** Constructors
        *******************************************/
        Pose2D() : rot( RotationMath::Identity()), trans(Translation::Zero()) {}

        Pose2D(const Float &x, const Float &y, const Angle<Float> &angle) :
                rot( RotationMath::Rotation(angle)),
                trans( (Translation() << x,y).finished() )
        {}

        template<typename Rot, typename Trans>
        Pose2D(const ecl::linear_algebra::MatrixBase<Rot>& R, const ecl::linear_algebra::MatrixBase<Trans>& T) :
                rot( RotationMath::Rotation(R) ),
                trans(T)
        {}
        template<typename Trans>
        Pose2D(const Angle<Float>& angle, const ecl::linear_algebra::MatrixBase<Trans>& T) :
                rot( RotationMath::Rotation(angle) ),
                trans(T)
        {}
        template <enum Pose2DStorageType Storage_>
        Pose2D(const Pose2D<Float,Storage_>& pose) :
                rot(RotationMath::Rotation(pose.rotation())),
                trans(pose.translation())
        {}

        virtual ~Pose2D() {}

        /******************************************
        ** Assignment
        *******************************************/
        template <enum Pose2DStorageType Storage_>
        Pose2D<Float,Storage>& operator=(const Pose2D<Float,Storage_>& pose) {
                trans = pose.translation();
                rot = RotationMath::Rotation(pose.rotation());
                return *this;
        }

        /******************************************
        ** Eigen Style Setters
        *******************************************/
        void rotation(const Angle<Float> &heading) {
                rot = RotationMath::Rotation(heading);
        }
        void rotation(const RotationMatrix &rotation_matrix) {
                rot = RotationMath::Rotation(rotation_matrix);
        }
        void translation(const Float &x, const Float &y) {
                this->trans << x, y;
        }
        template <typename Trans>
        void translation(const ecl::linear_algebra::MatrixBase<Trans>& T) {
                this->trans = T;
        }

        void setIdentity() {
                rot = RotationMath::Identity();
                trans << 0.0, 0.0;
        }

        static Pose2D<Float,Storage> Identity() {
                return Pose2D<Float,Storage>();
        }


        /******************************************
        ** Convenience Setters
        *******************************************/
        void setPose(const Float& x, const Float& y, const Angle<Float>& heading) { trans << x,y; rot = RotationMath::Rotation(heading); } 
        void x(const Float& value) { trans[0] = value; }        
        void y(const Float& value) { trans[1] = value; }        
        void heading(const Angle<Float>& value) { rot = RotationMath::Rotation(value); } 

        template <typename Rot>
        void rotationMatrix(const ecl::linear_algebra::MatrixBase<Rot>& rotation_matrix) {
                rot = RotationMath::Rotation(rotation_matrix);
        }

        /******************************************
        ** Eigen Style Accessors
        *******************************************/
        RotationType& rotation() { return rot; } 
        Translation& translation() { return trans; } 
        const RotationType& rotation() const { return rot; } 
        const Translation& translation() const { return trans; }  
        /******************************************
        ** Convenience Accessors
        *******************************************/
        Float x() const { return trans[0]; }            
        Float y() const { return trans[1]; }            
        Angle<Float> heading() const {  return RotationMath::Heading(rot); }    
        RotationMatrix rotationMatrix() const { return RotationMatrixMath::Rotation(rot); }

        /******************************************
        ** Operators
        *******************************************/
    Pose2D<Float,Storage> inverse() const {
        Pose2D<Float,Storage> inverse;
        inverse.rotation(RotationMath::Inverse(rot));
        inverse.translation(-1*(RotationMatrixMath::Rotation(inverse.rot)*trans));
        return inverse;
    }

        template <enum Pose2DStorageType Storage_>
    Pose2D<Float,Storage> operator*(const Pose2D<Float,Storage_> &pose) const {
                return Pose2D<Float,Storage>(RotationMath::Product(rot,pose.rot),trans + rotationMatrix()*pose.trans);
    }
    Pose2D<Float>& operator*=(const Pose2D<Float> &pose) {
        // This probably needs checking...could also be (marginally) sped up.
        *this = (*this)*pose;
        return (*this);
    }
        template <enum Pose2DStorageType Storage_>
    Pose2D<Float,Storage> relative(const Pose2D<Float,Storage_> &pose) const {
                return pose.inverse()*(*this);
    }

    /*********************
        ** Streaming
        **********************/
        template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
        friend OutputStream& operator<<(OutputStream &ostream , const Pose2D<Float_, Storage_> &pose);

private:
        RotationType rot;
        Translation trans;
};

/*****************************************************************************
** Insertion Operators
*****************************************************************************/
template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
OutputStream& operator<<(OutputStream &ostream , const Pose2D<Float_,Storage_> &pose) {
        ecl::Format<Float_> format;
        format.width(6);
        format.precision(3);
        ostream << "[ ";
        ostream << format(pose.x()) << " ";
        ostream << format(pose.y()) << " ";
        ostream << format(pose.heading());
        ostream << " ]";
        ostream.flush();
  return ostream;
}

} // namespace ecl


#endif /* ECL_GEOMETRY_POSE2D_EIGEN2_HPP_ */