Pose.hpp

Go to the documentation of this file.

#ifndef __BASE_POSE_HH__
#define __BASE_POSE_HH__

#include "Eigen.hpp"
#include "Angle.hpp"

namespace base
{
    typedef base::Vector3d    Position;
    typedef base::Quaterniond Orientation;

    typedef base::Vector2d    Position2D;
    typedef double            Orientation2D;

    /*
     * Decomposes the orientation in euler angles (non-proper, Tait-Bryan angles)
     * so that this can be obtained by applying the following rotations in order:
     *
     *  rotation of a2 around x-axis
     *  rotation of a1 around y-axis
     *  rotation of a0 around z-axis
     *
     * assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2)
     *
     */
    base::Vector3d getEuler(const base::Orientation &orientation);

    /*
     * Decomposes the orientation in euler angles so that this can be
     * obtained by applying the following rotations in order:
     *
     *  rotation of a2 around x-axis
     *  rotation of a1 around y-axis
     *  rotation of a0 around z-axis
     *
     * assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2)
     *
     */
    base::Vector3d getEuler(const base::AngleAxisd &orientation);

    double getYaw(const base::Orientation& orientation);
    

    double getYaw(const base::AngleAxisd& orientation);

    double getPitch(const base::Orientation& orientation);

    double getPitch(const base::AngleAxisd& orientation);

    double getRoll(const base::Orientation& orientation);

    double getRoll(const base::AngleAxisd& orientation);

    base::Orientation removeYaw(const base::Orientation& orientation);

    base::Orientation removeYaw(const base::AngleAxisd& orientation);

    base::Orientation removePitch(const base::Orientation& orientation);

    base::Orientation removePitch(const base::AngleAxisd& orientation);

    base::Orientation removeRoll(const base::Orientation& orientation);

    base::Orientation removeRoll(const base::AngleAxisd& orientation);

    struct PoseUpdateThreshold
    {
        PoseUpdateThreshold();

        PoseUpdateThreshold( double _distance, double _angle );

        bool test( double other_distance, double other_angle );

        bool test( const Eigen::Affine3d& pdelta );

        bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b );

        double distance;
        double angle;
    };

    struct Pose
    {
        Position    position;
        Orientation orientation;

        Pose()
            : position(Position::Zero()), orientation(Orientation::Identity()) {}

        Pose(Position const& p, Orientation const& o)
            : position(p), orientation(o) {}

        Pose(const Eigen::Affine3d &t)
        {
            fromTransform( t );
        }

        Pose(const Vector6d &v)
        {
            fromVector6d( v );
        }

        void fromTransform( const Eigen::Affine3d &t )
        {
            position = t.translation();
            orientation = t.linear();
        }

        Eigen::Affine3d toTransform() const
        {
            Eigen::Affine3d t;
            t = orientation;
            t.pretranslate( position );
            return t;
        }

        void fromVector6d( const Vector6d &v );

        Vector6d toVector6d() const;

        double getYaw() const
        {
            return base::getYaw(orientation);
        }
    };

    std::ostream& operator << (std::ostream& io, base::Pose const& pose);

    struct Pose2D
    {
        Position2D    position;
        Orientation2D orientation;

        Pose2D()
            : position(Position2D::Zero()), orientation(0) {}

        Pose2D(Position2D const& p, Orientation2D const& o)
            : position(p), orientation(o) {}

        Pose2D(Position const& p, Orientation const& o)
            : position(Vector2d(p.x(), p.y())), orientation(base::getYaw(o)) {}

        Pose2D( const Pose &p)
            : position(Vector2d(p.position.x(), p.position.y())), orientation(p.getYaw()) {}

        bool isApprox(const Pose2D &other, double distPecision, double anglePrecision) const
        {
            return ((other.position - position).norm() < distPecision) && 
                   (Angle::fromRad(other.orientation).isApprox(Angle::fromRad(orientation), anglePrecision));
        }

    };

    std::ostream& operator << (std::ostream& io, base::Pose2D const& pose);
}

#endif