ellipses_representation.cpp

Go to the documentation of this file.

#include "ellipses_representation.h"

using namespace cv;
using namespace std;


EllipsesRepresentation::EllipsesRepresentation(void)
{
        // arbitrarial intial position
        currentArmPosition.x = 0;
        currentArmPosition.y = 0;
}

EllipsesRepresentation::~EllipsesRepresentation(void)
{
}

void EllipsesRepresentation::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  this->minFillThreshold     = new_cfg.minFillThreshold;
  this->minAreaThreshold     = new_cfg.minAreaThreshold;
  this->board_plan           = new_cfg.board_plan;
  this->max_number_ellipses  = new_cfg.max_number_ellipses;
  this->small_objects_join_distance = new_cfg.small_objects_join_distance;
  this->initial_state_idx    = new_cfg.initial_state_idx;
  
  // Rules generation
  // Erase a board
  this->min_width_threshold = new_cfg.min_width_threshold;
  this->max_width_threshold = new_cfg.max_width_threshold;
  
  // Small objects
  this->small_area_threshold = new_cfg.small_area_threshold;
  this->medium_area_threshold = new_cfg.medium_area_threshold;
  this->near_distance_threshold = new_cfg.near_distance_threshold;
  
  this->is_planning = ((!new_cfg.no_perceptions) && (!new_cfg.no_planning));
  
  this->is_learning = new_cfg.is_learning;
  
  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}


void EllipsesRepresentation::createNewState(cv::Mat image)
{
        std::vector<estirabot_msgs::DirtyArea> previousDirtyAreas = this->dirtyAreas_;
        clearLastEllipses();
        setSourceImage(image);
        computeBoundingEllipses(image);
        this->dirtyAreas_ = orderDirtyAreas(calculateRosDirtyAreas(imageEllipses), previousDirtyAreas);
}

void EllipsesRepresentation::setSourceImage(cv::Mat image)
{
        // save bgr image for displaying plan
        cv::cvtColor(image, this->show_image, CV_GRAY2BGR);
        
        // get binarized image
        threshold(image, this->state_image, 10, 255, cv::THRESH_BINARY );
}


// divides image in two halfs, one containing half a ellipse
// then gets ellipses in each half calling recursively getBoundingEllipses
// returns false if couldn't divide ellipses
bool EllipsesRepresentation::divideBigEllipse(cv::Mat image, cv::Mat ellipseMask, RotatedRect currentEllipse)
{
        Mat rectangleMask, finalMask;
        Mat firstEllipsePart, secondEllipsePart;
        Point2i rectanglePoint;
        
        ROS_DEBUG("Recursive ellipse");
        
        float angle = currentEllipse.angle;
        // we want angle in [0..90]
        if (angle > 180)
                angle -=180;
        if (angle > 90)
                angle = 180 - angle;
        
        //float angle = (ellipse.angle / 180) * M_PI;
        
        if (currentEllipse.size.width > currentEllipse.size.height) {
                if (angle < 45)
                        rectanglePoint = Point2i(currentEllipse.center.x, image.size().height); // divide width
                else
                        rectanglePoint = Point2i(image.size().width, currentEllipse.center.y); // divide height
        }
        else {
                if (angle < 45)
                        rectanglePoint = Point2i(image.size().width, currentEllipse.center.y); // divide height
                else
                        rectanglePoint = Point2i(currentEllipse.center.x, image.size().height); // divide width
        }
        
        rectangleMask = Mat::zeros(image.size(),CV_8UC1);
        rectangle(rectangleMask, Point2i(0,0), rectanglePoint, 255, CV_FILLED);
        
        // First half
        bitwise_and(ellipseMask, rectangleMask, finalMask); // mask = rectangle and ellipse
        bitwise_and(finalMask, image, firstEllipsePart); // image and mask
        
        // Second half
        bitwise_not(rectangleMask, rectangleMask); // invert rectangle (the other half image)
        bitwise_and(ellipseMask, rectangleMask, finalMask); // mask = rectangle and ellipse
        bitwise_and(finalMask, image, secondEllipsePart); // image and mask
        
        // check for infinite loops -> one half gets all the ellipse
        int sumFirstHalf, sumSecondHalf;
        sumFirstHalf = countNonZero(firstEllipsePart);
        sumSecondHalf = countNonZero(secondEllipsePart);
        
        // if a new ellipse has too few points -> cancel division (if not, we may enter a infinite loop)
        if ((sumFirstHalf < 150) || (sumSecondHalf < 150)) {
                ROS_INFO("EllipsesRepresentation: Couldn't divide ellipses");
                
                return false;
        }
        else{
                computeBoundingEllipses(firstEllipsePart); //recursive
                computeBoundingEllipses(secondEllipsePart); //recursive
                
                return true;
        }
}


// EllipsesRepresentation Public API
void EllipsesRepresentation::computeBoundingEllipses(cv::Mat image)
{
        double filledArea, totalArea;
        bool sparse;
        std::vector<std::vector<cv::Point> > contours;
        std::vector<Vec4i> hierarchy;
        
        ROS_DEBUG("Compute ellipses");
        
        // extract contours
        if (this->board_plan)
                findContours( image, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
        else {
                cv::Mat image_closed;
                cv::Mat element = cv::getStructuringElement(MORPH_ELLIPSE, cv::Size(small_objects_join_distance, small_objects_join_distance));
                cv::morphologyEx(image, image_closed, cv::MORPH_CLOSE, element);
                findContours( image_closed, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
        }
        
        ROS_DEBUG("Extracted contours");
        // get ellipse for each contour
        RotatedRect currentEllipse;
        for( size_t i = 0; i < contours.size(); i++ ) {
                if( contours[i].size() > 5 ) { 
                        ROS_DEBUG("A ellipse");
                        currentEllipse = fitEllipse( Mat(contours[i]) );
                        
                        // Sometimes fitEllipse returns ellipses bigger than the image. Check for it
                        if ((currentEllipse.size.width > 500) || (currentEllipse.size.height > 500)) {
                                ROS_ERROR("Detected ellipse with a dimension bigger than 500. Ignoring it...");
                                return;
                        }
                        
                        // get real filled area
                        Mat region = Mat::zeros(image.rows, image.cols, CV_8UC1);
                        drawContours(region, contours, i, Scalar(255), CV_FILLED );
                        bitwise_and(region, image, region);
                        filledArea = countNonZero(region);
                        //filledArea = contourArea(contours[i]);
                        totalArea = currentEllipse.size.width * currentEllipse.size.height;
                        sparse = (filledArea/totalArea) < minFillThreshold;
                        ROS_DEBUG_STREAM("filledArea: "<<filledArea);
                        ROS_DEBUG_STREAM("totalArea: "<< totalArea);
                        
                        // Check if ellipse is big and too sparse
                        // only divide ellipses for board plans
                        bool divide_ellipse = (totalArea > minAreaThreshold) and (sparse);// and this->board_plan;
                        
                        if (divide_ellipse) {
                                Mat ellipseMask = Mat::zeros(image.size(),CV_8UC1);
                                drawContours(ellipseMask, contours, i, 255, CV_FILLED);
                                
                                //recursive algorithm for dividing ellipses
                                divide_ellipse = divideBigEllipse(image, ellipseMask, currentEllipse); 
                        }
                        
                        // it isn't else because it can change in the previous if()
                        if (not divide_ellipse) {
                                this->imageEllipses.push_back(currentEllipse);
                                this->ellipsesMaxWidths.push_back(currentEllipse.size.height);
                                this->ellipsesMinWidths.push_back(currentEllipse.size.width);
                                this->ellipsesArea.push_back(totalArea);
                                this->ellipsesSparse.push_back(sparse);
                                
                                // save visual image for publishing and debugging
                                ellipse(show_image, currentEllipse, Scalar(0,0,255), 2, CV_AA);
                        }
                }
        }
        
        // Truncate number of ellipses to improve plan quality
        if (this->is_planning)
                truncateEllipsesNumber();
}


void EllipsesRepresentation::clearLastEllipses()
{
        this->imageEllipses.clear();
        this->ellipsesMinWidths.clear();
        this->ellipsesMaxWidths.clear();
        this->ellipsesArea.clear();
        this->ellipsesSparse.clear();
}


void EllipsesRepresentation::truncateEllipsesNumber()
{
        if (imageEllipses.size() <= this->max_number_ellipses) // check if we already have few ellipses
                return;
        
        Point origPoint = Point(0,0);
        std::set<size_t> availableEllipses;
        std::vector<size_t> selectedEllipses;
        
        for (size_t i = 0; i < this->imageEllipses.size(); i++)
                availableEllipses.insert(i);
        
        size_t nearestEllipse;
        double distance, minDist;
        
        for (size_t i = 0; i < this->max_number_ellipses; i++) {
                minDist = 2000;
                for (size_t j = 0; j < this->imageEllipses.size(); j++) {
                        if (availableEllipses.find(j) != availableEllipses.end()) { //
                                distance = getDistance(origPoint, imageEllipses[j].center);
                                if (distance < minDist) {
                                        minDist = distance;
                                        nearestEllipse = j;
                                }
                        }
                }
                selectedEllipses.push_back(nearestEllipse);
                availableEllipses.erase(nearestEllipse);
        }
        
        std::vector<cv::RotatedRect> newImageEllipses;
        std::vector<double> newEllipsesMinWidths;
        std::vector<double> newEllipsesMaxWidths;
        std::vector<double> newEllipsesArea;
        std::vector<uint8_t> newEllipsesSparse;
        
        for (size_t i = 0; i < selectedEllipses.size(); i++) {
                newImageEllipses.push_back(this->imageEllipses[ selectedEllipses[i] ]);
                newEllipsesMinWidths.push_back(this->ellipsesMinWidths[ selectedEllipses[i] ]);
                newEllipsesMaxWidths.push_back(this->ellipsesMaxWidths[ selectedEllipses[i] ]);
                newEllipsesArea.push_back(this->ellipsesArea[ selectedEllipses[i] ]);
                newEllipsesSparse.push_back(this->ellipsesSparse[ selectedEllipses[i] ]);
        }
        
        this->imageEllipses = newImageEllipses;
        this->ellipsesMinWidths = newEllipsesMinWidths;
        this->ellipsesMaxWidths = newEllipsesMaxWidths;
        this->ellipsesArea = newEllipsesArea;
        this->ellipsesSparse = newEllipsesSparse;
}


std::vector< estirabot_msgs::PointsDistanceMsg > EllipsesRepresentation::distancesBetweenEllipses(std::vector<estirabot_msgs::DirtyArea> dirty_areas)
{
    std::vector< estirabot_msgs::PointsDistanceMsg > result;
    
    for(size_t i=0; i < dirty_areas.size(); i++) {
        for(size_t j=i+1; j < dirty_areas.size(); j++) {
            estirabot_msgs::PointsDistanceMsg distanceMsg;
            distanceMsg.origIdx = dirty_areas[i].id;
            distanceMsg.dstIdx  = dirty_areas[j].id;
            distanceMsg.distance = getDistance(dirty_areas[i].ellipse.center, dirty_areas[j].ellipse.center);
            
            result.push_back(distanceMsg);
        }
    }
    
    return result;
}

// ugly conversions
bool EllipsesRepresentation::pointIsBetween(iri_perception_msgs::ImagePoint new_p1, iri_perception_msgs::ImagePoint new_p2, iri_perception_msgs::ImagePoint new_point)
{
    cv::Point2f p2;
    p2.x = new_p2.x;
    p2.y = new_p2.y;
    return pointIsBetween(new_p1, p2, new_point);
}

bool EllipsesRepresentation::pointIsBetween(iri_perception_msgs::ImagePoint new_p1, cv::Point2f p2, iri_perception_msgs::ImagePoint new_point)
{
    cv::Point2f p1, point;
    p1.x = new_p1.x;
    p1.y = new_p1.y;
    point.x = new_point.x;
    point.y = new_point.y;
    return pointIsBetween(p1, p2, point);
}

bool EllipsesRepresentation::pointIsBetween(cv::Point2f p1, cv::Point2f p2, cv::Point2f point)
{
        float distance, crossproduct, dotproduct, squaredlengthba;
        float epsilon = 12;
        
        /*
        * http://stackoverflow.com/questions/328107/how-can-you-determine-a-point-is-between-two-other-points-on-a-line-segment
        * 
        * Check if the cross product of (b-a) and (c-a) is 0, as tells Darius Bacon, tells 
        * you if the points a, b and c are aligned.
        * 
        * But, as you want to know if c is between a and b, you also have to check that the
        * dot product of (b-a) and (c-a) is positive and is less than the square of the 
        * distance between a and b.
        */
        
        distance = sqrt( pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) );
        
        crossproduct = (point.y - p1.y) * (p2.x - p1.x) - (point.x - p1.x) * (p2.y - p1.y);
        
        if (abs(crossproduct) > (epsilon * distance))
                return false;   // (or != 0 if using integers)
        
        dotproduct = (point.x - p1.x) * (p2.x - p1.x) + (point.y - p1.y)*(p2.y - p1.y);
        if (dotproduct < 0)
                return false;
        
        squaredlengthba = (p2.x - p1.x)*(p2.x - p1.x) + (p2.y - p1.y)*(p2.y - p1.y);
        if (dotproduct > squaredlengthba)
                return false;
        
        return true;
}

std::vector< estirabot_msgs::TraversedEllipses > EllipsesRepresentation::getTraversedEllipses(std::vector<estirabot_msgs::DirtyArea> dirty_areas)
{
    std::vector< estirabot_msgs::TraversedEllipses > result;
    
    for(size_t i=0; i < dirty_areas.size(); i++) {
        // traversals from ellipse i to other ellipses
        if (!this->is_learning) {
                for(size_t j=i+1; j < dirty_areas.size(); j++) {
                estirabot_msgs::TraversedEllipses traversedEllipsesMsg;
                traversedEllipsesMsg.idx1 = dirty_areas[i].id;
                traversedEllipsesMsg.idx2 = dirty_areas[j].id;
                
                for (size_t k=0; k < dirty_areas.size(); k++) {
                        if ((k != i) && (k != j) && (pointIsBetween(dirty_areas[i].ellipse.center, dirty_areas[j].ellipse.center, dirty_areas[k].ellipse.center)))
                        traversedEllipsesMsg.traversedIdxs.push_back(dirty_areas[k].id);
                }
                result.push_back(traversedEllipsesMsg);
                }
        }
        
        // traversals from ellipse i to edge
        cv::Point2f edge_point;
        int edge_idx;
        edge_idx = -1;
        edge_point.y = dirty_areas[i].ellipse.center.y;
        edge_point.x = IMAGE_WIDTH;
        for (size_t k=0; k < dirty_areas.size(); k++) {
            estirabot_msgs::TraversedEllipses traversedEllipsesMsg;
            traversedEllipsesMsg.idx1 = dirty_areas[i].id;
            traversedEllipsesMsg.idx2 = edge_idx;
            
            if ( (k != i) && pointIsBetween(dirty_areas[i].ellipse.center, edge_point, dirty_areas[k].ellipse.center) ) {
                traversedEllipsesMsg.traversedIdxs.push_back(dirty_areas[k].id);
            }
            result.push_back(traversedEllipsesMsg);
        }
    }
    
    return result;
}


int32_t EllipsesRepresentation::getNearestEllipse()
{
        double min = 10000;
        int32_t min_idx = -1;
        
        for(size_t i=0; i < imageEllipses.size(); i++) {
                double dist = getDistance(currentArmPosition, imageEllipses[i].center);
                if (dist < min) {
                        min = dist;
                        min_idx = i;
                }
        }
        
        ROS_DEBUG_STREAM("Minimun distance is: " << min);
        
        return min_idx;
}


void EllipsesRepresentation::drawPoints(std::vector< Point2f > img_coords_orig)
{
        std::vector< Point2f > img_coords = img_coords_orig;
        img_coords.pop_back();
        
        // both straight and circular cleans
        if (img_coords.size() > 0) {
                circle(show_image, img_coords[0], 5, Scalar(255, 0, 0), -1);
        }
        if (img_coords.size() > 1) {
                circle(show_image, img_coords[1], 5, Scalar(0, 255, 0), -1);
                line(show_image, img_coords[0], img_coords[1], Scalar(0, 255, 0));
        }
        if (img_coords.size() > 2) {
                circle(show_image, img_coords[2], 5, Scalar(255, 0, 255), -1);
                line(show_image, img_coords[1], img_coords[2], Scalar(255, 0, 255));
        }
        if (img_coords.size() > 3) {
                circle(show_image, img_coords[3], 5, Scalar(0, 255, 255), -1);
                line(show_image, img_coords[2], img_coords[3], Scalar(0, 255, 255));
        }
        if (img_coords.size() > 4) {
                circle(show_image, img_coords[4], 5, Scalar(125, 125, 0), -1);
                line(show_image, img_coords[3], img_coords[4], Scalar(125, 125, 0));
        }
        if (img_coords.size() > 5) {
                circle(show_image, img_coords[5], 5, Scalar(0, 125, 125), -1);
                line(show_image, img_coords[4], img_coords[5], Scalar(0, 125, 125));
        }
        if (img_coords.size() > 6)  {
                circle(show_image, img_coords[6], 5, Scalar(125, 0, 125), -1);
                line(show_image, img_coords[5], img_coords[6], Scalar(125, 0, 125));
        }
        if (img_coords.size() > 7) {
                circle(show_image, img_coords[7], 5, Scalar(100, 255, 100), -1);
                line(show_image, img_coords[6], img_coords[7], Scalar(100, 255, 100));
        }
}


std::vector< size_t > EllipsesRepresentation::getGoodPathThroughAllEllipses(size_t init_ellipse)
{
        std::vector< size_t > result;
        
        std::set<size_t> visited;
        
        int min_dist, distance;
        size_t min_idx, last_idx;
        
        result.push_back(init_ellipse);
        visited.insert(init_ellipse);
        last_idx = init_ellipse;
        
        for(size_t i=1; i < imageEllipses.size(); i++) { // add imageEllipses.size() - 1 ellipses
                ROS_DEBUG_STREAM("Nearest ellipse for " << last_idx);
                min_dist = 10000;
                for(size_t j=0; j < imageEllipses.size(); j++) {
                        ROS_DEBUG_STREAM("New j is " << j);
                        if (visited.find(j) == visited.end()) { // if it isn't on the result yet
                                distance = getDistance(imageEllipses[last_idx].center, imageEllipses[j].center);
                                ROS_DEBUG_STREAM("Distance " << distance);
                                if (distance < min_dist) {
                                        min_dist = distance;
                                        min_idx = j;
                                }
                        }
                }
                
                result.push_back(min_idx);
                visited.insert(min_idx);
                last_idx = min_idx;
                ROS_DEBUG_STREAM("Nearest ellipse was " << last_idx);
        }
        
        return result;
}


std::vector< size_t > EllipsesRepresentation::getGoodPathThroughAllDirtyAreas(std::vector< estirabot_msgs::DirtyArea > dirty_areas)
{
    std::vector< size_t > result;
    
    std::set<size_t> visited;
    
    int min_dist, distance;
    size_t min_idx, last_idx;
    
    result.push_back(dirty_areas[0].id);
    visited.insert(0);
    last_idx = 0;
    
    for(size_t i=1; i < dirty_areas.size(); i++) { // add imageEllipses.size() - 1 ellipses
        ROS_DEBUG_STREAM("Nearest ellipse for " << last_idx);
        min_dist = 10000;
        for(size_t j=0; j < dirty_areas.size(); j++) {
            ROS_DEBUG_STREAM("New j is " << j);
            if (visited.find(j) == visited.end()) { // if it isn't on the result yet
                distance = getDistance( rosEllipseToRotatedRect(dirty_areas[last_idx].ellipse).center, rosEllipseToRotatedRect(dirty_areas[j].ellipse).center);
                ROS_DEBUG_STREAM("Distance " << distance);
                if (distance < min_dist) {
                    min_dist = distance;
                    min_idx = j;
                }
            }
        }
        
        result.push_back(dirty_areas[min_idx].id);
        visited.insert(min_idx);
        last_idx = min_idx;
        ROS_DEBUG_STREAM("Nearest ellipse was " << last_idx);
    }
    
    return result;
}


std::vector< cv::RotatedRect> EllipsesRepresentation::loadEllipsesFromRosMsg(vector< estirabot_msgs::Ellipse > ellipses_msg)
{
        vector< RotatedRect > ellipses;
        
        for(vector<estirabot_msgs::Ellipse>::iterator j=ellipses_msg.begin(); j!=ellipses_msg.end(); ++j) {
                ellipses.push_back(rosEllipseToRotatedRect(*j));
        }
        
        return ellipses;
}

std::vector< cv::RotatedRect > EllipsesRepresentation::loadEllipsesFromRosMsg(std::vector< estirabot_msgs::DirtyArea > dirty_areas_msg)
{
        vector< RotatedRect > ellipses;
        
        for(vector<estirabot_msgs::DirtyArea>::iterator j=dirty_areas_msg.begin(); j!=dirty_areas_msg.end(); ++j) {
                ellipses.push_back(rosEllipseToRotatedRect(j->ellipse));
        }
        
        return ellipses;
}


cv::RotatedRect EllipsesRepresentation::getEllipseFromRosDirtyAreas(std::vector< estirabot_msgs::DirtyArea > dirty_areas, int idx)
{
    cv::RotatedRect result;
    ROS_DEBUG_STREAM("Looking for " << idx);
    for(std::vector<estirabot_msgs::DirtyArea>::iterator dirty_area=dirty_areas.begin(); dirty_area!=dirty_areas.end(); ++dirty_area) {
        ROS_DEBUG_STREAM("Dirty area is " << *dirty_area);
        if (dirty_area->id == idx) {
            result = rosEllipseToRotatedRect(dirty_area->ellipse);
            ROS_DEBUG_STREAM("Found!");
        }
    }
    return result;
}


std::vector<estirabot_msgs::DirtyArea> EllipsesRepresentation::getRosDirtyAreas()
{
    return this->dirtyAreas_;
}

std::vector<estirabot_msgs::DirtyArea> EllipsesRepresentation::calculateRosDirtyAreas(std::vector< cv::RotatedRect > ellipses)
{
        std::vector<estirabot_msgs::DirtyArea> dirty_areas;
        estirabot_msgs::DirtyArea dirty_area;
        double area, min_width, max_width;
        
        for (size_t i = 0; i < ellipses.size(); i++) {
        dirty_area.id = i;
                dirty_area.ellipse = rotatedRectToRosEllipse(ellipses[i]);
                dirty_area.sparse = ellipsesSparse[i];
                
                area = ellipsesArea[i];
                if (area < small_area_threshold)
                        dirty_area.area = 0;
                else if (area < medium_area_threshold)
                        dirty_area.area = 1;
                else
                        dirty_area.area = 2;
                
                min_width = ellipsesMinWidths[i];
                max_width = ellipsesMaxWidths[i];
                if (max_width < this->max_width_threshold)
                        dirty_area.shape = 0;
                else if (min_width < this->min_width_threshold)
                        dirty_area.shape = 1;
                else
                        dirty_area.shape = 2;
                
                dirty_areas.push_back(dirty_area);
        }
        
        return dirty_areas;
}


void EllipsesRepresentation::writeStateToFile(std::string path, 
                                              std::vector<estirabot_msgs::DirtyArea> dirty_areas)
{
        std::ofstream state_file(path.c_str());
        if (state_file.is_open())
        {
                state_file << getStateString(dirty_areas);
        }
        state_file.close();
}


std::string EllipsesRepresentation::dirtyAreaToString(estirabot_msgs::DirtyArea dirty_area, int idx)
{
        std::stringstream ss;
        
        ss << "-clean(" << idx << "),";
        
        if (dirty_area.sparse)
                ss << "sparse(" << idx << "),";
        else
                ss << "-sparse(" << idx << "),";
        
        if (dirty_area.area == 0)
                ss << "smallArea(" << idx << ")";
        else if (dirty_area.area == 1)
                ss << "mediumArea(" << idx << ")";
        else if (dirty_area.area == 2)
                ss << "bigArea(" << idx << ")";
        
        // TODO erase a board
        /*if (dirty_area.shape == 0)
                ss << "smallDirt(" << idx << "),";
        else if (dirty_area.shape == 1)
                ss << "narrowDirt(" << idx << "),";
        else (dirty_area.shape == 2)
                ss << "bigDirt(" << idx << "),";
        */
        
        return ss.str();
}


std::string EllipsesRepresentation::traversedEllipsesToString(std::vector<estirabot_msgs::TraversedEllipses> traversed_ellipses)
{
        std::stringstream ss;
        for (size_t i = 0; i < traversed_ellipses.size(); i++) {
                for (size_t j = 0; j < traversed_ellipses[i].traversedIdxs.size(); j++) {
                        if (traversed_ellipses[i].idx1 >= 0) {
                                if (this->is_learning) {
                                        ss << ", traversed2(" << traversed_ellipses[i].idx1 + this->initial_state_idx << "," << traversed_ellipses[i].traversedIdxs[j] + this->initial_state_idx << ")";
                                }
                                else { 
                                        ss << ", traversed(" << traversed_ellipses[i].idx1 + this->initial_state_idx << "," << traversed_ellipses[i].idx2 + this->initial_state_idx << "," << traversed_ellipses[i].traversedIdxs[j] + this->initial_state_idx << ")";
                                        // inverse order: traversed(idx2, idx1, traversed)
                                        ss << ", traversed(" << traversed_ellipses[i].idx2 + this->initial_state_idx << "," << traversed_ellipses[i].idx1 + this->initial_state_idx << "," << traversed_ellipses[i].traversedIdxs[j] + this->initial_state_idx << ")";
                                }
                        }
                }
        }
        
        return ss.str();
}


std::string EllipsesRepresentation::dirtyAreasDistancesToString(std::vector< estirabot_msgs::PointsDistanceMsg> distances)
{
        std::stringstream ss;
        for (size_t i = 0; i < distances.size(); i++) {
                ROS_DEBUG_STREAM("Distance is " << distances[i].distance);
                if (distances[i].distance < near_distance_threshold) {
                        ss << ",near(" << distances[i].origIdx + this->initial_state_idx << "," << distances[i].dstIdx + this->initial_state_idx << ")";
                        ss << ",near(" << distances[i].dstIdx + this->initial_state_idx << "," << distances[i].origIdx + this->initial_state_idx << ")";
                }
        }
        return ss.str();
}


std::string EllipsesRepresentation::getStateStringOrdered(const std::vector<estirabot_msgs::DirtyArea> dirty_areas)
{
    std::stringstream ss;
    
    std::vector<estirabot_msgs::TraversedEllipses> traversed_ellipses = getTraversedEllipses(dirty_areas);
    std::vector< estirabot_msgs::PointsDistanceMsg> distances = distancesBetweenEllipses(dirty_areas);
    
    if (!this->is_learning) {
        ss << "edge(" << this->initial_state_idx - 1 <<  "),";
    }
    for (size_t i = 0; i < dirty_areas.size(); i++) {
        ss << dirtyAreaToString(dirty_areas[i], dirty_areas[i].id + this->initial_state_idx);
        if (i + 1 < dirty_areas.size())
            ss << ",";
    }

    ss << dirtyAreasDistancesToString(distances);
//     if (!this->is_learning) {
        ss << traversedEllipsesToString(traversed_ellipses);
//     }
    return ss.str();
}

std::string EllipsesRepresentation::getStateString(const std::vector<estirabot_msgs::DirtyArea> dirty_areas)
{
    return getStateStringOrdered(dirty_areas);
}


std::vector<estirabot_msgs::DirtyArea> EllipsesRepresentation::orderDirtyAreas(const std::vector<estirabot_msgs::DirtyArea> current, const std::vector<estirabot_msgs::DirtyArea> previous)
{
    std::vector<estirabot_msgs::DirtyArea> prev_copy(previous), current_copy(current);
    std::vector< estirabot_msgs::DirtyArea > current_with_ids, current_ordered;
    int max_id = -1;
    
//     for(size_t i = 0; i<previous.size();i++)
//         ROS_INFO_STREAM("Previous " << previous[i]);
//     for(size_t i = 0; i<current.size();i++)
//         ROS_INFO_STREAM("Current " << current[i]);
    
    while ((prev_copy.size() > 0) && (current_copy.size() > 0)) {
        double min_distance = 10000;
        std::vector<estirabot_msgs::DirtyArea>::iterator it_previous, it_current, it_previous_max, it_current_max;
        for ( it_previous=prev_copy.begin() ; it_previous < prev_copy.end(); it_previous++ ) {
            for ( it_current=current_copy.begin() ; it_current < current_copy.end(); it_current++ ) {
                // euclidean distance
                int x_inc = it_previous->ellipse.center.x - it_current->ellipse.center.x;
                int y_inc = it_previous->ellipse.center.y - it_current->ellipse.center.y;
                double distance = sqrt( (x_inc*x_inc) + (y_inc*y_inc) );
                ROS_DEBUG_STREAM("New distance is " << distance << "\nx " << x_inc << "\ny " << y_inc);
                if (distance < min_distance) {
                    min_distance = distance;
                    it_previous_max = it_previous;
                    it_current_max = it_current;
                }
            }
        }
        // Use nearest ellipse id
        if (it_previous_max->id > max_id) {
            max_id = it_previous_max->id;
        }
        it_current_max->id = it_previous_max->id;
        
        // push_back to ordered list, remove from original lists
        current_with_ids.push_back(estirabot_msgs::DirtyArea(*it_current_max));
        prev_copy.erase(it_previous_max);
        current_copy.erase(it_current_max);
    }
    
//     for(size_t i = 0; i<current_with_ids.size();i++)
//         ROS_INFO_STREAM("Current with ids " << current_with_ids[i]);
    // add remaining dirty areas (if any)
    if (current_copy.size() > 0) {
        for (std::vector<estirabot_msgs::DirtyArea>::iterator it=current_copy.begin() ; it < current_copy.end(); it++ ) {
            max_id++;
            it->id = max_id;
            current_with_ids.push_back(estirabot_msgs::DirtyArea(*it));
        }
    }
    
//     for(size_t i = 0; i<current_with_ids.size();i++)
//         ROS_INFO_STREAM("Current with ids2 " << current_with_ids[i]);
    // order dirty areas based on their symbol
    while (current_with_ids.size() > 0) {
        int min_id = 1000;
        std::vector<estirabot_msgs::DirtyArea>::iterator min_it;
        for (std::vector<estirabot_msgs::DirtyArea>::iterator it=current_with_ids.begin() ; it < current_with_ids.end(); it++ ) {
            if (it->id < min_id) {
                min_id = it->id;
                min_it = it;
            }
        }
                current_ordered.push_back(estirabot_msgs::DirtyArea(*min_it));
        current_with_ids.erase(min_it);
    }
//     for(size_t i = 0; i<current_ordered.size();i++)
//         ROS_INFO_STREAM("Current with ordered " << current_ordered[i]);
    
    return current_ordered;
}