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; }