pose2d.cpp

Go to the documentation of this file.

#include "tuw_geometry/pose2d.h"
using namespace tuw;

Pose2D::Pose2D() : position_(), orientation_ ( 0 ) {};
Pose2D::Pose2D ( const Point2D &p, double orientation_ ) : position_ ( p ), orientation_ ( orientation_ ), cossin_uptodate_ ( false ) {};
Pose2D::Pose2D ( const Pose2D &p ) : position_ ( p.position_ ), orientation_ ( p.orientation_ ), cossin_uptodate_ ( false ) {};
Pose2D::Pose2D ( double x, double y, double orientation_ ) : position_ ( x,y ), orientation_ ( orientation_ ), cossin_uptodate_ ( false ) {};
Pose2D::Pose2D ( const cv::Vec<double, 3> &s ) : position_ ( s ( 0 ),s ( 1 ) ), orientation_ ( s ( 2 ) ), cossin_uptodate_ ( false ) {};

Pose2D &Pose2D::set ( double x, double y, double phi ) {
    angle_normalize ( phi, -M_PI, +M_PI );
    position_ = cv::Vec<double, 3> ( x, y, 1 ), orientation_ = phi;
    cossin_uptodate_ = false;
    return *this;
}
Pose2D &Pose2D::set ( const Point2D &position, const Point2D &point_ahead ) {
    position_.set ( position.x(), position.y() );
    double dx = point_ahead.x() - position.x(), dy = point_ahead.y() - position.y();
    orientation_ = atan2 ( dy,dx );
    cossin_uptodate_ = false;
    return *this;
}
Pose2D &Pose2D::set ( const Pose2D &p ) {
    position_ = p.position_, orientation_ = p.orientation_;
    cossin_uptodate_ = false;
    return *this;
}
const Point2D &Pose2D::position () const {
    return position_;
}
Point2D Pose2D::point_ahead ( double d ) const {
    update_cached_cos_sin();
    return Point2D ( x() + costheta_ * d, y() + sintheta_ * d );
}
const double &Pose2D::x () const {
    return position_[0];
}
const double &Pose2D::y () const {
    return position_[1];
}
const double &Pose2D::theta () const {
    return orientation_;
}
Point2D &Pose2D::position () {
    return position_;
}
double &Pose2D::x () {
    return position_[0];
}
double &Pose2D::y () {
    return position_[1];
}
double &Pose2D::theta () {
    cossin_uptodate_ = false;
    return orientation_;
}

void Pose2D::set_x ( double v ) {
    this->x() = v;
}
double Pose2D::get_x () const {
    return this->x();
}
void Pose2D::set_y ( double v ) {
    this->y() = v;
}
double Pose2D::get_y () const {
    return this->y();
}
void Pose2D::set_theta ( double v ) {
    this->theta () = v;
}
double Pose2D::get_theta () const {
    return this->theta ();
}

void Pose2D::normalizeOrientation () {
    angle_normalize ( orientation_, -M_PI, +M_PI );
}
Tf2D Pose2D::tf () const {
    update_cached_cos_sin();
    return cv::Matx33d ( costheta_, -sintheta_, x(), sintheta_, costheta_, y(), 0, 0, 1. );
}
cv::Vec<double, 3> Pose2D::state_vector () const {
    return cv::Vec<double, 3> ( x(), y(), theta() );
}
Pose2D Pose2D::inv () const {
    Pose2D p ( -this->x(), -this->y(), -this->theta() );
    return p;
}
Point2D &Pose2D::transform_into_base(const Point2D &src, Point2D &des) const{
    //update_cached_cos_sin();
    des.set(src.x() * costheta_ - src.y() * sintheta_ + src.h() * x(), 
            src.x() * sintheta_ + src.y() * costheta_ + src.h() * y(),
            src.h() );
    return des;
}

Pose2D &Pose2D::operator += ( const cv::Vec<double, 3> &s ) {
    this->x() += s.val[0], this->y() += s.val[1], this->theta() += s.val[2];
    angle_normalize ( this->theta() );
    cossin_uptodate_ = false;
    return *this;
}
Pose2D &Pose2D::operator -= ( const cv::Vec<double, 3> &s ) {
    this->x() -= s.val[0], this->y() -= s.val[1], angle_difference ( this->theta(), s.val[2] );
    cossin_uptodate_ = false;
    return *this;
}
Pose2D Pose2D::transform_into( const Pose2D &target ) const{
    return Pose2D(this->x() - target.x(), this->y() - target.y(), angle_normalize (this->theta() - target.theta()));
}

void Pose2D::recompute_cached_cos_sin() const {
    costheta_ = cos ( orientation_ );
    sintheta_ = sin ( orientation_ );
    cossin_uptodate_=true;
}
void Pose2D::update_cached_cos_sin() const {
    if ( cossin_uptodate_ ) {
        return;
    }
    recompute_cached_cos_sin();
}
double Pose2D::theta_cos() const { 
  update_cached_cos_sin(); 
  return costheta_;   
}
double Pose2D::theta_sin() const { 
  update_cached_cos_sin(); 
  return sintheta_;   
}
std::string Pose2D::str(const char* format) const
{
    char str[0xFF];
    sprintf(str,format, x(), y(), theta()); 
    return std::string(str);
}

bool Pose2D::equal( const Pose2D& o, double tolerance) const {
      double d_position = cv::norm(o.position() - this->position());
      double d_angle = angle_difference(o.theta(),this->theta());
      return (d_angle < tolerance) && (fabs(d_angle) < tolerance);
}