39 #include <opencv2/opencv.hpp> 45 DiagnosticNodelet::onInit();
46 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
47 dynamic_reconfigure::Server<Config>::CallbackType
f =
50 srv_->setCallback (f);
52 = advertise<sensor_msgs::Image>(*
pnh_,
"output/foreground", 1);
54 = advertise<sensor_msgs::Image>(*
pnh_,
"output/background", 1);
56 = advertise<sensor_msgs::Image>(*
pnh_,
"output/foreground_mask", 1);
58 = advertise<sensor_msgs::Image>(*
pnh_,
"output/background_mask", 1);
67 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
85 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
90 const sensor_msgs::Image::ConstPtr& image_msg,
91 const sensor_msgs::Image::ConstPtr& foreground_msg,
92 const sensor_msgs::Image::ConstPtr& background_msg)
97 image_msg->encoding)->image;
98 if (in_image.channels() == 3) {
101 else if (in_image.channels() == 1) {
102 input = cv::Mat::zeros(in_image.rows, in_image.cols, CV_8UC3);
103 for (
size_t j = 0; j < in_image.rows; ++j) {
104 for (
size_t i = 0; i < in_image.cols; ++i) {
105 input.at<cv::Vec3b>(j, i) = cv::Vec3b(in_image.at<uchar>(j, i),
106 in_image.at<uchar>(j, i),
107 in_image.at<uchar>(j, i));
115 if (!(input.cols == foreground.cols &&
116 input.rows == foreground.rows &&
117 background.cols == foreground.cols &&
118 background.rows == foreground.rows)) {
122 cv::Mat mask = cv::Mat::zeros(input.size(), CV_8UC1);
123 mask.setTo(cv::Scalar::all(cv::GC_PR_BGD));
124 for (
size_t j = 0; j < input.rows; j++) {
125 for (
size_t i = 0; i < input.cols; i++) {
126 if (foreground.at<uchar>(j, i) == 255) {
128 mask.at<uchar>(j, i) = cv::GC_PR_FGD;
131 mask.at<uchar>(j, i) = cv::GC_FGD;
134 if (background.at<uchar>(j, i) == 255) {
136 mask.at<uchar>(j, i) = cv::GC_PR_BGD;
139 mask.at<uchar>(j, i) = cv::GC_BGD;
148 cv::grabCut(input, mask, roi, bgd_model, fgd_model, 5, cv::GC_INIT_WITH_MASK);
149 cv::Mat bgd, fgd, bgd_mask, fgd_mask;
152 bgd_mask = (mask == cv::GC_BGD) | (mask == cv::GC_PR_BGD);
154 fgd_mask = (mask == cv::GC_FGD) | (mask == cv::GC_PR_FGD);
156 input.copyTo(bgd, bgd_mask);
157 input.copyTo(fgd, fgd_mask);
message_filters::Subscriber< sensor_msgs::Image > sub_foreground_
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
void summary(unsigned char lvl, const std::string s)
virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
bool use_probable_pixel_seed_
message_filters::Subscriber< sensor_msgs::Image > sub_background_
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
ros::Publisher pub_background_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void unsubscribe()
virtual void segment(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::Image::ConstPtr &foreground_msg, const sensor_msgs::Image::ConstPtr &background_msg)
ros::Publisher pub_foreground_
PLUGINLIB_EXPORT_CLASS(jsk_perception::GrabCut, nodelet::Nodelet)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
ros::Publisher pub_background_mask_
ros::Publisher pub_foreground_mask_
sensor_msgs::ImagePtr toImageMsg() const