3 #if ( CV_MAJOR_VERSION >= 4) 4 #include <opencv2/imgproc/imgproc_c.h> 11 DiagnosticNodelet::onInit();
15 "/saliency_map_generator/output/saliency_map", 1);
32 const sensor_msgs::Image::ConstPtr &image_msg)
35 image_msg, image_msg->encoding)->image;
39 if (image.channels() == 3) {
40 cv::cvtColor(image, image, CV_BGR2GRAY);
43 this->
start_ = omp_get_wtime();
48 cv::cvtColor(saliency_map, saliency_map, CV_GRAY2BGR);
52 double sec = (omp_get_wtime() - this->
start_) /
54 double fps =
static_cast<double>(this->
counter_) / sec;
55 fps = ceil(fps * 100) / 100;
59 cv::putText(saliency_map,
"FPS: " + boost::lexical_cast<std::string>(fps),
60 cv::Point(30, 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8,
61 cv::Scalar(200, 200, 250), 1, CV_AA);
70 cv::Mat image, cv::Mat &saliencyMap)
75 cv::Mat dst(cv::Size(image.cols, image.rows), CV_8UC1);
77 saliencyMap = cv::Mat::zeros(image.size(), CV_8UC1);
78 dst.copyTo(saliencyMap);
89 srcArg.copyTo(dstArg);
93 cv::Mat srcArg, cv::Mat dstArg)
95 if (dstArg.channels() > 1) {
98 const int numScales = 6;
99 cv::Mat intensityScaledOn[numScales];
100 cv::Mat intensityScaledOff[numScales];
101 cv::Mat gray = cv::Mat::zeros(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
102 cv::Mat integralImage(cv::Size(srcArg.cols + 1, srcArg.rows + 1), CV_32FC1);
103 cv::Mat intensity(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
104 cv::Mat intensityOn(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
105 cv::Mat intensityOff(cv::Size(srcArg.cols, srcArg.rows), CV_8UC1);
109 int neighborhoods[] = {3*4, 3*4*2, 3*4*2*2, 7*4, 7*4*2, 7*4*2*2};
111 for (i = 0; i < numScales; i++) {
112 intensityScaledOn[i] = cv::Mat(cv::Size(
114 srcArg.rows), CV_8UC1);
115 intensityScaledOff[i] = cv::Mat(cv::Size(
116 srcArg.cols, srcArg.rows), CV_8UC1);
118 if (srcArg.channels() == 3) {
119 cv::cvtColor(srcArg, gray, cv::COLOR_BGR2GRAY);
123 cv::GaussianBlur(gray, gray, cv::Size(3, 3), 0, 0);
124 cv::GaussianBlur(gray, gray, cv::Size(3, 3), 0, 0);
125 cv::integral(gray, integralImage, CV_32F);
126 for (i=0; i < numScales; i++) {
127 neighborhood = neighborhoods[i];
129 intensityScaledOn[i],
130 intensityScaledOff[i],
133 mixScales(intensityScaledOn, intensityOn,
137 mixOnOff(intensityOn, intensityOff, intensity);
138 intensity.copyTo(dstArg);
142 cv::Mat integralImage, cv::Mat gray, cv::Mat intensityScaledOn,
143 cv::Mat intensityScaledOff,
int neighborhood)
145 float value, meanOn, meanOff;
146 intensityScaledOn.setTo(cv::Scalar::all(0));
147 intensityScaledOff.setTo(cv::Scalar::all(0));
149 #pragma omp parallel for collapse(2) \ 150 private(value, meanOn, meanOff) \ 151 num_threads(num_threads_) 153 for (
int y = 0;
y < gray.rows;
y++) {
154 for (
int x = 0;
x < gray.cols;
x++) {
159 point, neighborhood, gray.at<uchar>(
y,
x));
160 meanOn = gray.at<uchar>(
y,
x) - value;
161 meanOff = value - gray.at<uchar>(y,
x);
163 intensityScaledOn.at<uchar>(y,
x) = (uchar)meanOn;
165 intensityScaledOn.at<uchar>(y,
x) = 0;
168 intensityScaledOff.at<uchar>(y,
x) = (uchar)meanOff;
170 intensityScaledOff.at<uchar>(y,
x) = 0;
176 cv::Mat srcArg, cv::Point2i PixArg,
int neighbourhood,
int centerVal)
181 P1.x = PixArg.x - neighbourhood + 1;
182 P1.y = PixArg.y - neighbourhood + 1;
183 P2.x = PixArg.x + neighbourhood + 1;
184 P2.y = PixArg.y + neighbourhood + 1;
188 else if (P1.x > srcArg.cols - 1)
189 P1.x = srcArg.cols - 1;
192 else if (P2.x > srcArg.cols - 1)
193 P2.x = srcArg.cols - 1;
196 else if (P1.y > srcArg.rows - 1)
197 P1.y = srcArg.rows - 1;
200 else if (P2.y > srcArg.rows - 1)
201 P2.y = srcArg.rows - 1;
204 value =
static_cast<float> (
205 (srcArg.at<
float>(P2.y, P2.x)) +
206 (srcArg.at<
float>(P1.y, P1.x)) -
207 (srcArg.at<
float>(P2.y, P1.x)) -
208 (srcArg.at<
float>(P1.y, P2.x)));
209 value = (value - centerVal)/ (( (P2.x - P1.x) * (P2.y - P1.y))-1);
214 cv::Mat *intensityScaledOn, cv::Mat intensityOn,
215 cv::Mat *intensityScaledOff, cv::Mat intensityOff,
const int numScales)
218 int width = intensityScaledOn[0].cols;
219 int height = intensityScaledOn[0].rows;
220 short int maxValOn = 0, currValOn = 0;
221 short int maxValOff = 0, currValOff = 0;
222 int maxValSumOff = 0, maxValSumOn = 0;
223 cv::Mat mixedValuesOn(cv::Size(width, height), CV_16UC1);
224 cv::Mat mixedValuesOff(cv::Size(width, height), CV_16UC1);
225 mixedValuesOn.setTo(cv::Scalar::all(0));
226 mixedValuesOff.setTo(cv::Scalar::all(0));
228 #pragma omp parallel for collapse(3) \ 229 private(i, maxValOn, currValOn, maxValOff, currValOff, maxValSumOn, maxValSumOff) \ 230 num_threads(num_threads_) 232 for (i = 0; i < numScales; i++) {
233 for (
int y = 0;
y < height;
y++)
234 for (
int x = 0;
x < width;
x++) {
235 currValOn = intensityScaledOn[i].at<uchar>(
y,
x);
236 if (currValOn > maxValOn)
237 maxValOn = currValOn;
239 currValOff = intensityScaledOff[i].at<uchar>(y,
x);
240 if (currValOff > maxValOff)
241 maxValOff = currValOff;
242 mixedValuesOn.at<
unsigned short>(y,
x) += currValOn;
243 mixedValuesOff.at<
unsigned short>(y,
x) += currValOff;
247 #pragma omp parallel for collapse(2) \ 248 shared(currValOn, currValOff, maxValSumOn, maxValSumOff) \ 249 num_threads(num_threads_) 251 for (
int y = 0;
y < height;
y++)
252 for (
int x = 0;
x < width;
x++) {
253 currValOn = mixedValuesOn.at<
unsigned short>(
y,
x);
254 currValOff = mixedValuesOff.at<
unsigned short>(y,
x);
255 if (currValOff > maxValSumOff)
256 maxValSumOff = currValOff;
257 if (currValOn > maxValSumOn)
258 maxValSumOn = currValOn;
261 #pragma omp parallel for collapse(2) num_threads(num_threads_) 263 for (
int y = 0;
y < height;
y++)
264 for (
int x = 0;
x < width;
x++) {
265 intensityOn.at<uchar>(
y,
x) = (uchar)(
266 255.*((
float)(mixedValuesOn.at<
unsigned short>(y,
x) / (
float)maxValSumOn)));
267 intensityOff.at<uchar>(y,
x) = (uchar)(
268 255.*((
float)(mixedValuesOff.at<
unsigned short>(y,
x) / (
float)maxValSumOff)));
274 cv::Mat intensityOn, cv::Mat intensityOff, cv::Mat intensityArg)
276 int width = intensityOn.cols;
277 int height = intensityOn.rows;
279 int currValOn, currValOff, maxValSumOff, maxValSumOn;
280 cv::Mat intensity(cv::Size(width, height), CV_8UC1);
284 #pragma omp parallel for collapse(2) \ 285 shared(currValOn, currValOff, maxValSumOn, maxValSumOff, maxVal) \ 286 num_threads(num_threads_) 288 for (
int y = 0;
y < height;
y++) {
289 for (
int x = 0;
x < width;
x++) {
290 currValOn = intensityOn.at<uchar>(
y,
x);
291 currValOff = intensityOff.at<uchar>(y,
x);
292 if (currValOff > maxValSumOff) {
293 maxValSumOff = currValOff;
295 if (currValOn > maxValSumOn) {
296 maxValSumOn = currValOn;
300 if (maxValSumOn > maxValSumOff) {
301 maxVal = maxValSumOn;
303 maxVal = maxValSumOff;
306 #pragma omp parallel for collapse(2) num_threads(num_threads_) 308 for (
int y = 0;
y < height;
y++) {
309 for (
int x = 0;
x < width;
x++) {
310 intensity.at<uchar>(
y,
x) = (uchar) (
311 255. * (
float) (intensityOn.at<uchar>(y,
x) +
312 intensityOff.at<uchar>(y,
x)) / (
float)maxVal);
315 intensity.copyTo(intensityArg);
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::Subscriber sub_image_
void publish(const boost::shared_ptr< M > &message) const
float getMean(cv::Mat, cv::Point2i, int, int)
void mixOnOff(cv::Mat intensityOn, cv::Mat intensityOff, cv::Mat intensity)
void copyImage(cv::Mat, cv::Mat)
PLUGINLIB_EXPORT_CLASS(jsk_perception::SaliencyMapGenerator, nodelet::Nodelet)
void getIntensityScaled(cv::Mat, cv::Mat, cv::Mat, cv::Mat, int)
void callback(const sensor_msgs::Image::ConstPtr &)
ros::Publisher pub_image_
void mixScales(cv::Mat *, cv::Mat, cv::Mat *, cv::Mat, const int)
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
bool computeSaliencyImpl(cv::Mat, cv::Mat &)
void calcIntensityChannel(cv::Mat, cv::Mat)