ellipses_representation.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _ellipses_representation_h_
#define _ellipses_representation_h_

#include <iri_clean_board/CleaningPlanConfig.h>
#include "mutex.h"

//include cleaning_plan_alg main library
// OpenCV
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>

// srvs and msgs
#include <estirabot_msgs/StateRepresentationChanges.h>
#include <estirabot_msgs/PointsDistanceMsg.h>
#include <estirabot_msgs/DirtyArea.h>
#include <estirabot_msgs/Ellipse.h>
#include <estirabot_msgs/TraversedEllipses.h>
#include <iri_perception_msgs/ImagePoint.h>

// Pointclouds
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>

// geometry_msgs and conversions
#include <tf/transform_datatypes.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

// // cv::Point_<float>::Point_(iri_perception_msgs::ImagePoint image_point)
// // {
// //     x = image_point.x;
// //     y = image_point.y;
// // }
// 
// 
// template <> // Corrected: specialization is a new definition, not a declaration, thanks again stefanB 
// // void Visitor::operator()( int data ) {
// //    std::cout << "specialization" << std::endl;
// // }
// cv::Point_<float>::operator iri_perception_msgs::ImagePoint_<std::allocator<void> >()
// {
//     iri_perception_msgs::ImagePoint image_point;
//     return image_point;
// }
// // iri_perception_msgs::ImagePoint::operator cv::Point2f() {
// //     cv::Point2f cv_point;
// //     cv_point.x = x;
// //     cv_point.y = y;
// //     return cv_point;
// // }
// 
// /*
// namespace iri_perception_msgs{
// template <>
// struct ImagePoint_<std::allocator<void> >
// {  
//     operator cv::Point2f() {
//       return cv::Point2f();
//    }
// };
// };*/

class EllipsesRepresentation
{
  protected:
    CMutex alg_mutex_;
    
    // some data suppositions
    static const int IMAGE_WIDTH = 640;
    static const int IMAGE_HEIGHT = 480;
    
    // private attributes and methods
    double minFillThreshold;
    double minAreaThreshold;
    bool board_plan;
    uint32_t max_number_ellipses;
    bool is_planning;
    bool is_learning;
    
    // variables
    std::vector<cv::RotatedRect> imageEllipses;
    std::vector<estirabot_msgs::DirtyArea> dirtyAreas_;
    std::vector<double> ellipsesMinWidths;
    std::vector<double> ellipsesMaxWidths;
    std::vector<double> ellipsesArea;
    std::vector<uint8_t> ellipsesSparse;
    uint32_t small_objects_join_distance;
    
    // state parameters
    // small objects
    double small_area_threshold, medium_area_threshold;
    // erase a board
    double min_width_threshold, max_width_threshold;
    double near_distance_threshold;
    int initial_state_idx;
    
    std::string dirtyAreaToString(estirabot_msgs::DirtyArea dirty_area, int idx);
    
    std::string dirtyAreasDistancesToString(std::vector< estirabot_msgs::PointsDistanceMsg > distances);
        
    std::string traversedEllipsesToString(std::vector<estirabot_msgs::TraversedEllipses> traversed_ellipses);
    
    inline double getDistance(cv::Point2f p1, cv::Point2f p2)
    {
        // sqrt (a^2 + b^2)
        return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
    }
    inline double getDistance(iri_perception_msgs::ImagePoint p1, iri_perception_msgs::ImagePoint p2)
    {
        return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
    }
    
    
    void computeBoundingEllipses(cv::Mat image);

    void clearLastEllipses();

    void setSourceImage(cv::Mat image);
    
    bool divideBigEllipse(cv::Mat image, cv::Mat ellipseMask, cv::RotatedRect currentEllipse);
    

  public:
        // Images
        cv::Mat state_image;
        cv::Mat show_image;
        
        
    typedef iri_clean_board::CleaningPlanConfig Config;

    Config config_;

    EllipsesRepresentation(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(Config& new_cfg, uint32_t level=0);

    // here define all cleaning_plan_alg interface methods to retrieve and set
    // the driver parameters

    ~EllipsesRepresentation(void);
    
   
        void createNewState(cv::Mat image);
    
    
    std::vector< estirabot_msgs::PointsDistanceMsg > distancesBetweenEllipses(std::vector<estirabot_msgs::DirtyArea> dirty_areas);
    
    
    std::vector< estirabot_msgs::TraversedEllipses > getTraversedEllipses(std::vector<estirabot_msgs::DirtyArea> dirty_areas);
    bool pointIsBetween(cv::Point2f p1, cv::Point2f p2, cv::Point2f point);
    bool pointIsBetween(iri_perception_msgs::ImagePoint new_p1, iri_perception_msgs::ImagePoint new_p2, iri_perception_msgs::ImagePoint new_point);
    bool pointIsBetween(iri_perception_msgs::ImagePoint new_p1, cv::Point2f p2, iri_perception_msgs::ImagePoint new_point);
    
    
    int32_t getNearestEllipse();
    
    void drawPoints(std::vector< cv::Point2f > img_coords);
    
    
    
    estirabot_msgs::Ellipse rotatedRectToRosEllipse(cv::RotatedRect ellipse) { 
        estirabot_msgs::Ellipse emsg;
        emsg.size.width = ellipse.size.width;
        emsg.size.height = ellipse.size.height;
        emsg.center.x = ellipse.center.x;
        emsg.center.y = ellipse.center.y;
        emsg.angle = ellipse.angle;
        return emsg;
    };
    
    cv::RotatedRect rosEllipseToRotatedRect(estirabot_msgs::Ellipse ellipse_msg) { 
        cv::RotatedRect ellipse;
        cv::Size2f size(ellipse_msg.size.width, ellipse_msg.size.height);
        cv::Point2f center(ellipse_msg.center.x, ellipse_msg.center.y);
//         size = cv::Size2f(ellipse_msg.size.width, ellipse_msg.size.height);
//         center = cv::Point2f(ellipse_msg.center.x, ellipse_msg.center.y);
        ellipse = cv::RotatedRect(center, size, ellipse_msg.angle);
        return ellipse;
    };
    
    
    void truncateEllipsesNumber();
    
    std::vector< cv::RotatedRect > loadEllipsesFromRosMsg(std::vector< estirabot_msgs::Ellipse > ellipses_msg);
    std::vector< cv::RotatedRect > loadEllipsesFromRosMsg(std::vector< estirabot_msgs::DirtyArea > dirty_areas_msg);
    
    void writeStateToFile(std::string path, 
                          std::vector<estirabot_msgs::DirtyArea> dirty_areas);
   
    
    // GETS
    
    std::vector<estirabot_msgs::Ellipse> getEllipsesRosMsg() { 
        std::vector<estirabot_msgs::Ellipse> ellipses_msg;
        
        for(std::vector<cv::RotatedRect>::iterator j=imageEllipses.begin(); j!=imageEllipses.end(); ++j) {
                ellipses_msg.push_back(rotatedRectToRosEllipse(*j));
        }
        
        return ellipses_msg; 
    };
    
    std::vector<cv::RotatedRect> getCurrentEllipses() { return imageEllipses; };
    
    std::string getStateString(const std::vector<estirabot_msgs::DirtyArea> dirty_areas);
    std::string getStateStringOrdered(const std::vector<estirabot_msgs::DirtyArea> dirty_areas);
        
        std::vector<estirabot_msgs::DirtyArea> orderDirtyAreas(const std::vector<estirabot_msgs::DirtyArea> dirty_areas, 
                                                                                                                    const std::vector<estirabot_msgs::DirtyArea> previous_dirty_areas);
    
    std::vector<double> getEllipsesMinWidths() { return ellipsesMinWidths; };
    
    std::vector<double> getEllipsesMaxWidths() { return ellipsesMaxWidths; };
    
    std::vector<double> getEllipsesAreas() { return ellipsesArea; };
    
    std::vector<estirabot_msgs::DirtyArea> getRosDirtyAreas();
    std::vector<estirabot_msgs::DirtyArea> calculateRosDirtyAreas(std::vector<cv::RotatedRect> ellipses);
    
    double getEllipsesSummedAreas() 
    { 
        double sum_areas;
        for(std::vector<double>::iterator j=ellipsesArea.begin();j!=ellipsesArea.end();++j)
            sum_areas += *j;
        return sum_areas;
    };
    
    std::vector< uint8_t > getEllipsesSparse() { return ellipsesSparse; };
    
    size_t getEllipsesNumber() { return imageEllipses.size(); };
    
    std::vector< size_t > getGoodPathThroughAllEllipses(size_t init_ellipse);
    std::vector< size_t > getGoodPathThroughAllDirtyAreas(std::vector< estirabot_msgs::DirtyArea > dirty_areas);
    
    cv::RotatedRect getEllipseFromRosDirtyAreas(std::vector< estirabot_msgs::DirtyArea > dirty_areas, int idx);
    
    
    // VARIABLES
    
    cv::Point2f currentArmPosition; // x = -1 when it isn't cleaning on the plane
};

#endif