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