saliency_map_generator_node.cpp
Go to the documentation of this file.
00001 
00002 #include <jsk_perception/saliency_map_generator.h>
00003 
00004 namespace jsk_perception
00005 {
00006    void SaliencyMapGenerator::onInit()
00007    {
00008       DiagnosticNodelet::onInit();
00009       pnh_->getParam("num_threads", this->num_threads_);
00010       pnh_->getParam("fps", this->print_fps_);
00011       this->pub_image_ = advertise<sensor_msgs::Image>(*pnh_,
00012          "/saliency_map_generator/output/saliency_map", 1);
00013       onInitPostProcess();
00014    }
00015 
00016    void SaliencyMapGenerator::subscribe()
00017    {
00018       this->sub_image_ = pnh_->subscribe(
00019          "input", 1, &SaliencyMapGenerator::callback, this);
00020    }
00021 
00022    void SaliencyMapGenerator::unsubscribe()
00023    {
00024       NODELET_DEBUG("Unsubscribing from ROS topic.");
00025       this->sub_image_.shutdown();
00026    }
00027    
00028    void SaliencyMapGenerator::callback(
00029       const sensor_msgs::Image::ConstPtr &image_msg)
00030    {      
00031       cv::Mat image = cv_bridge::toCvShare(
00032          image_msg, image_msg->encoding)->image;
00033       if (image.empty()) {
00034          return;
00035       }
00036       if (image.channels() == 3) {
00037          cv::cvtColor(image, image, CV_BGR2GRAY);
00038       }
00039       if (this->counter_ == 0) {
00040          this->start_ = omp_get_wtime();
00041       }
00042       
00043       cv::Mat saliency_map;
00044       this->computeSaliencyImpl(image, saliency_map);
00045       cv::cvtColor(saliency_map, saliency_map, CV_GRAY2BGR);
00046 
00047       if (this->print_fps_) {
00048          this->counter_++;
00049          double sec = (omp_get_wtime() - this->start_) /
00050             static_cast<double>(this->num_threads_);
00051          double fps = static_cast<double>(this->counter_) / sec;
00052          fps = ceil(fps * 100) / 100;      
00053          if (this->counter_ == (INT_MAX)) {
00054             this->counter_ = 0;
00055          }
00056          cv::putText(saliency_map, "FPS: " + boost::lexical_cast<std::string>(fps),
00057                      cv::Point(30, 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8,
00058                      cv::Scalar(200, 200, 250), 1, CV_AA);
00059       }
00060       cv_bridge::CvImage pub_img(image_msg->header,
00061                                  image_msg->encoding,
00062                                  saliency_map);
00063       this->pub_image_.publish(pub_img.toImageMsg());
00064    }
00065 
00066    bool SaliencyMapGenerator::computeSaliencyImpl(
00067       cv::Mat image, cv::Mat &saliencyMap)
00068    {
00069       if (image.empty()) {
00070          return false;
00071       }
00072       cv::Mat dst(cv::Size(image.cols, image.rows), CV_8UC1);
00073       calcIntensityChannel(image, dst);
00074       saliencyMap = cv::Mat::zeros(image.size(), CV_8UC1);
00075       dst.copyTo(saliencyMap);
00076       return true;
00077    }
00078 
00079    void SaliencyMapGenerator::setNumThreads(int num_threads)
00080    {
00081       this->num_threads_ = num_threads;
00082    }
00083 
00084    void SaliencyMapGenerator::copyImage(cv::Mat srcArg, cv::Mat dstArg)
00085    {
00086       srcArg.copyTo(dstArg);
00087    }
00088 
00089    void SaliencyMapGenerator::calcIntensityChannel(
00090       cv::Mat srcArg, cv::Mat dstArg)
00091    {
00092       if (dstArg.channels() > 1) {
00093          return;
00094       }
00095       const int numScales = 6;
00096       cv::Mat intensityScaledOn[numScales];
00097       cv::Mat intensityScaledOff[numScales];
00098       cv::Mat gray = cv::Mat::zeros(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
00099       cv::Mat integralImage(cv::Size(srcArg.cols + 1, srcArg.rows + 1), CV_32FC1);
00100       cv::Mat intensity(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
00101       cv::Mat intensityOn(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
00102       cv::Mat intensityOff(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
00103 
00104       int i;
00105       int neighborhood;
00106       int neighborhoods[] = {3*4, 3*4*2, 3*4*2*2, 7*4, 7*4*2, 7*4*2*2};
00107 
00108       for (i = 0; i < numScales; i++) {
00109          intensityScaledOn[i] = cv::Mat(cv::Size(
00110                                            srcArg.cols,
00111                                            srcArg.rows), CV_8UC1);
00112          intensityScaledOff[i] = cv::Mat(cv::Size(
00113                                             srcArg.cols, srcArg.rows), CV_8UC1);
00114       }
00115       if (srcArg.channels() == 3) {
00116          cv::cvtColor(srcArg, gray, cv::COLOR_BGR2GRAY);
00117       } else {
00118          srcArg.copyTo(gray);
00119       }
00120       cv::GaussianBlur(gray, gray, cv::Size(3, 3), 0, 0);
00121       cv::GaussianBlur(gray, gray, cv::Size(3, 3), 0, 0);
00122       cv::integral(gray, integralImage, CV_32F);
00123       for (i=0; i < numScales; i++) {
00124          neighborhood = neighborhoods[i];
00125          getIntensityScaled(integralImage, gray,
00126                             intensityScaledOn[i],
00127                             intensityScaledOff[i],
00128                             neighborhood);
00129       }
00130       mixScales(intensityScaledOn, intensityOn,
00131                 intensityScaledOff,
00132                 intensityOff,
00133                 numScales);
00134       mixOnOff(intensityOn, intensityOff, intensity);
00135       intensity.copyTo(dstArg);
00136    }
00137 
00138    void SaliencyMapGenerator::getIntensityScaled(
00139       cv::Mat integralImage, cv::Mat gray, cv::Mat intensityScaledOn,
00140       cv::Mat intensityScaledOff, int neighborhood)
00141    {
00142       float value, meanOn, meanOff;
00143       intensityScaledOn.setTo(cv::Scalar::all(0));
00144       intensityScaledOff.setTo(cv::Scalar::all(0));
00145 #ifdef _OPENMP
00146 #pragma omp parallel for collapse(2)            \
00147    private(value, meanOn, meanOff)              \
00148    num_threads(num_threads_)
00149 #endif
00150       for (int y = 0; y < gray.rows; y++) {
00151          for (int x = 0; x < gray.cols; x++) {
00152             cv::Point2i point;
00153             point.x = x;
00154             point.y = y;
00155             value = getMean(integralImage,
00156                             point, neighborhood, gray.at<uchar>(y, x));
00157             meanOn = gray.at<uchar>(y, x) - value;
00158             meanOff = value - gray.at<uchar>(y, x);
00159             if (meanOn > 0)
00160                intensityScaledOn.at<uchar>(y, x) = (uchar)meanOn;
00161             else
00162                intensityScaledOn.at<uchar>(y, x) = 0;
00163 
00164             if (meanOff > 0)
00165                intensityScaledOff.at<uchar>(y, x) = (uchar)meanOff;
00166             else
00167                intensityScaledOff.at<uchar>(y, x) = 0;
00168          }
00169       }
00170    }
00171 
00172    float SaliencyMapGenerator::getMean(
00173       cv::Mat srcArg, cv::Point2i PixArg, int neighbourhood, int centerVal)
00174    {
00175       cv::Point2i P1, P2;
00176       float value;
00177 
00178       P1.x = PixArg.x - neighbourhood + 1;
00179       P1.y = PixArg.y - neighbourhood + 1;
00180       P2.x = PixArg.x + neighbourhood + 1;
00181       P2.y = PixArg.y + neighbourhood + 1;
00182 
00183       if (P1.x < 0)
00184          P1.x = 0;
00185       else if (P1.x > srcArg.cols - 1)
00186          P1.x = srcArg.cols - 1;
00187       if (P2.x < 0)
00188          P2.x = 0;
00189       else if (P2.x > srcArg.cols - 1)
00190          P2.x = srcArg.cols - 1;
00191       if (P1.y < 0)
00192          P1.y = 0;
00193       else if (P1.y > srcArg.rows - 1)
00194          P1.y = srcArg.rows - 1;
00195       if (P2.y < 0)
00196          P2.y = 0;
00197       else if (P2.y > srcArg.rows - 1)
00198          P2.y = srcArg.rows - 1;
00199 
00200       // we use the integral image to compute fast features
00201       value = static_cast<float> (
00202          (srcArg.at<float>(P2.y, P2.x)) +
00203          (srcArg.at<float>(P1.y, P1.x)) -
00204          (srcArg.at<float>(P2.y, P1.x)) -
00205          (srcArg.at<float>(P1.y, P2.x)));
00206       value = (value - centerVal)/  (( (P2.x - P1.x) * (P2.y - P1.y))-1);
00207       return value;
00208    }
00209 
00210    void SaliencyMapGenerator::mixScales(
00211       cv::Mat *intensityScaledOn, cv::Mat intensityOn,
00212       cv::Mat *intensityScaledOff, cv::Mat intensityOff, const int numScales)
00213    {
00214       int i = 0;
00215       int width = intensityScaledOn[0].cols;
00216       int height = intensityScaledOn[0].rows;
00217       short int maxValOn = 0, currValOn = 0;
00218       short int maxValOff = 0, currValOff = 0;
00219       int maxValSumOff = 0, maxValSumOn = 0;
00220       cv::Mat mixedValuesOn(cv::Size(width, height), CV_16UC1);
00221       cv::Mat mixedValuesOff(cv::Size(width, height), CV_16UC1);
00222       mixedValuesOn.setTo(cv::Scalar::all(0));
00223       mixedValuesOff.setTo(cv::Scalar::all(0));
00224 #ifdef _OPENMP
00225 #pragma omp parallel for collapse(3)                                    \
00226    private(i, maxValOn, currValOn, maxValOff, currValOff, maxValSumOn, maxValSumOff) \
00227    num_threads(num_threads_) 
00228 #endif
00229       for (i = 0; i < numScales; i++) {
00230          for (int y = 0; y < height; y++)
00231             for (int x = 0; x < width; x++) {
00232                currValOn = intensityScaledOn[i].at<uchar>(y, x);
00233                if (currValOn > maxValOn)
00234                   maxValOn = currValOn;
00235 
00236                currValOff = intensityScaledOff[i].at<uchar>(y, x);
00237                if (currValOff > maxValOff)
00238                   maxValOff = currValOff;
00239                mixedValuesOn.at<unsigned short>(y, x) += currValOn;
00240                mixedValuesOff.at<unsigned short>(y, x) += currValOff;
00241             }
00242       }
00243 #ifdef _OPENMP
00244 #pragma omp parallel for collapse(2)                        \
00245    shared(currValOn, currValOff, maxValSumOn, maxValSumOff) \
00246    num_threads(num_threads_) 
00247 #endif
00248       for (int y = 0; y < height; y++)
00249          for (int x = 0; x < width; x++) {
00250             currValOn = mixedValuesOn.at<unsigned short>(y, x);
00251             currValOff = mixedValuesOff.at<unsigned short>(y, x);
00252             if (currValOff > maxValSumOff)
00253                maxValSumOff = currValOff;
00254             if (currValOn > maxValSumOn)
00255                maxValSumOn = currValOn;
00256          }
00257 #ifdef _OPENMP
00258 #pragma omp parallel for collapse(2) num_threads(num_threads_) 
00259 #endif
00260       for (int y = 0; y < height; y++)
00261          for (int x = 0; x < width; x++) {
00262             intensityOn.at<uchar>(y, x) = (uchar)(
00263                255.*((float)(mixedValuesOn.at<unsigned short>(y, x) / (float)maxValSumOn)));
00264             intensityOff.at<uchar>(y, x) = (uchar)(
00265                255.*((float)(mixedValuesOff.at<unsigned short>(y, x) / (float)maxValSumOff)));
00266          }
00267 
00268    }
00269 
00270    void SaliencyMapGenerator::mixOnOff(
00271       cv::Mat intensityOn, cv::Mat intensityOff, cv::Mat intensityArg)
00272    {
00273       int width = intensityOn.cols;
00274       int height = intensityOn.rows;
00275       int maxVal = 0;
00276       int currValOn, currValOff, maxValSumOff, maxValSumOn;
00277       cv::Mat intensity(cv::Size(width, height), CV_8UC1);
00278       maxValSumOff = 0;
00279       maxValSumOn = 0;
00280 #ifdef _OPENMP
00281 #pragma omp parallel for collapse(2)                                \
00282    shared(currValOn, currValOff, maxValSumOn, maxValSumOff, maxVal) \
00283    num_threads(num_threads_) 
00284 #endif
00285       for (int y = 0; y < height; y++) {
00286          for (int x = 0; x < width; x++) {
00287             currValOn = intensityOn.at<uchar>(y, x);
00288             currValOff = intensityOff.at<uchar>(y, x);
00289             if (currValOff > maxValSumOff) {
00290                maxValSumOff = currValOff;
00291             }
00292             if (currValOn > maxValSumOn) {
00293                maxValSumOn = currValOn;
00294             }
00295          }
00296       }
00297       if (maxValSumOn > maxValSumOff) {
00298          maxVal = maxValSumOn;
00299       } else {
00300          maxVal = maxValSumOff;
00301       }
00302 #ifdef _OPENMP
00303 #pragma omp parallel for collapse(2) num_threads(num_threads_) 
00304 #endif
00305       for (int y = 0; y < height; y++) {
00306          for (int x = 0; x < width; x++) {
00307             intensity.at<uchar>(y, x) = (uchar) (
00308                255. * (float) (intensityOn.at<uchar>(y, x) +
00309                                intensityOff.at<uchar>(y, x)) / (float)maxVal);
00310          }
00311       }
00312       intensity.copyTo(intensityArg);
00313    }
00314 }
00315 
00316 #include <pluginlib/class_list_macros.h>
00317 PLUGINLIB_EXPORT_CLASS (jsk_perception::SaliencyMapGenerator, nodelet::Nodelet);


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