Program Listing for File world_scoped_maps.hpp

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

#ifndef TUW_GEOMETRY__WORLD_SCOPED_MAPS_HPP
#define TUW_GEOMETRY__WORLD_SCOPED_MAPS_HPP
#include <opencv2/core/core_c.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <tuw_geometry/pose2d.hpp>

namespace tuw
{
class WorldScopedMaps;
using WorldScopedMapsPtr = std::shared_ptr<WorldScopedMaps>;
using WorldScopedMapsConstPtr = std::shared_ptr<WorldScopedMaps const>;

class WorldScopedMaps
{
  cv::Matx33d Mw2m_;
  cv::Matx33d Mm2w_;
  int width_pixel_, height_pixel_;
  double min_x_, max_x_, min_y_, max_y_, rotation_;
  double dx_, dy_;
  double ox_, oy_;
  double mx_, my_;
  double sx_, sy_;

  void init();

public:
  //special class member functions
  WorldScopedMaps();
  virtual ~WorldScopedMaps() = default;
  WorldScopedMaps(const WorldScopedMaps &) = default;
  WorldScopedMaps & operator=(const WorldScopedMaps &) = default;
  WorldScopedMaps(WorldScopedMaps &&) = default;
  WorldScopedMaps & operator=(WorldScopedMaps &&) = default;

  void init(int width_pixel, int height_pixel, cv::Matx33d Mw2m)
  {
    width_pixel_ = width_pixel, height_pixel_ = height_pixel;
    Mw2m_ = Mw2m;
    Mm2w_ = Mw2m_.inv();
    Point2D p_o = w2m(0, 0);
    ox_ = p_o.x();
    oy_ = p_o.y();
    Point2D p_min = m2w(0, 0);
    Point2D p_max = m2w(width_pixel_, height_pixel_);
    min_y_ = std::min(p_min.y(), p_max.y());
    max_y_ = std::max(p_min.y(), p_max.y());
    min_x_ = std::min(p_min.x(), p_max.x());
    max_x_ = std::max(p_min.x(), p_max.x());
    dx_ = max_x_ - min_x_;
    dy_ = max_y_ - min_y_;
    sx_ = ((double)width_pixel_) / dx_;
    sy_ = ((double)height_pixel_) / dy_;
  }

  void init(
    int width_pixel, int height_pixel, double min_y, double max_y, double min_x, double max_x,
    double rotation = 0);

  template<typename T>
  void init(const T & metadata)
  {
    width_pixel_ = metadata.width, height_pixel_ = metadata.height;
    dx_ = metadata.resolution * (double)metadata.width;
    dy_ = metadata.resolution * (double)metadata.height;
    sx_ = 1.0 / metadata.resolution;
    sy_ = 1.0 / metadata.resolution;
    ox_ = 0.;
    oy_ = 0.;
    double roll = 0, pitch = 0, yaw = 0;
    QuaternionToEuler(metadata.origin.orientation, roll, pitch, yaw);
    rotation_ = -yaw;
    rotation_ = 0;
    double ca = cos(rotation_), sa = sin(rotation_);
    mx_ = metadata.origin.position.x;
    my_ = metadata.origin.position.y;
    cv::Matx<double, 3, 3> Tw(1, 0, -mx_, 0, 1, -my_, 0, 0, 1);  // translation
    cv::Matx<double, 3, 3> Sc(sx_, 0, 0, 0, sy_, 0, 0, 0, 1);    // scaling
    cv::Matx<double, 3, 3> R(ca, -sa, 0, sa, ca, 0, 0, 0, 1);    // rotation
    Mw2m_ = R * Sc * Tw;
    Mm2w_ = Mw2m_.inv();
    Point2D p = m2w(width_pixel_, height_pixel_);
    min_y_ = mx_;
    min_x_ = my_;
    max_x_ = p.x();
    max_y_ = p.y();
  }

  bool initialized();
  template<typename T>
  void line(
    T & map, const Point2D & p0, const Point2D & p1, const cv::Scalar & color, int thickness = 1,
    int lineType = cv::LINE_AA) const
  {
    cv::line(map, w2m(p0).cv(), w2m(p1).cv(), color, thickness, lineType);
  }
  template<typename T>
  void circle(
    T & map, const Point2D & p, int radius, const cv::Scalar & color, int thickness = 1,
    int lineType = cv::LINE_AA) const
  {
    cv::circle(map, w2m(p).cv(), radius, color, thickness, lineType);
  }

  template<typename T>
  cv::Scalar_<T> get(cv::Mat_<T> & map, const Point2D & p) const
  {
    return map.at(w2m(p).cv());
  }

  const cv::Matx33d & Mw2m() const;
  const cv::Matx33d & Mm2w() const;

  Point2D w2m(const Point2D & src) const;
  Point2D w2m(double x, double y) const;
  Point2D & w2m(const Point2D & src, Point2D & des) const;
  Point2D m2w(const Point2D & src) const;
  Point2D m2w(double x, double y) const;
  Point2D & m2w(const Point2D & src, Point2D & des) const;

  int width() const;
  int height() const;
  double scale_x() const;
  double scale_y() const;
  double min_x() const;
  double max_x() const;
  double min_y() const;
  double max_y() const;
  double scale_w2m(double v) const;
  std::string infoHeader() const;
};

}  // namespace tuw
#endif  // TUW_GEOMETRY__WORLD_SCOPED_MAPS_HPP