VENUS ROBOTIX

#include "CameraColorExtraction.h"

// OpenCV

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#define WIDTH  640

#define HEIGHT 480

cv::VideoCapture cap;

RTC::ReturnCode_t CameraColorExtraction::onInitialize()

{

  addOutPort("point2d", m_center_pointOut);

  std::cout << "CameraCaptureComp" << std::endl;

  cap = cv::VideoCapture(0);

  cap.set(CV_CAP_PROP_FRAME_WIDTH, WIDTH);

  cap.set(CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);

  if(!cap.isOpened())

  {

      return RTC::RTC_ERROR;    

  }

  return RTC::RTC_OK;

}

RTC::ReturnCode_t CameraColorExtraction::onActivated(RTC::UniqueId ec_id)

{

  std::cout << "CreateWindow" << std::endl;

  cv::namedWindow("Capture", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);

  cv::namedWindow("Binary", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);

  return RTC::RTC_OK;

}

RTC::ReturnCode_t CameraColorExtraction::onDeactivated(RTC::UniqueId ec_id)

{

  std::cout << "DestroyWindow" << std::endl;

  cv::destroyWindow("Capture");

  cv::destroyWindow("Binary");

  return RTC::RTC_OK;

}

RTC::ReturnCode_t CameraColorExtraction::onExecute(RTC::UniqueId ec_id)

{

  cv::Mat frame;

  cap >> frame;

  // ------------------------Color extraction------------------------

  cv::Mat gaussian, hsv, channels[3], hue_temp, hue, saturation, value, binary_temp, binary;

  cv::GaussianBlur(frame, gaussian, cv::Size(11,11), 10, 10);

  cv::cvtColor(frame, hsv, CV_BGR2HSV);

  cv::split(hsv, channels);

  cv::threshold(channels[0], hue_temp,    55, 255, CV_THRESH_TOZERO);

  cv::threshold(hue_temp,    hue       , 77, 255, CV_THRESH_TOZERO_INV);

  cv::threshold(channels[1], saturation, 51, 255, CV_THRESH_BINARY);

  cv::threshold(channels[2], value     , 153, 255, CV_THRESH_BINARY);

  cv::bitwise_and(hue,         saturation, binary_temp);

  cv::bitwise_and(binary_temp, value,      binary);

  cv::Moments moment = moments(binary, true);

  int center_x = (int)(moment.m10 / moment.m00);

  int center_y = (int)(moment.m01 / moment.m00);

  cv::circle(frame, cv::Point(center_x, center_y), 80, cv::Scalar(0,0,200), 4, 4);

  cv::imshow("Binary",  binary);

  std::cout << "x:" << center_x << " y:" <<center_y << std::endl;

  m_center_point.data.x = center_x;

  m_center_point.data.y = center_y;

  m_center_pointOut.write();

  // ------------------------Color extraction------------------------

  cv::imshow("Capture", frame);

  cv::waitKey(1);

  return RTC::RTC_OK;

}