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