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