Program Listing for File pose2d.hpp

↰ Return to documentation for file (include/tuw_geometry/pose2d.hpp)

#ifndef TUW_GEOMETRY__POSE2D_HPP
#define TUW_GEOMETRY__POSE2D_HPP

#include <memory>
#include <opencv2/core/core.hpp>
#include <tuw_geometry/point2d.hpp>
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;

  Point2D transform_into_base(const Point2D & src) 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);
  }
};
using Poses2D = std::vector<Pose2D>;
using Poses2DPtr = std::shared_ptr<Poses2D>;
using Poses2DConstPtr = std::shared_ptr<Poses2D const>;

}  // namespace tuw
#endif  //TUW_GEOMETRY__POSE2D_HPP