world_scoped_maps.h

Go to the documentation of this file.

#ifndef WORLD_SCOPED_MAPS_H
#define WORLD_SCOPED_MAPS_H
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <tuw_geometry/pose2d.h>
 
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, 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;
};
 
}
#endif // WORLD_SCOPED_MAPS_H