grabcut_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_perception/grabcut.h"
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 #include <opencv2/opencv.hpp>
00040 
00041 namespace jsk_perception
00042 {
00043   void GrabCut::onInit()
00044   {
00045     DiagnosticNodelet::onInit();
00046     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00047     dynamic_reconfigure::Server<Config>::CallbackType f =
00048       boost::bind (
00049         &GrabCut::configCallback, this, _1, _2);
00050     srv_->setCallback (f);
00051     pub_foreground_
00052       = advertise<sensor_msgs::Image>(*pnh_, "output/foreground", 1);
00053     pub_background_
00054       = advertise<sensor_msgs::Image>(*pnh_, "output/background", 1);
00055     pub_foreground_mask_
00056       = advertise<sensor_msgs::Image>(*pnh_, "output/foreground_mask", 1);
00057     pub_background_mask_
00058       = advertise<sensor_msgs::Image>(*pnh_, "output/background_mask", 1);
00059     onInitPostProcess();
00060   }
00061 
00062   void GrabCut::subscribe()
00063   {
00064     sub_image_.subscribe(*pnh_, "input", 1);
00065     sub_foreground_.subscribe(*pnh_, "input/foreground", 1);
00066     sub_background_.subscribe(*pnh_, "input/background", 1);
00067     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00068     sync_->connectInput(sub_image_, sub_foreground_, sub_background_);
00069     sync_->registerCallback(boost::bind(&GrabCut::segment,
00070                                         this, _1, _2, _3));
00071   }
00072 
00073   void GrabCut::unsubscribe()
00074   {
00075     sub_image_.unsubscribe();
00076     sub_foreground_.unsubscribe();
00077     sub_background_.unsubscribe();
00078   }
00079 
00080   void GrabCut::updateDiagnostic(
00081       diagnostic_updater::DiagnosticStatusWrapper &stat)
00082   {
00083     // foreground and background is not continuous,
00084     // so just say OK
00085     stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00086                  "GrabCut running");
00087   }
00088 
00089   void GrabCut::segment(
00090     const sensor_msgs::Image::ConstPtr& image_msg,
00091     const sensor_msgs::Image::ConstPtr& foreground_msg,
00092     const sensor_msgs::Image::ConstPtr& background_msg)
00093   {
00094     boost::mutex::scoped_lock lock(mutex_);
00095     cv::Mat input;
00096     cv::Mat in_image = cv_bridge::toCvShare(image_msg,
00097                                             image_msg->encoding)->image;
00098     if (in_image.channels() == 3) {
00099       input = in_image;
00100     }
00101     else if (in_image.channels() == 1) {
00102       input = cv::Mat::zeros(in_image.rows, in_image.cols, CV_8UC3);
00103       for (size_t j = 0; j < in_image.rows; ++j) {
00104         for (size_t i = 0; i < in_image.cols; ++i) {
00105           input.at<cv::Vec3b>(j, i) = cv::Vec3b(in_image.at<uchar>(j, i),
00106                                                 in_image.at<uchar>(j, i),
00107                                                 in_image.at<uchar>(j, i));
00108         }
00109       }
00110     }
00111     cv::Mat foreground = cv_bridge::toCvCopy(
00112       foreground_msg, sensor_msgs::image_encodings::MONO8)->image;
00113     cv::Mat background = cv_bridge::toCvCopy(
00114       background_msg, sensor_msgs::image_encodings::MONO8)->image;
00115     if (!(input.cols == foreground.cols &&
00116           input.rows == foreground.rows &&
00117           background.cols == foreground.cols &&
00118           background.rows == foreground.rows)) {
00119       NODELET_WARN("size of image is not corretct");
00120       return;
00121     }
00122     cv::Mat mask = cv::Mat::zeros(input.size(), CV_8UC1);
00123     mask.setTo(cv::Scalar::all(cv::GC_PR_BGD));
00124     for (size_t j = 0; j < input.rows; j++) {
00125       for (size_t i = 0; i < input.cols; i++) {
00126         if (foreground.at<uchar>(j, i) == 255) {
00127           if (use_probable_pixel_seed_) {
00128             mask.at<uchar>(j, i) = cv::GC_PR_FGD;
00129           }
00130           else {
00131             mask.at<uchar>(j, i) = cv::GC_FGD;
00132           }
00133         }
00134         if (background.at<uchar>(j, i) == 255) {
00135           if (use_probable_pixel_seed_) {
00136             mask.at<uchar>(j, i) = cv::GC_PR_BGD;
00137           }
00138           else {
00139             mask.at<uchar>(j, i) = cv::GC_BGD;
00140           }
00141         }
00142       }
00143     }
00144     cv::Rect roi;
00145     cv::Mat bgd_model;
00146     cv::Mat fgd_model;
00147     
00148     cv::grabCut(input, mask, roi, bgd_model, fgd_model, 5, cv::GC_INIT_WITH_MASK);
00149     cv::Mat bgd, fgd, bgd_mask, fgd_mask;
00150     // model -> mask
00151     //cv::compare(mask, cv::GC_BGD, bgd_mask, cv::CMP_EQ);
00152     bgd_mask = (mask == cv::GC_BGD) | (mask == cv::GC_PR_BGD);
00153     //cv::compare(mask, cv::GC_FGD, fgd_mask, cv::CMP_EQ);
00154     fgd_mask = (mask == cv::GC_FGD) | (mask == cv::GC_PR_FGD);
00155     // mask -> image
00156     input.copyTo(bgd, bgd_mask);
00157     input.copyTo(fgd, fgd_mask);
00158     cv_bridge::CvImage fg_bridge(
00159       image_msg->header, sensor_msgs::image_encodings::BGR8, fgd);
00160     cv_bridge::CvImage bg_bridge(
00161       image_msg->header, sensor_msgs::image_encodings::BGR8, bgd);
00162     cv_bridge::CvImage fg_mask_bridge(
00163       image_msg->header, sensor_msgs::image_encodings::MONO8, fgd_mask);
00164     cv_bridge::CvImage bg_mask_bridge(
00165       image_msg->header, sensor_msgs::image_encodings::MONO8, bgd_mask);
00166     pub_foreground_.publish(fg_bridge.toImageMsg());
00167     pub_background_.publish(bg_bridge.toImageMsg());
00168     pub_foreground_mask_.publish(fg_mask_bridge.toImageMsg());
00169     pub_background_mask_.publish(bg_mask_bridge.toImageMsg());
00170   }
00171 
00172   void GrabCut::configCallback(Config &config, uint32_t level)
00173   {
00174     boost::mutex::scoped_lock lock(mutex_);
00175     use_probable_pixel_seed_ = (config.seed_pixel_policy == 1);
00176   }
00177   
00178 }
00179 
00180 #include <pluginlib/class_list_macros.h>
00181 PLUGINLIB_EXPORT_CLASS (jsk_perception::GrabCut, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23