Go to the documentation of this file.00001 #include "fill_holes_alg_node.h"
00002
00003 using namespace cv;
00004
00005 FillHolesAlgNode::FillHolesAlgNode(void) :
00006 algorithm_base::IriBaseAlgorithm<FillHolesAlgorithm>()
00007 {
00008
00009
00010
00011
00012 this->image_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("image_out", 1);
00013
00014
00015 this->image_in_subscriber_ = this->public_node_handle_.subscribe("image_in", 1, &FillHolesAlgNode::image_in_callback, this);
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 }
00026
00027 FillHolesAlgNode::~FillHolesAlgNode(void)
00028 {
00029
00030 }
00031
00032 void FillHolesAlgNode::mainNodeThread(void)
00033 {
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 }
00044
00045
00046 void FillHolesAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg)
00047 {
00048 cv_bridge::CvImagePtr cv_ptr;
00049
00050 try
00051 {
00052 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00053 }
00054 catch (cv_bridge::Exception& e)
00055 {
00056 ROS_ERROR("cv_bridge exception: %s", e.what());
00057 return;
00058 }
00059
00060 cv_ptr->image = floodfillRemoval(cv_ptr->image);
00061
00062 ROS_DEBUG_STREAM("Publishing floodfill");
00063
00064 this->image_out_publisher_.publish(cv_ptr->toImageMsg());
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 }
00076
00077
00078
00079
00080
00081
00082
00083
00084 cv::Mat FillHolesAlgNode::floodfillRemoval(cv::Mat image)
00085 {
00086 Mat src_gray, res;
00087 Mat threshold_output;
00088 vector<vector<Point> > contours;
00089 vector<Vec4i> hierarchy;
00090 int thresh = 10;
00091 cvtColor( image, src_gray, CV_BGR2GRAY );
00092 blur( src_gray, src_gray, Size(3,3) );
00093
00095 threshold( src_gray, threshold_output, thresh, 255, THRESH_BINARY );
00096
00098 findContours( threshold_output, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
00099
00101 vector<vector<Point> >hull( contours.size() );
00102
00104 Mat drawing = Mat::zeros( threshold_output.size(), CV_8UC3 );
00105 for( size_t i = 0; i< contours.size(); i++ )
00106 {
00107 Scalar color = Scalar( 255, 255, 255 );
00108 drawContours( drawing, contours, i, color, CV_FILLED, 8, vector<Vec4i>(), 0, Point() );
00109 }
00110
00111
00112
00113
00114 return drawing;
00115 }
00116
00117 void FillHolesAlgNode::node_config_update(Config &config, uint32_t level)
00118 {
00119 this->alg_.lock();
00120
00121
00122 this->alg_.config_=config;
00123
00124 this->alg_.unlock();
00125 }
00126
00127 void FillHolesAlgNode::addNodeDiagnostics(void)
00128 {
00129 }
00130
00131
00132 int main(int argc,char *argv[])
00133 {
00134 return algorithm_base::main<FillHolesAlgNode>(argc, argv, "fill_holes_alg_node");
00135 }