get_board_information_alg.cpp

Go to the documentation of this file.

#include "get_board_information_alg.h"

using namespace cv;

GetBoardInformationAlgorithm::GetBoardInformationAlgorithm(void)
{
        current_believe = cv::Mat::zeros(cv::Size(640,480), CV_8UC1);
        unreachable_zones = cv::Mat::zeros(cv::Size(640,480), CV_8UC1);
}

GetBoardInformationAlgorithm::~GetBoardInformationAlgorithm(void)
{
}

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

  this->believe_increase  = new_cfg.believe_increase;
  this->believe_decrease  = new_cfg.believe_decrease;
  this->believe_threshold = new_cfg.believe_threshold;
  
  this->remove_unreachable_points = new_cfg.remove_unreachable_points;
  
  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// GetBoardInformationAlgorithm Public API
void GetBoardInformationAlgorithm::updateCurrentBelieve(const sensor_msgs::Image::ConstPtr& current_image_msg) {
        cv_bridge::CvImagePtr cv_ptr;
        cv::Mat image;
        int new_val;
        
        try
        {
                cv_ptr = cv_bridge::toCvCopy(current_image_msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
                ROS_ERROR("cv_bridge exception: %s", e.what());
                // TODO should return error
        }
        
        image = cv_ptr->image;
        
        cvtColor(image, image, CV_BGR2GRAY);
        threshold( image, image, 10, 255, THRESH_BINARY );
        
        MatConstIterator_<uchar> img_it = image.begin<uchar>(), img_it_end = image.end<uchar>();
        MatIterator_<uchar> bel_it = current_believe.begin<uchar>();
        
        for( ; img_it != img_it_end; ++img_it, ++bel_it )
        {
                if (*img_it > 1) {
                        new_val = *bel_it + believe_increase;
                        if (new_val > 255)
                                new_val = 255;
                }
                else {
                        new_val = *bel_it + believe_decrease;
                        if (new_val < 0)
                                new_val = 0;
                }
                
                *bel_it = (uchar)new_val;
        }
        
        // Save binarized image
        threshold( current_believe, cv_ptr->image, believe_threshold, 255, THRESH_BINARY );
        cv_ptr->encoding = sensor_msgs::image_encodings::TYPE_8UC1;
        
        if (this->remove_unreachable_points) {
                Mat substraction_mat(cv::Size(640,480), CV_8UC1, 3);
                subtract(unreachable_zones, substraction_mat, unreachable_zones);
        }
}


void GetBoardInformationAlgorithm::updateUnreachableZones(const estirabot_msgs::ImagePoint::ConstPtr& point) 
{
        if (this->remove_unreachable_points)
                cv::circle(unreachable_zones, cv::Point(point->x, point->y), 10, 255, -1);
}


// GetBoardInformationAlgorithm Public API
sensor_msgs::Image GetBoardInformationAlgorithm::joinImageAndBelieve(const sensor_msgs::Image current_image_msg) {
        cv_bridge::CvImagePtr cv_ptr;
        cv::Mat mask_believe, mask_unreachable, image_res;
        
        try
        {
                cv_ptr = cv_bridge::toCvCopy(current_image_msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
                ROS_ERROR("cv_bridge exception: %s", e.what());
                // TODO should return error
        }
        
        // binarize current_image
        cvtColor(cv_ptr->image, image_res, CV_BGR2GRAY);
        threshold( image_res, image_res, 10, 255, THRESH_BINARY );
        
        // get current believe mask
        threshold( current_believe, mask_believe, believe_threshold, 255, THRESH_BINARY );
        // get resulting join of perception and believe
        bitwise_and(image_res, mask_believe, image_res);
        
        if (this->remove_unreachable_points) {
                // remove unreachable parts of the image
                threshold( unreachable_zones, mask_unreachable, 10, 255, THRESH_BINARY_INV );
                bitwise_and(image_res, mask_unreachable, image_res);
        }
        
        // to cv_ptr
        cv_ptr->image = image_res;
        cv_ptr->encoding = sensor_msgs::image_encodings::TYPE_8UC1;
        
        return *cv_ptr->toImageMsg();
}


sensor_msgs::Image GetBoardInformationAlgorithm::getCurrentBelieveColors()
{
        cv_bridge::CvImagePtr cv_ptr;
        cv::Mat believe_color, unreachable_color, image_res;
        
        Mat mat_red(cv::Size(640,480), CV_8UC3, Scalar(0,0,255));
        Mat mat_green(cv::Size(640,480), CV_8UC3, Scalar(255,0,0));
        
        cvtColor(current_believe, believe_color, CV_GRAY2BGR);
        bitwise_and(believe_color, mat_red, believe_color);
        
        cvtColor(unreachable_zones, unreachable_color, CV_GRAY2BGR);
        bitwise_and(unreachable_color, mat_green, unreachable_color);
        
        bitwise_or(unreachable_color, believe_color, image_res);
        
        // to cv_ptr
        cv_bridge::CvImage cvbridge_aux;
        cvbridge_aux.encoding     = sensor_msgs::image_encodings::BGR8;
        cvbridge_aux.image        = image_res;
        cvbridge_aux.header.stamp = ros::Time::now();
        
        return *cvbridge_aux.toImageMsg();
}