camera_object_detection_alg.cpp

Go to the documentation of this file.

#include "camera_object_detection_alg.h"

CameraObjectDetectionAlgorithm::CameraObjectDetectionAlgorithm(void)
{
  this->ptrPrms = &this->prms;
  this->ptrRFs = &this->RFs;
  this->ptrClfr = &this->clfr;

  ptrRFs->fun_set_values(ptrPrms->fun_get_num_ferns(), ptrPrms->fun_get_num_features(), ptrPrms->fun_get_fern_size());
  ptrRFs->fun_random_ferns();
}

CameraObjectDetectionAlgorithm::~CameraObjectDetectionAlgorithm(void)
{
}

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

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// CameraObjectDetectionAlgorithm Public API
bool CameraObjectDetectionAlgorithm::init_detector(sensor_msgs::ImageConstPtr &image,cv::Rect &coord)
{
  ROS_INFO("CameraObjectDetectionAlgorithm::init_detector");
  this->lock();
  try
  {
    // transform ROS image into OpenCV image
    this->cv_image = cv_bridge::toCvCopy(image,sensor_msgs::image_encodings::BGR8);
    this->img=this->cv_image->image;
    this->show_img=this->cv_image->image;
    // transform rectangle information from ROS to library
    CvRect box = cvRect(coord.x-coord.width/2,coord.y-coord.height/2 ,coord.width ,coord.height); //mid point transformed to upper left point
    // call the iri library initialization function
    ROS_INFO("CameraObjectDetectionAlgorithm::init_detector: fun_train_classifier(..., img, box)");
    fun_train_classifier(this->ptrPrms, this->ptrClfr, this->ptrRFs, &this->img, box);
    // save all initialization data into the class attributes
    this->unlock();
    ROS_INFO("CameraObjectDetectionAlgorithm::init_detector done");
    return true;
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    this->unlock();
    return false;
  }
}

bool CameraObjectDetectionAlgorithm::detect(sensor_msgs::ImageConstPtr &image)
{
  try
  {
    this->lock();
    // transform ROS image into OpenCV image
    this->cv_image = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
    this->img=this->cv_image->image;
    this->show_img=this->cv_image->image;

    // call the iri library detection function
    /* (2) detection stage */
    this->detections = fun_detection(this->ptrPrms, this->ptrClfr, this->ptrRFs, &this->img);

    /* (3) check assistance */
    this->assistance = fun_check_assistance(this->ptrPrms, this->detections);

    // call the iri library learning function
    if(this->assistance)
    {
      /* (4) show detections */
      fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);
      this->unlock();
      return assistance;
    }
    else
    {
      /* (5) learning stage */
      fun_learning(this->ptrPrms, this->ptrRFs, this->ptrClfr, &this->img, this->detections, this->assistance, true);

      /* (4) show detections */
      fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);

      this->unlock();
      return this->assistance;
    }
  }
  catch (cv_bridge::Exception& e)
  {
    this->unlock();
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return true;
  }
}

bool CameraObjectDetectionAlgorithm::learn_detector(bool valid)
{
  try
  {
    this->lock();
    // call the iri library learning function
    /* (5) learning stage */
    fun_learning(this->ptrPrms, this->ptrRFs, this->ptrClfr, &this->img, this->detections, this->assistance, valid);

    /* (4) show detections */
    fun_show_detections(this->ptrPrms, this->ptrClfr, &this->show_img, this->detections, this->assistance);
    // save all initialization data into the class attributes
    this->unlock();
    return true;
  }
  catch (cv_bridge::Exception& e)
  {
    this->unlock();
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return false;
  }
}

bool CameraObjectDetectionAlgorithm::get_output_rect(cv::Rect & rect)
{
  int xa, ya, xb, yb;
  float value;
  get_max_detection_values(this->detections, xa, ya, xb, yb, value);
  //TODO get threshold from class (green box)
  if(value>0.5)
  {
    rect.width  = abs(xa-xb);
    rect.height = abs(ya-yb);
    rect.x      = xa+ rect.width/2; //mid point
    rect.y      = ya+ rect.height/2;
    return true;
  }
  else
  {
    rect.width  = 0;
    rect.height = 0;
    rect.x      = 0;
    rect.y      = 0;
    return false;
  }
}

void CameraObjectDetectionAlgorithm::get_output_image(sensor_msgs::ImagePtr &image)
{
  cv_bridge::CvImage roscv_outImage;
  roscv_outImage.header   = this->cv_image->header;
  roscv_outImage.encoding = this->cv_image->encoding;
  roscv_outImage.image    = cv::cvarrToMat(&this->show_img);
  image                   = roscv_outImage.toImageMsg();  
}