oriented_gradient.cpp
Go to the documentation of this file.
1 // @brief calc oriented gradent
2 // @author Hiroaki Yaguchi, JSK
3 #include "jsk_perception/oriented_gradient.hpp"
4 #include <jsk_topic_tools/log_utils.h>
5 
6 #if ( CV_MAJOR_VERSION >= 4)
7 #include <opencv2/imgproc/types_c.h>
8 #endif
9 
10 namespace jsk_perception {
11 
12 // @brief calc 8 neighbor oriented gradient image
13 // @param src source image
14 // @param dst destination image (HSV, H : orientation, V : intensity)
15 void calcOrientedGradient(cv::Mat& src, cv::Mat& dst) {
16  int width, height;
17  cv::Mat gimg;
18 
19  width = src.cols;
20  height = src.rows;
21 
22  cv::cvtColor(src, gimg, CV_BGR2GRAY);
23 
24  dst.create(height, width, CV_8UC3);
25  for (int y = 0; y < height; y++) {
26  for (int x = 0; x < width; x++) {
27  cv::Vec3b& px=dst.at<cv::Vec3b>(y, x);
28  for (int k = 0; k < 3; k++) {
29  px[k] = 0;
30  }
31  }
32  }
33 
34  float r2 = sqrt(2.0);
35 
36  for (int y = 1; y < height - 1; y++) {
37  for (int x = 1; x < width - 1; x++) {
38  int m,th;
39  float dx,dy;
40  float dxx,dyy,dxy,dyx;
41  double dth;
42 
43  dxx = (float)gimg.at<unsigned char>(y, x + 1)
44  - (float)gimg.at<unsigned char>(y, x - 1);
45  dyy = (float)gimg.at<unsigned char>(y + 1, x)
46  - (float)gimg.at<unsigned char>(y - 1, x);
47  dxy = (float)gimg.at<unsigned char>(y + 1, x + 1)
48  - (float)gimg.at<unsigned char>(y - 1, x - 1);
49  dyx = (float)gimg.at<unsigned char>(y + 1, x - 1)
50  - (float)gimg.at<unsigned char>(y - 1, x + 1);
51 
52  dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
53  dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
54  // dx = dxx; dy = dyy;
55 
56  m = (int)(sqrt(0.5 * (dx * dx + dy * dy)));
57 
58  dth = atan2(dy, dx) * 180.0 / M_PI;
59  if (dth < 0.0) dth += 180.0;
60  if (dth >= 180.0) dth -= 180.0;
61  th = (int)dth;
62 
63  dst.at<cv::Vec3b>(y, x) = cv::Vec3b(th, 255, m);
64 
65  }
66  }
67 }
68 
69 // @brief calc key points from oriented gradient image
70 // @param src source image
71 // @param dst destination image (HSV, H : orientation, V : intensity)
72 // @param result key points
73 // @param thres (optional) threshold of intensity
74 // @param bs (optional) block size
75 void calcOGKeyPoints(cv::Mat& src,
76  cv::Mat& dst,
77  std::vector<cv::Point>& result,
78  int thres,
79  int bs) {
80  calcOrientedGradient(src, dst);
81  int width, height;
82  width = src.cols;
83  height = src.rows;
84 
85  result.clear();
86 
87  for (int y = bs; y < height - bs; y++) {
88  for (int x = bs; x < width - bs; x++) {
89  cv::Vec3b& px0 = dst.at<cv::Vec3b>(y, x);
90  if (px0[2] > thres) {
91  bool iskey = true;
92  for (int dx = -bs; dx <= bs; dx++) {
93  for (int dy = -bs; dy <= bs; dy++) {
94  if (dx == 0 && dy == 0) break;
95  cv::Vec3b& px1 = dst.at<cv::Vec3b>(y + dy, x + dx);
96  if (px0[2] < px1[2]) iskey = false;
97  }
98  }
99  if (iskey) result.push_back(cv::Point(x, y));
100  }
101 
102  }
103  }
104 
105 }
106 
107 // @brief calc 8 neighbor scaled oriented gradient image
108 // @param src source image
109 // @param dst destination image (HSV, H : orientation, V : intensity)
110 // @param scale scale
111 void calcScaledOrientedGradient(cv::Mat& src, cv::Mat& dst, int scale) {
112  cv::Mat gimg;
113  cv::Mat intimg, sqintimg, tintimg;
114 
115  int width = src.cols;
116  int height = src.rows;
117 
118  cv::cvtColor(src, gimg, CV_BGR2GRAY);
119 
120  dst.create(height, width, CV_8UC3);
121  unsigned char *gradbuf = dst.ptr();
122 
123  intimg.create(height + 1, width + 1, CV_32S);
124  sqintimg.create(height + 1, width + 1, CV_32S);
125  tintimg.create(height + 1, width + 1, CV_32S);
126 
127  cv::integral(gimg, intimg, sqintimg, tintimg);
128 
129  float r2 = sqrt(2.0);
130  int xidx, idx;
131  int hscale = scale / 2;
132  int bsize = hscale * 2 + 1;
133  int barea0 = scale * scale;
134  int barea1 = scale * bsize;
135  int margin = scale;
136  for (int y = margin; y < height - margin; y++) {
137  xidx = y * width;
138  for (int x = margin; x < width - margin; x++) {
139  int m, th;
140  float dx, dy;
141  float dxx, dyy, dxy, dyx;
142  double dth;
143 
144  dxx = ((float)(intimg.at<int>(y + hscale + 1, x + scale + 1)
145  + intimg.at<int>(y - hscale, x + 1)
146  - intimg.at<int>(y - hscale, x + scale + 1)
147  - intimg.at<int>(y + hscale + 1, x + 1))
148  - (float)(intimg.at<int>(y + hscale + 1, x)
149  +intimg.at<int>(y - hscale, x - scale)
150  -intimg.at<int>(y - hscale, x)
151  -intimg.at<int>(y + hscale + 1, x - scale))) / barea1;
152  dyy = ((float)(intimg.at<int>(y + scale + 1, x + hscale + 1)
153  + intimg.at<int>(y + 1, x - hscale)
154  - intimg.at<int>(y + scale + 1, x - hscale)
155  - intimg.at<int>(y + 1, x + hscale + 1))
156  - (float)(intimg.at<int>(y - scale, x - hscale)
157  +intimg.at<int>(y, x + hscale + 1)
158  -intimg.at<int>(y - scale, x + hscale + 1)
159  -intimg.at<int>(y, x - hscale))) / barea1;
160 
161  dxy = ((float)(intimg.at<int>(y + scale + 1, x + scale + 1)
162  + intimg.at<int>(y + 1, x + 1)
163  - intimg.at<int>(y + 1, x + scale + 1)
164  - intimg.at<int>(y + scale + 1, x + 1))
165  - (float)(intimg.at<int>(y, x)
166  + intimg.at<int>(y - scale, x - scale)
167  - intimg.at<int>(y, x - scale)
168  - intimg.at<int>(y - scale, x))) / barea0;
169  dyx = ((float)(intimg.at<int>(y + scale + 1, x)
170  + intimg.at<int>(y + 1, x - scale)
171  - intimg.at<int>(y + 1, x)
172  - intimg.at<int>(y + scale + 1, x - scale))
173  - (float)(intimg.at<int>(y, x + scale + 1)
174  + intimg.at<int>(y - scale, x + 1)
175  - intimg.at<int>(y - scale, x + scale + 1)
176  - intimg.at<int>(y, x + 1))) / barea0;
177 
178  dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
179  dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
180 
181  m = (int)(sqrt(0.5 * (dx * dx + dy * dy)));
182 
183  dth = atan2(dy, dx) * 180.0 / M_PI;
184  if (dth < 0.0) dth += 180.0;
185  if (dth >= 180.0) dth -= 180.0;
186  th = (int)dth;
187 
188  gradbuf[(xidx + x) * 3] = th;
189  gradbuf[(xidx + x) * 3 + 1] = 255;
190  gradbuf[(xidx + x) * 3 + 2] = m;
191  }
192  }
193 }
194 
195 // @brief calc key points from scaled oriented gradient image
196 // @param src source image
197 // @param dst destination image (HSV, H : orientation, V : intensity)
198 void calcSOGKeyPoints(cv::Mat& src, cv::Mat& dst) {
199  cv::Mat gimg;
200  cv::Mat intimg, sqintimg, tintimg;
201 
202  int width = src.cols;
203  int height = src.rows;
204 
205  std::vector<std::vector<float> > gradimglist;
206  std::vector<int> scalelist;
207 
208  cv::cvtColor(src, gimg, CV_BGR2GRAY);
209 
210  dst.create(height, width, CV_8UC3);
211  unsigned char *gradbuf = dst.ptr();
212 
213  intimg.create(height + 1, width + 1, CV_32S);
214  sqintimg.create(height + 1, width + 1, CV_32S);
215  tintimg.create(height + 1, width + 1, CV_32S);
216 
217  cv::integral(gimg, intimg, sqintimg, tintimg);
218 
219  // scale=1,1+1*2,1+2*2,...,1+s*2
220  int maxscale = 10;
221  scalelist.resize(width * height);
222 
223  // gradimglist = g(0) gx(0), gy(0), ...
224  gradimglist.resize((maxscale + 1) * 3);
225 
226  for(int i = 0; i < gradimglist.size(); i++)
227  gradimglist[i].resize(width * height);
228 
229  // s=1
230  float r2 = sqrt(2.0);
231  int xidx, idx;
232  for (int y = 1; y < height - 1; y++) {
233  xidx = y * width;
234  for (int x = 1; x < width - 1; x++) {
235  float dx, dy;
236  float dxx, dyy, dxy, dyx;
237 
238  dxx = (float)gimg.at<unsigned char>(y, x + 1)
239  - (float)gimg.at<unsigned char>(y, x - 1);
240  dyy = (float)gimg.at<unsigned char>(y + 1, x)
241  - (float)gimg.at<unsigned char>(y - 1, x);
242  dxy = (float)gimg.at<unsigned char>(y + 1, x + 1)
243  - (float)gimg.at<unsigned char>(y - 1, x - 1);
244  dyx = (float)gimg.at<unsigned char>(y + 1, x - 1)
245  - (float)gimg.at<unsigned char>(y - 1, x + 1);
246 
247  dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
248  dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
249 
250  idx = xidx + x;
251  gradimglist[0][idx] = dx * dx + dy * dy;
252  gradimglist[1][idx] = dx;
253  gradimglist[2][idx] = dy;
254  }
255  }
256 
257  // s >= 2
258  for (int scale = 2; scale <= maxscale; scale++) {
259 
260  int hscale = scale / 2;
261  int bsize = hscale * 2 + 1;
262  int barea0 = scale * scale;
263  int barea1 = scale * bsize;
264  int margin = scale;
265 
266  int sidx = (scale - 1) * 3;
267  for (int y = margin; y < height - margin; y++) {
268  xidx = y * width;
269  for (int x = margin; x < width - margin; x++) {
270  float dx, dy;
271  float dxx, dyy, dxy, dyx;
272 
273  dxx = ((float)(intimg.at<int>(y + hscale + 1, x + scale + 1)
274  + intimg.at<int>(y - hscale, x + 1)
275  - intimg.at<int>(y - hscale, x + scale + 1)
276  - intimg.at<int>(y + hscale + 1, x + 1))
277  - (float)(intimg.at<int>(y + hscale + 1, x)
278  + intimg.at<int>(y - hscale, x - scale)
279  - intimg.at<int>(y - hscale, x)
280  - intimg.at<int>(y + hscale + 1, x - scale)))
281  / barea1;
282  dyy = ((float)(intimg.at<int>(y + scale + 1, x + hscale + 1)
283  + intimg.at<int>(y + 1, x - hscale)
284  - intimg.at<int>(y + scale + 1, x - hscale)
285  - intimg.at<int>(y + 1, x + hscale + 1))
286  -(float)(intimg.at<int>(y - scale, x - hscale)
287  +intimg.at<int>(y, x + hscale + 1)
288  -intimg.at<int>(y - scale, x + hscale + 1)
289  -intimg.at<int>(y, x - hscale))) / barea1;
290 
291 
292  dxy = ((float)(intimg.at<int>(y + scale + 1, x + scale + 1)
293  + intimg.at<int>(y + 1, x + 1)
294  - intimg.at<int>(y + 1, x + scale + 1)
295  - intimg.at<int>(y + scale + 1, x + 1))
296  - (float)(intimg.at<int>(y, x)
297  + intimg.at<int>(y - scale, x - scale)
298  - intimg.at<int>(y, x - scale)
299  - intimg.at<int>(y - scale, x))) / barea0;
300  dyx = ((float)(intimg.at<int>(y + scale + 1, x)
301  + intimg.at<int>(y + 1, x - scale)
302  - intimg.at<int>(y + 1, x)
303  - intimg.at<int>(y + scale + 1, x - scale))
304  - (float)(intimg.at<int>(y, x + scale + 1)
305  + intimg.at<int>(y - scale, x + 1)
306  - intimg.at<int>(y - scale, x + scale + 1)
307  - intimg.at<int>(y, x + 1))) / barea0;
308 
309  dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
310  dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
311 
312  idx = xidx + x;
313  gradimglist[sidx][idx] = dx * dx + dy * dy;
314  gradimglist[sidx + 1][idx] = dx;
315  gradimglist[sidx + 2][idx] = dy;
316  }
317  }
318  }
319 
320  // scale estimation
321  int ofs = 1 + maxscale;
322  for (int y = ofs; y < height - ofs; y++) {
323  xidx = y * width;
324  for (int x = ofs; x < width - ofs; x++) {
325  idx = xidx + x;
326  float m, mmax, th;
327  int sscale = 0;
328  int mscale = sscale;
329  float dx, dy;
330  mmax = gradimglist[sscale][idx];
331  for (int scale = sscale + 1; scale <= maxscale; scale++) {
332  m = gradimglist[scale * 3][idx];
333  if (mmax < m) {
334  mmax = m;
335  mscale = scale;
336  } else {
337  break;
338  }
339  }
340  m = sqrt(mmax);
341  dx = gradimglist[mscale * 3 + 1][idx];
342  dy = gradimglist[mscale * 3 + 2][idx];
343  th = atan2(dy, dx) * 180.0 / M_PI;
344  if (th < 0.0) th += 180.0;
345  if (th >= 180.0) th -= 180.0;
346  scalelist[idx] = mscale;
347 
348  gradbuf[idx * 3] = (unsigned char)th;
349  gradbuf[idx * 3 + 1] = 255;
350  gradbuf[idx * 3 + 2] = (unsigned char)m;
351  }
352  }
353 
354  // keypoint localization
355  for (int y = ofs; y < height - ofs; y++) {
356  xidx = y * width;
357  for (int x = ofs; x < width - ofs; x++) {
358  idx = xidx + x;
359  int scale = scalelist[idx];
360  float m = gradimglist[scale][idx];
361  bool iskey = false;
362  if (sqrt(m) > 32) {
363  iskey = true;
364  for (int sc = scale > 0 ? scale - 1 : 0;
365  sc < (scale < maxscale ? scale + 1 : maxscale);
366  sc++) {
367  for (int xx = -1; xx <= 1; xx++) {
368  if (m < gradimglist[sc][idx + xx]
369  || m < gradimglist[sc][idx + xx + width]
370  || m < gradimglist[sc][idx + xx - width]) {
371  iskey = false;
372  break;
373  }
374  }
375  }
376  }
377 
378  if (iskey) {
379  cv::circle(src,
380  cv::Point(x, y),
381  scale + 1,
382  cv::Scalar(0, 0, 255),
383  1,
384  4);
385  } else {
386  // gradbuf[idx*3]=0;
387  // gradbuf[idx*3+1]=0;
388  // gradbuf[idx*3+2]=0;
389  }
390  }
391  }
392 }
393 
394 
395 } // namespace
jsk_perception::calcOrientedGradient
void calcOrientedGradient(cv::Mat &src, cv::Mat &dst)
Definition: oriented_gradient.cpp:15
random_forest_client_sample.circle
circle
Definition: random_forest_client_sample.py:78
ssd_train_dataset.int
int
Definition: ssd_train_dataset.py:175
oriented_gradient.hpp
jsk_perception::calcScaledOrientedGradient
void calcScaledOrientedGradient(cv::Mat &src, cv::Mat &dst, int scale)
Definition: oriented_gradient.cpp:111
width
width
float
float
atan2
double atan2()
M_PI
#define M_PI
sqrt
double sqrt()
x
x
y
y
jsk_perception
Definition: add_mask_image.h:48
jsk_perception::calcOGKeyPoints
void calcOGKeyPoints(cv::Mat &src, cv::Mat &dst, std::vector< cv::Point > &result, int thres=32, int bs=1)
Definition: oriented_gradient.cpp:75
height
height
log_utils.h
jsk_perception::calcSOGKeyPoints
void calcSOGKeyPoints(cv::Mat &src, cv::Mat &dst)
Definition: oriented_gradient.cpp:198
resize
GLvoid resize(GLsizei, GLsizei)

jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Mon May 3 2021 03:03:27