API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 19th, 2017
90
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 16.39 KB | None | 0 0
raw download clone embed print report
  1. #include "StdAfx.h"
  2. #include "SubPixEdgeDet.h"
  3. #include <cmath>
  4. #include "opencv2/imgproc/imgproc.hpp"
  5. using namespace cv;
  6. using namespace std;
  7.  
  8.  
  9. #define CV_2PI 6.283185307179586476925286766559
  10.  
  11. const double scale = 128.0; // sum of half Canny filter is 128
  12.  
  13. static void getCannyKernel(OutputArray _d, double alpha)
  14. {
  15. int r = cvRound(alpha * 3);
  16. int ksize = 2 * r + 1;
  17.  
  18. _d.create(ksize, 1, CV_16S, -1, true);
  19.  
  20. Mat k = _d.getMat();
  21.  
  22. vector<float> kerF(ksize, 0.0f);
  23. kerF[r] = 0.0f;
  24. double a2 = alpha * alpha;
  25. float sum = 0.0f;
  26. for (int x = 1; x <= r; ++x)
  27. {
  28. float v = (float)(-x * std::exp(-x * x / (2 * a2)));
  29. sum += v;
  30. kerF[r + x] = v;
  31. kerF[r - x] = -v;
  32. }
  33. float scale = 128 / sum;
  34. for (int i = 0; i < ksize; ++i)
  35. {
  36. kerF[i] *= scale;
  37. }
  38. Mat temp(ksize, 1, CV_32F, &kerF[0]);
  39. temp.convertTo(k, CV_16S);
  40. }
  41.  
  42. // non-maximum supression and hysteresis
  43. static void postCannyFilter(const Mat &src, Mat &dx, Mat &dy, int low, int high, Mat &dst)
  44. {
  45. ptrdiff_t mapstep = src.cols + 2;
  46. AutoBuffer<uchar> buffer((src.cols + 2)*(src.rows + 2) + mapstep * 3 * sizeof(int));
  47.  
  48. // L2Gradient comparison with square
  49. high = high * high;
  50. low = low * low;
  51.  
  52. int* mag_buf[3];
  53. mag_buf[0] = (int*)(uchar*)buffer;
  54. mag_buf[1] = mag_buf[0] + mapstep;
  55. mag_buf[2] = mag_buf[1] + mapstep;
  56. memset(mag_buf[0], 0, mapstep*sizeof(int));
  57.  
  58. uchar* map = (uchar*)(mag_buf[2] + mapstep);
  59. memset(map, 1, mapstep);
  60. memset(map + mapstep*(src.rows + 1), 1, mapstep);
  61.  
  62. int maxsize = std::max(1 << 10, src.cols * src.rows / 10);
  63. std::vector<uchar*> stack(maxsize);
  64. uchar **stack_top = &stack[0];
  65. uchar **stack_bottom = &stack[0];
  66.  
  67. /* sector numbers
  68. (Top-Left Origin)
  69.  
  70. 1 2 3
  71. * * *
  72. * * *
  73. 0*******0
  74. * * *
  75. * * *
  76. 3 2 1
  77. */
  78.  
  79. #define CANNY_PUSH(d) *(d) = uchar(2), *stack_top++ = (d)
  80. #define CANNY_POP(d) (d) = *--stack_top
  81.  
  82. #if CV_SSE2
  83. bool haveSSE2 = checkHardwareSupport(CV_CPU_SSE2);
  84. #endif
  85.  
  86. // calculate magnitude and angle of gradient, perform non-maxima suppression.
  87. // fill the map with one of the following values:
  88. // 0 - the pixel might belong to an edge
  89. // 1 - the pixel can not belong to an edge
  90. // 2 - the pixel does belong to an edge
  91. for (int i = 0; i <= src.rows; i++)
  92. {
  93. int* _norm = mag_buf[(i > 0) + 1] + 1;
  94. if (i < src.rows)
  95. {
  96. short* _dx = dx.ptr<short>(i);
  97. short* _dy = dy.ptr<short>(i);
  98.  
  99. int j = 0, width = src.cols;
  100. #if CV_SSE2
  101. if (haveSSE2)
  102. {
  103. for (; j <= width - 8; j += 8)
  104. {
  105. __m128i v_dx = _mm_loadu_si128((const __m128i *)(_dx + j));
  106. __m128i v_dy = _mm_loadu_si128((const __m128i *)(_dy + j));
  107.  
  108. __m128i v_dx_ml = _mm_mullo_epi16(v_dx, v_dx), v_dx_mh = _mm_mulhi_epi16(v_dx, v_dx);
  109. __m128i v_dy_ml = _mm_mullo_epi16(v_dy, v_dy), v_dy_mh = _mm_mulhi_epi16(v_dy, v_dy);
  110.  
  111. __m128i v_norm = _mm_add_epi32(_mm_unpacklo_epi16(v_dx_ml, v_dx_mh), _mm_unpacklo_epi16(v_dy_ml, v_dy_mh));
  112. _mm_storeu_si128((__m128i *)(_norm + j), v_norm);
  113.  
  114. v_norm = _mm_add_epi32(_mm_unpackhi_epi16(v_dx_ml, v_dx_mh), _mm_unpackhi_epi16(v_dy_ml, v_dy_mh));
  115. _mm_storeu_si128((__m128i *)(_norm + j + 4), v_norm);
  116. }
  117. }
  118. #elif CV_NEON
  119. for (; j <= width - 8; j += 8)
  120. {
  121. int16x8_t v_dx = vld1q_s16(_dx + j), v_dy = vld1q_s16(_dy + j);
  122. int16x4_t v_dxp = vget_low_s16(v_dx), v_dyp = vget_low_s16(v_dy);
  123. int32x4_t v_dst = vmlal_s16(vmull_s16(v_dxp, v_dxp), v_dyp, v_dyp);
  124. vst1q_s32(_norm + j, v_dst);
  125.  
  126. v_dxp = vget_high_s16(v_dx), v_dyp = vget_high_s16(v_dy);
  127. v_dst = vmlal_s16(vmull_s16(v_dxp, v_dxp), v_dyp, v_dyp);
  128. vst1q_s32(_norm + j + 4, v_dst);
  129. }
  130. #endif
  131. for (; j < width; ++j)
  132. _norm[j] = int(_dx[j])*_dx[j] + int(_dy[j])*_dy[j];
  133.  
  134. _norm[-1] = _norm[src.cols] = 0;
  135. }
  136. else
  137. memset(_norm - 1, 0, /* cn* */mapstep*sizeof(int));
  138.  
  139. // at the very beginning we do not have a complete ring
  140. // buffer of 3 magnitude rows for non-maxima suppression
  141. if (i == 0)
  142. continue;
  143.  
  144. uchar* _map = map + mapstep*i + 1;
  145. _map[-1] = _map[src.cols] = 1;
  146.  
  147. int* _mag = mag_buf[1] + 1; // take the central row
  148. ptrdiff_t magstep1 = mag_buf[2] - mag_buf[1];
  149. ptrdiff_t magstep2 = mag_buf[0] - mag_buf[1];
  150.  
  151. const short* _x = dx.ptr<short>(i - 1);
  152. const short* _y = dy.ptr<short>(i - 1);
  153.  
  154. if ((stack_top - stack_bottom) + src.cols > maxsize)
  155. {
  156. int sz = (int)(stack_top - stack_bottom);
  157. maxsize = std::max(maxsize * 3 / 2, sz + src.cols);
  158. stack.resize(maxsize);
  159. stack_bottom = &stack[0];
  160. stack_top = stack_bottom + sz;
  161. }
  162.  
  163. int prev_flag = 0;
  164. for (int j = 0; j < src.cols; j++)
  165. {
  166. #define CANNY_SHIFT 15
  167. const int TG22 = (int)(0.4142135623730950488016887242097*(1 << CANNY_SHIFT) + 0.5);
  168.  
  169. int m = _mag[j];
  170.  
  171. if (m > low)
  172. {
  173. int xs = _x[j];
  174. int ys = _y[j];
  175. int x = std::abs(xs);
  176. int y = std::abs(ys) << CANNY_SHIFT;
  177.  
  178. int tg22x = x * TG22;
  179.  
  180. if (y < tg22x)
  181. {
  182. if (m > _mag[j - 1] && m >= _mag[j + 1]) goto __ocv_canny_push;
  183. }
  184. else
  185. {
  186. int tg67x = tg22x + (x << (CANNY_SHIFT + 1));
  187. if (y > tg67x)
  188. {
  189. if (m > _mag[j + magstep2] && m >= _mag[j + magstep1]) goto __ocv_canny_push;
  190. }
  191. else
  192. {
  193. int s = (xs ^ ys) < 0 ? -1 : 1;
  194. if (m > _mag[j + magstep2 - s] && m > _mag[j + magstep1 + s]) goto __ocv_canny_push;
  195. }
  196. }
  197. }
  198. prev_flag = 0;
  199. _map[j] = uchar(1);
  200. continue;
  201. __ocv_canny_push:
  202. if (!prev_flag && m > high && _map[j - mapstep] != 2)
  203. {
  204. CANNY_PUSH(_map + j);
  205. prev_flag = 1;
  206. }
  207. else
  208. _map[j] = 0;
  209. }
  210.  
  211. // scroll the ring buffer
  212. _mag = mag_buf[0];
  213. mag_buf[0] = mag_buf[1];
  214. mag_buf[1] = mag_buf[2];
  215. mag_buf[2] = _mag;
  216. }
  217.  
  218. // now track the edges (hysteresis thresholding)
  219. while (stack_top > stack_bottom)
  220. {
  221. uchar* m;
  222. if ((stack_top - stack_bottom) + 8 > maxsize)
  223. {
  224. int sz = (int)(stack_top - stack_bottom);
  225. maxsize = maxsize * 3 / 2;
  226. stack.resize(maxsize);
  227. stack_bottom = &stack[0];
  228. stack_top = stack_bottom + sz;
  229. }
  230.  
  231. CANNY_POP(m);
  232.  
  233. if (!m[-1]) CANNY_PUSH(m - 1);
  234. if (!m[1]) CANNY_PUSH(m + 1);
  235. if (!m[-mapstep - 1]) CANNY_PUSH(m - mapstep - 1);
  236. if (!m[-mapstep]) CANNY_PUSH(m - mapstep);
  237. if (!m[-mapstep + 1]) CANNY_PUSH(m - mapstep + 1);
  238. if (!m[mapstep - 1]) CANNY_PUSH(m + mapstep - 1);
  239. if (!m[mapstep]) CANNY_PUSH(m + mapstep);
  240. if (!m[mapstep + 1]) CANNY_PUSH(m + mapstep + 1);
  241. }
  242.  
  243. // the final pass, form the final image
  244. const uchar* pmap = map + mapstep + 1;
  245. uchar* pdst = dst.ptr();
  246. for (int i = 0; i < src.rows; i++, pmap += mapstep, pdst += dst.step)
  247. {
  248. for (int j = 0; j < src.cols; j++)
  249. pdst[j] = (uchar)-(pmap[j] >> 1);
  250. }
  251. }
  252.  
  253. static inline double getAmplitude(Mat &dx, Mat &dy, int i, int j)
  254. {
  255. Point2d mag(dx.at<short>(i, j), dy.at<short>(i, j));
  256. return norm(mag);
  257. }
  258.  
  259. static inline void getMagNeighbourhood(Mat &dx, Mat &dy, Point &p, int w, int h, vector<double> &mag)
  260. {
  261. int top = p.y - 1 >= 0 ? p.y - 1 : p.y;
  262. int down = p.y + 1 < h ? p.y + 1 : p.y;
  263. int left = p.x - 1 >= 0 ? p.x - 1 : p.x;
  264. int right = p.x + 1 < w ? p.x + 1 : p.x;
  265.  
  266. mag[0] = getAmplitude(dx, dy, top, left);
  267. mag[1] = getAmplitude(dx, dy, top, p.x);
  268. mag[2] = getAmplitude(dx, dy, top, right);
  269. mag[3] = getAmplitude(dx, dy, p.y, left);
  270. mag[4] = getAmplitude(dx, dy, p.y, p.x);
  271. mag[5] = getAmplitude(dx, dy, p.y, right);
  272. mag[6] = getAmplitude(dx, dy, down, left);
  273. mag[7] = getAmplitude(dx, dy, down, p.x);
  274. mag[8] = getAmplitude(dx, dy, down, right);
  275. }
  276.  
  277. static inline void get2ndFacetModelIn3x3(vector<double> &mag, vector<double> &a)
  278. {
  279. a[0] = (-mag[0] + 2.0 * mag[1] - mag[2] + 2.0 * mag[3] + 5.0 * mag[4] + 2.0 * mag[5] - mag[6] + 2.0 * mag[7] - mag[8]) / 9.0;
  280. a[1] = (-mag[0] + mag[2] - mag[3] + mag[5] - mag[6] + mag[8]) / 6.0;
  281. a[2] = (mag[6] + mag[7] + mag[8] - mag[0] - mag[1] - mag[2]) / 6.0;
  282. a[3] = (mag[0] - 2.0 * mag[1] + mag[2] + mag[3] - 2.0 * mag[4] + mag[5] + mag[6] - 2.0 * mag[7] + mag[8]) / 6.0;
  283. a[4] = (-mag[0] + mag[2] + mag[6] - mag[8]) / 4.0;
  284. a[5] = (mag[0] + mag[1] + mag[2] - 2.0 * (mag[3] + mag[4] + mag[5]) + mag[6] + mag[7] + mag[8]) / 6.0;
  285. }
  286. /*
  287. Compute the eigenvalues and eigenvectors of the Hessian matrix given by
  288. dfdrr, dfdrc, and dfdcc, and sort them in descending order according to
  289. their absolute values.
  290. */
  291. static inline void eigenvals(vector<double> &a, double eigval[2], double eigvec[2][2])
  292. {
  293. // derivatives
  294. // fx = a[1], fy = a[2]
  295. // fxy = a[4]
  296. // fxx = 2 * a[3]
  297. // fyy = 2 * a[5]
  298. double dfdrc = a[4];
  299. double dfdcc = a[3] * 2.0;
  300. double dfdrr = a[5] * 2.0;
  301. double theta, t, c, s, e1, e2, n1, n2; /* , phi; */
  302.  
  303. /* Compute the eigenvalues and eigenvectors of the Hessian matrix. */
  304. if (dfdrc != 0.0) {
  305. theta = 0.5*(dfdcc - dfdrr) / dfdrc;
  306. t = 1.0 / (fabs(theta) + sqrt(theta*theta + 1.0));
  307. if (theta < 0.0) t = -t;
  308. c = 1.0 / sqrt(t*t + 1.0);
  309. s = t*c;
  310. e1 = dfdrr - t*dfdrc;
  311. e2 = dfdcc + t*dfdrc;
  312. }
  313. else {
  314. c = 1.0;
  315. s = 0.0;
  316. e1 = dfdrr;
  317. e2 = dfdcc;
  318. }
  319. n1 = c;
  320. n2 = -s;
  321.  
  322. /* If the absolute value of an eigenvalue is larger than the other, put that
  323. eigenvalue into first position. If both are of equal absolute value, put
  324. the negative one first. */
  325. if (fabs(e1) > fabs(e2)) {
  326. eigval[0] = e1;
  327. eigval[1] = e2;
  328. eigvec[0][0] = n1;
  329. eigvec[0][1] = n2;
  330. eigvec[1][0] = -n2;
  331. eigvec[1][1] = n1;
  332. }
  333. else if (fabs(e1) < fabs(e2)) {
  334. eigval[0] = e2;
  335. eigval[1] = e1;
  336. eigvec[0][0] = -n2;
  337. eigvec[0][1] = n1;
  338. eigvec[1][0] = n1;
  339. eigvec[1][1] = n2;
  340. }
  341. else {
  342. if (e1 < e2) {
  343. eigval[0] = e1;
  344. eigval[1] = e2;
  345. eigvec[0][0] = n1;
  346. eigvec[0][1] = n2;
  347. eigvec[1][0] = -n2;
  348. eigvec[1][1] = n1;
  349. }
  350. else {
  351. eigval[0] = e2;
  352. eigval[1] = e1;
  353. eigvec[0][0] = -n2;
  354. eigvec[0][1] = n1;
  355. eigvec[1][0] = n1;
  356. eigvec[1][1] = n2;
  357. }
  358. }
  359. }
  360.  
  361. static inline double vector2angle(double x, double y)
  362. {
  363. double a = std::atan2(y, x);
  364. return a >= 0.0 ? a : a + CV_2PI;
  365. }
  366.  
  367. void extractSubPixPoints(Mat &dx, Mat &dy, vector<vector<Point> > &contoursInPixel, vector<Contour> &contours)
  368. {
  369. int w = dx.cols;
  370. int h = dx.rows;
  371. contours.resize(contoursInPixel.size());
  372. for (size_t i = 0; i < contoursInPixel.size(); ++i)
  373. {
  374. vector<Point> &icontour = contoursInPixel[i];
  375. Contour &contour = contours[i];
  376. contour.points.resize(icontour.size());
  377. contour.response.resize(icontour.size());
  378. contour.direction.resize(icontour.size());
  379. #if defined(_OPENMP) && defined(NDEBUG)
  380. #pragma omp parallel for
  381. #endif
  382. for (int j = 0; j < (int)icontour.size(); ++j)
  383. {
  384. vector<double> magNeighbour(9);
  385. // gets module between icontour[j] in dx and dy of pixel's neighbours
  386. getMagNeighbourhood(dx, dy, icontour[j], w, h, magNeighbour);
  387. vector<double> a(9);
  388. get2ndFacetModelIn3x3(magNeighbour, a);
  389.  
  390. // Hessian eigen vector
  391. double eigvec[2][2], eigval[2];
  392. eigenvals(a, eigval, eigvec);
  393. double t = 0.0;
  394. double ny = eigvec[0][0];
  395. double nx = eigvec[0][1];
  396. if (eigval[0] < 0.0)
  397. {
  398. double rx = a[1], ry = a[2], rxy = a[4], rxx = a[3] * 2.0, ryy = a[5] * 2.0;
  399. t = -(rx * nx + ry * ny) / (rxx * nx * nx + 2.0 * rxy * nx * ny + ryy * ny * ny);
  400. }
  401. double px = nx * t;
  402. double py = ny * t;
  403. float x = (float)icontour[j].x;
  404. float y = (float)icontour[j].y;
  405. if (fabs(px) <= 0.5 && fabs(py) <= 0.5)
  406. {
  407. x += (float)px;
  408. y += (float)py;
  409. }
  410. contour.points[j] = Point2f(x, y);
  411. contour.response[j] = (float)(a[0] / scale);
  412. contour.direction[j] = (float)vector2angle(ny, nx);
  413. }
  414. }
  415. }
  416.  
  417. //---------------------------------------------------------------------
  418. // INTERFACE FUNCTION
  419. //---------------------------------------------------------------------
  420.  
  421. void DrawSubPixelContours(vector<Contour>, InputArray, Mat);
  422.  
  423. void EdgesSubPix(Mat &gray, double alpha, int low, int high,
  424. vector<Contour> &contours, OutputArray hierarchy, int mode)
  425. {
  426. /// applies gaussian kernel
  427. Mat blur;
  428. GaussianBlur(gray, blur, Size(0, 0), alpha, alpha);
  429.  
  430. /// get Canny's sobel kernel
  431. Mat d;
  432. getCannyKernel(d, alpha);
  433.  
  434. cout << "Canny Kernel = "<< endl << " " << d << endl << endl;
  435.  
  436. /// applies separable filter to X and Y (Sobel received from Canny's)
  437. Mat one = Mat::ones(Size(1, 1), CV_16S);
  438. Mat dx, dy;
  439. sepFilter2D(blur, dx, CV_16S, d, one);
  440. sepFilter2D(blur, dy, CV_16S, one, d);
  441.  
  442. /// drawing for testing
  443. namedWindow( "dx", CV_WINDOW_AUTOSIZE );
  444. imshow( "dx", dx );
  445.  
  446. /// drawing for testing
  447. namedWindow( "dy", CV_WINDOW_AUTOSIZE );
  448. imshow( "dy", dy );
  449.  
  450. /// creates a matrix of zeroes with the gray image size
  451. Mat edge = Mat::zeros(gray.size(), CV_8UC1);
  452. int lowThresh = cvRound(scale * low);
  453. int highThresh = cvRound(scale * high);
  454. /// non-maximum supression (keep strongest pixels considering orientation (dx/dy)) & hysteresis threshold
  455. postCannyFilter(gray, dx, dy, lowThresh, highThresh, edge);
  456.  
  457. // contours in pixel precision
  458. vector<vector<Point>> contoursInPixel;
  459. findContours(edge, contoursInPixel, hierarchy, mode, CHAIN_APPROX_NONE, Point(0, 0));
  460.  
  461. // subpixel position extraction with steger's method and facet model 2nd polynominal in 3x3 neighbourhood
  462. extractSubPixPoints(dx, dy, contoursInPixel, contours);
  463. DrawSubPixelContours(contours, hierarchy, gray);
  464. }
  465.  
  466. void EdgesSubPix(Mat &gray, double alpha, int low, int high, vector<Contour> &contours)
  467. {
  468. vector<Vec4i> hierarchy;
  469. EdgesSubPix(gray, alpha, low, high, contours, hierarchy, RETR_LIST);
  470. }
  471.  
  472. void DrawSubPixelContours(vector<Contour> contours, InputArray hierarchy, Mat gray)
  473. {
  474. vector<vector<Point>> contoursToPixel;
  475. vector<vector<float>> contoursDirections;
  476. for(int i = 0; i < contours.size(); i++)
  477. {
  478. vector<Point> points;
  479. for (int j = 0; j < contours[i].points.size(); j++)
  480. {
  481. points.push_back( Point((int)contours[i].points[j].x, (int)contours[i].points[j].y) );
  482. }
  483. contoursDirections.push_back( contours[i].direction );
  484. contoursToPixel.push_back( points );
  485. }
  486.  
  487.  
  488. for(int i = 0; i < contoursToPixel.size(); i++)
  489. {
  490. Scalar color = Scalar( 255, 0, 0 );
  491. drawContours( gray, contoursToPixel, i, color, 2, 8, hierarchy, 0, Point() );
  492. }
  493. /// Show in a window
  494. namedWindow( "Contours_FILTER2", CV_WINDOW_AUTOSIZE );
  495. imshow( "Contours_FILTER2", gray );
  496. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!