void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::CircleArrayStamped circles_msg;
circles_msg.header = msg->header;
// Do the work
std::vector<cv::Rect> faces;
cv::Mat src_gray, edges;
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
} else {
src_gray = frame;
}
// Reduce the noise so we avoid false circle detection
cv::GaussianBlur( src_gray, src_gray, cv::Size(9, 9), 2, 2 );
// create the main window, and attach the trackbars
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_, max_canny_threshold_, trackbarCallback);
cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_, max_accumulator_threshold_, trackbarCallback);
if (need_config_update_) {
config_.canny_threshold = canny_threshold_;
config_.accumulator_threshold = accumulator_threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
}
// those paramaters cannot be =0
// so we must check here
canny_threshold_ = std::max(canny_threshold_, 1);
accumulator_threshold_ = std::max(accumulator_threshold_, 1);
//runs the detection, and update the display
// will hold the results of the detection
std::vector<cv::Vec3f> circles;
// runs the actual detection
cv::HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows/8, canny_threshold_, accumulator_threshold_, 0, 0 );
// clone the colour, input image for displaying purposes
for( size_t i = 0; i < circles.size(); i++ )
{
cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
// circle center
circle( frame, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circle outline
circle( frame, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
opencv_apps::Circle circle_msg;
circle_msg.center.x = center.x;
circle_msg.center.y = center.y;
circle_msg.radius = radius;
circles_msg.circles.push_back(circle_msg);
}
// shows the results
if( debug_view_) {
cv::imshow( window_name_, frame );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
img_pub_.publish(out_img);
msg_pub_.publish(circles_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::ContourArrayStamped contours_msg;
contours_msg.header = msg->header;
// Do the work
cv::Mat src_gray;
/// Convert it to gray
cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
cv::GaussianBlur( src_gray, src_gray, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT );
/// Create window
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
}
cv::Mat canny_output;
int max_thresh = 255;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::RNG rng(12345);
/// Reduce noise with a kernel 3x3
cv::blur( src_gray, canny_output, cv::Size(3,3) );
/// Canny detector
cv::Canny( canny_output, canny_output, low_threshold_, low_threshold_*2, 3 );
/// Find contours
cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
/// Draw contours
cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
for( size_t i = 0; i< contours.size(); i++ )
{
cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
opencv_apps::Contour contour_msg;
for ( size_t j = 0; j < contours[i].size(); j++ ) {
opencv_apps::Point2D point_msg;
point_msg.x = contours[i][j].x;
point_msg.y = contours[i][j].y;
contour_msg.points.push_back(point_msg);
}
contours_msg.contours.push_back(contour_msg);
}
/// Create a Trackbar for user to enter threshold
if( debug_view_) {
if (need_config_update_) {
config_.canny_low_threshold = low_threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
cv::createTrackbar( "Canny thresh:", window_name_, &low_threshold_, max_thresh, trackbarCallback);
cv::imshow( window_name_, drawing );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
img_pub_.publish(out_img);
msg_pub_.publish(contours_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::FlowArrayStamped flows_msg;
flows_msg.header = msg->header;
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
if (need_config_update_) {
srv.updateConfig(config_);
need_config_update_ = false;
}
}
// Do the work
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, gray, cv::COLOR_BGR2GRAY );
} else {
frame.copyTo(gray);
}
if( prevgray.data )
{
cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR);
//drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0));
int step = 16;
cv::Scalar color = cv::Scalar(0, 255, 0);
for(int y = 0; y < cflow.rows; y += step)
for(int x = 0; x < cflow.cols; x += step)
{
const cv::Point2f& fxy = flow.at<cv::Point2f>(y, x);
cv::line(cflow, cv::Point(x,y), cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)),
color);
cv::circle(cflow, cv::Point(x,y), 2, color, -1);
opencv_apps::Flow flow_msg;
opencv_apps::Point2D point_msg;
opencv_apps::Point2D velocity_msg;
point_msg.x = x;
point_msg.y = y;
velocity_msg.x = fxy.x;
velocity_msg.y = fxy.y;
flow_msg.point = point_msg;
flow_msg.velocity = velocity_msg;
flows_msg.flow.push_back(flow_msg);
}
}
std::swap(prevgray, gray);
//-- Show what you got
if( debug_view_) {
cv::imshow( window_name_, cflow );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg();
img_pub_.publish(out_img);
msg_pub_.publish(flows_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
void onEnableController(const std_msgs::Bool::ConstPtr& msg) {
setEnabled(msg->data);
current_cfg.enable = msg->data;
reconfigure_server.updateConfig(current_cfg);
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::RotatedRectArrayStamped rects_msg, ellipses_msg;
rects_msg.header = msg->header;
ellipses_msg.header = msg->header;
// Do the work
cv::Mat src_gray;
/// Convert image to gray and blur it
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
} else {
src_gray = frame;
}
cv::blur( src_gray, src_gray, cv::Size(3,3) );
/// Create window
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
}
cv::Mat threshold_output;
int max_thresh = 255;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::RNG rng(12345);
/// Detect edges using Threshold
cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
/// Find contours
cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
/// Find the rotated rectangles and ellipses for each contour
std::vector<cv::RotatedRect> minRect( contours.size() );
std::vector<cv::RotatedRect> minEllipse( contours.size() );
for( size_t i = 0; i < contours.size(); i++ )
{ minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) );
if( contours[i].size() > 5 )
{ minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); }
}
/// Draw contours + rotated rects + ellipses
cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
for( size_t i = 0; i< contours.size(); i++ )
{
cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
// contour
cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
// ellipse
cv::ellipse( drawing, minEllipse[i], color, 2, 8 );
// rotated rectangle
cv::Point2f rect_points[4]; minRect[i].points( rect_points );
for( int j = 0; j < 4; j++ )
cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 );
opencv_apps::RotatedRect rect_msg;
opencv_apps::Point2D rect_center;
opencv_apps::Size rect_size;
rect_center.x = minRect[i].center.x;
rect_center.y = minRect[i].center.y;
rect_size.width = minRect[i].size.width;
rect_size.height = minRect[i].size.height;
rect_msg.center = rect_center;
rect_msg.size = rect_size;
rect_msg.angle = minRect[i].angle;
opencv_apps::RotatedRect ellipse_msg;
opencv_apps::Point2D ellipse_center;
opencv_apps::Size ellipse_size;
ellipse_center.x = minEllipse[i].center.x;
ellipse_center.y = minEllipse[i].center.y;
ellipse_size.width = minEllipse[i].size.width;
ellipse_size.height = minEllipse[i].size.height;
ellipse_msg.center = ellipse_center;
ellipse_msg.size = ellipse_size;
ellipse_msg.angle = minEllipse[i].angle;
rects_msg.rects.push_back(rect_msg);
ellipses_msg.rects.push_back(ellipse_msg);
}
/// Create a Trackbar for user to enter threshold
if( debug_view_) {
if (need_config_update_) {
config_.threshold = threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
cv::imshow( window_name_, drawing );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
img_pub_.publish(out_img);
rects_pub_.publish(rects_msg);
ellipses_pub_.publish(ellipses_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::ContourArrayStamped contours_msg;
contours_msg.header = msg->header;
// Do the work
cv::Mat src_gray;
/// Convert image to gray and blur it
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
} else {
src_gray = frame;
}
cv::blur( src_gray, src_gray, cv::Size(3,3) );
/// Create window
if( debug_view_) {
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
}
cv::Mat threshold_output;
int max_thresh = 255;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::RNG rng(12345);
/// Detect edges using Threshold
cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
/// Find contours
cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
/// Find the convex hull object for each contour
std::vector<std::vector<cv::Point> >hull( contours.size() );
for( size_t i = 0; i < contours.size(); i++ )
{ cv::convexHull( cv::Mat(contours[i]), hull[i], false ); }
/// Draw contours + hull results
cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
for( size_t i = 0; i< contours.size(); i++ )
{
cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
cv::drawContours( drawing, hull, (int)i, color, 4, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
opencv_apps::Contour contour_msg;
for ( size_t j = 0; j < hull[i].size(); j++ ) {
opencv_apps::Point2D point_msg;
point_msg.x = hull[i][j].x;
point_msg.y = hull[i][j].y;
contour_msg.points.push_back(point_msg);
}
contours_msg.contours.push_back(contour_msg);
}
/// Create a Trackbar for user to enter threshold
if( debug_view_) {
if (need_config_update_) {
config_.threshold = threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
cv::imshow( window_name_, drawing );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
img_pub_.publish(out_img);
msg_pub_.publish(contours_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
{
// Work on the image.
try
{
// Convert the image into something opencv can handle.
cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
// Messages
opencv_apps::LineArrayStamped lines_msg;
lines_msg.header = msg->header;
// Do the work
std::vector<cv::Rect> faces;
cv::Mat src_gray, edges;
if ( frame.channels() > 1 ) {
cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
} else {
src_gray = frame;
}
/// Apply Canny edge detector
Canny( src_gray, edges, 50, 200, 3 );
if( debug_view_) {
/// Create Trackbars for Thresholds
char thresh_label[50];
sprintf( thresh_label, "Thres: %d + input", min_threshold_ );
cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
cv::createTrackbar( thresh_label, window_name_, &threshold_, max_threshold_, trackbarCallback);
if (need_config_update_) {
config_.threshold = threshold_;
srv.updateConfig(config_);
need_config_update_ = false;
}
}
/// Initialize
cv::cvtColor( edges, frame, cv::COLOR_GRAY2BGR );
switch (config_.hough_type) {
case hough_lines::HoughLines_Standard_Hough_Transform:
{
std::vector<cv::Vec2f> s_lines;
/// 1. Use Standard Hough Transform
cv::HoughLines( edges, s_lines, 1, CV_PI/180, min_threshold_ + threshold_, 0, 0 );
/// Show the result
for( size_t i = 0; i < s_lines.size(); i++ )
{
float r = s_lines[i][0], t = s_lines[i][1];
double cos_t = cos(t), sin_t = sin(t);
double x0 = r*cos_t, y0 = r*sin_t;
double alpha = 1000;
cv::Point pt1( cvRound(x0 + alpha*(-sin_t)), cvRound(y0 + alpha*cos_t) );
cv::Point pt2( cvRound(x0 - alpha*(-sin_t)), cvRound(y0 - alpha*cos_t) );
#ifndef CV_VERSION_EPOCH
cv::line( frame, pt1, pt2, cv::Scalar(255,0,0), 3, cv::LINE_AA);
#else
cv::line( frame, pt1, pt2, cv::Scalar(255,0,0), 3, CV_AA);
#endif
opencv_apps::Line line_msg;
line_msg.pt1.x = pt1.x;
line_msg.pt1.y = pt1.y;
line_msg.pt2.x = pt2.x;
line_msg.pt2.y = pt2.y;
lines_msg.lines.push_back(line_msg);
}
break;
}
case hough_lines::HoughLines_Probabilistic_Hough_Transform:
default:
{
std::vector<cv::Vec4i> p_lines;
/// 2. Use Probabilistic Hough Transform
cv::HoughLinesP( edges, p_lines, 1, CV_PI/180, min_threshold_ + threshold_, 30, 10 );
/// Show the result
for( size_t i = 0; i < p_lines.size(); i++ )
{
cv::Vec4i l = p_lines[i];
#ifndef CV_VERSION_EPOCH
cv::line( frame, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, cv::LINE_AA);
#else
cv::line( frame, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, CV_AA);
#endif
opencv_apps::Line line_msg;
line_msg.pt1.x = l[0];
line_msg.pt1.y = l[1];
line_msg.pt2.x = l[2];
line_msg.pt2.y = l[3];
lines_msg.lines.push_back(line_msg);
}
break;
}
}
//-- Show what you got
if( debug_view_) {
cv::imshow( window_name_, frame );
int c = cv::waitKey(1);
}
// Publish the image.
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
img_pub_.publish(out_img);
msg_pub_.publish(lines_msg);
}
catch (cv::Exception &e)
{
NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}
prev_stamp_ = msg->header.stamp;
}