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
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);