pose2d.h

Go to the documentation of this file.

#ifndef POSE2D_H
#define POSE2D_H

#include <memory>
#include <opencv2/core/core.hpp>
#include <tuw_geometry/point2d.h>
namespace tuw
{
class Pose2D;
using Pose2DPtr = std::shared_ptr< Pose2D >;
using Pose2DConstPtr = std::shared_ptr< Pose2D const>;

class Pose2D
{
protected:
    Point2D position_;    
    double orientation_;  
    mutable double costheta_, sintheta_; // precomputed cos() & sin() of theta.
    mutable bool   cossin_uptodate_;

    void update_cached_cos_sin() const;

public:
    Pose2D();
    Pose2D ( const Point2D &p, double orientation );
    Pose2D ( const Pose2D &p );
    Pose2D ( double x, double y, double orientation );
    Pose2D ( const cv::Vec<double, 3> &s );
    Pose2D &set ( double x, const double y, double phi ) ;
    Pose2D &set ( const Point2D &position, const Point2D &point_ahead ) ;

    Pose2D &set ( const Pose2D &p ) ;

    const Point2D &position () const;

    Point2D &position ();

    const double &x () const;

    double &x ();

    const double &y () const;

    double &y ();

    const double &theta () const;

    double &theta ();

    void set_x ( double v );

    double get_x () const;

    void set_y ( double v );

    double get_y () const;

    void set_theta ( double v );
    Point2D &transform_into_base ( const Point2D &src, Point2D &des ) const;

    Pose2D transform_into( const Pose2D &target ) const;
    
    double get_theta () const;

    void recompute_cached_cos_sin() const;
    double theta_cos() const;
    double theta_sin() const;

    Point2D point_ahead ( double d = 1. ) const;

    void normalizeOrientation ();

    Tf2D tf () const;

    cv::Vec<double, 3> state_vector () const;

    Pose2D inv () const;


    Pose2D &operator += ( const cv::Vec<double, 3> &s );

    Pose2D &operator -= ( const cv::Vec<double, 3> &s ) ;
    

    friend std::ostream &operator << ( std::ostream &os, const Pose2D &o )
    {
        os << "[" << o.x() <<  ", " << o.y() <<  ", " << o.theta()  << "]";
        return os;
    }

    std::string str ( const char* format = "[%6.4lf, %6.4lf, %6.5lf]" ) const;

    bool equal ( const Pose2D& o, double tolerance ) const;


    template <typename T>
    void copyToROSPose ( T &des ) const
    {
        des.position.x = x();
        des.position.y = y();
        des.position.z = 0.;
        EulerYawToQuaternion ( theta(), des.orientation );
    }
};

}
#endif //POSE2D_H