37 #include <boost/assign.hpp> 38 #if ( CV_MAJOR_VERSION >= 4) 39 #include <opencv2/imgproc/imgproc_c.h> 47 DiagnosticNodelet::onInit();
49 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
50 dynamic_reconfigure::Server<Config>::CallbackType
f =
54 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
81 cv::Mat mask = cv_ptr->image;
84 std::vector<std::vector<cv::Point> > contours;
85 std::vector<cv::Vec4i> hierarchy;
86 cv::findContours(mask, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
90 max_area = mask.cols * mask.rows;
96 std::vector<std::vector<cv::Point> > pruned_contours;
97 for (
size_t i = 0; i < contours.size(); i++) {
98 double area = cv::contourArea(contours[i]);
99 if (
min_area_ <= area && area <= max_area) {
100 pruned_contours.push_back(contours[i]);
105 std::vector<std::vector<cv::Point> > smoothed_contours;
106 smoothed_contours.resize(pruned_contours.size());
107 for (
size_t i = 0; i < pruned_contours.size(); i++) {
108 std::vector<float>
x;
109 std::vector<float>
y;
110 const size_t n = pruned_contours[i].size();
112 for (
size_t j = 0; j < n; j++) {
113 x.push_back(pruned_contours[i][j].x);
114 y.push_back(pruned_contours[i][j].y);
118 cv::transpose(cv::getGaussianKernel(11, 4.0, CV_32FC1), G);
120 std::vector<float> x_smooth;
121 std::vector<float> y_smooth;
123 cv::filter2D(x, x_smooth, CV_32FC1, G);
124 cv::filter2D(y, y_smooth, CV_32FC1, G);
126 for (
size_t j = 0; j < n; j++) {
127 smoothed_contours[i].push_back(cv::Point2f(x_smooth[j], y_smooth[j]));
131 cv::Mat concave_hull_mask = cv::Mat::zeros(mask.size(), mask.type());
132 for (
size_t i = 0; i < smoothed_contours.size(); i++) {
133 cv::drawContours(concave_hull_mask, smoothed_contours, i, cv::Scalar(255), CV_FILLED);
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(jsk_perception::ConcaveHullMaskImage, nodelet::Nodelet)
virtual void unsubscribe()
virtual void configCallback(Config &config, uint32_t level)
jsk_perception::ConcaveHullMaskImageConfig Config
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void concave(const sensor_msgs::Image::ConstPtr &mask_msg)
std::vector< std::string > V_string
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
sensor_msgs::ImagePtr toImageMsg() const