added Generalized Hough implementation
[profile/ivi/opencv.git] / modules / imgproc / src / generalized_hough.cpp
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 //  By downloading, copying, installing or using the software you agree to this license.
6 //  If you do not agree to this license, do not download, install,
7 //  copy or use the software.
8 //
9 //
10 //                        Intel License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 //   * Redistribution's of source code must retain the above copyright notice,
20 //     this list of conditions and the following disclaimer.
21 //
22 //   * Redistribution's in binary form must reproduce the above copyright notice,
23 //     this list of conditions and the following disclaimer in the documentation
24 //     and/or other materials provided with the distribution.
25 //
26 //   * The name of Intel Corporation may not be used to endorse or promote products
27 //     derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 #include "precomp.hpp"
43
44 using namespace std;
45 using namespace cv;
46
47 namespace
48 {
49     /////////////////////////////////////
50     // Common
51
52     template <typename T, class A> void releaseVector(vector<T, A>& v)
53     {
54         vector<T, A> empty;
55         empty.swap(v);
56     }
57
58     double toRad(double a)
59     {
60         return a * CV_PI / 180.0;
61     }
62
63     bool notNull(float v)
64     {
65         return fabs(v) > numeric_limits<float>::epsilon();
66     }
67     bool notNull(double v)
68     {
69         return fabs(v) > numeric_limits<double>::epsilon();
70     }
71
72     class GHT_Pos : public GeneralizedHough
73     {
74     public:
75         GHT_Pos();
76
77     protected:
78         void setTemplateImpl(const Mat& edges, const Mat& dx, const Mat& dy, Point templCenter);
79         void detectImpl(const Mat& edges, const Mat& dx, const Mat& dy, OutputArray positions, OutputArray votes);
80         void releaseImpl();
81
82         virtual void processTempl() = 0;
83         virtual void processImage() = 0;
84
85         void filterMinDist();
86         void convertTo(OutputArray positions, OutputArray votes);
87
88         double minDist;
89
90         Size templSize;
91         Point templCenter;
92         Mat templEdges;
93         Mat templDx;
94         Mat templDy;
95
96         Size imageSize;
97         Mat imageEdges;
98         Mat imageDx;
99         Mat imageDy;
100
101         vector<Vec4f> posOutBuf;
102         vector<Vec3i> voteOutBuf;
103     };
104
105     GHT_Pos::GHT_Pos()
106     {
107         minDist = 1.0;
108     }
109
110     void GHT_Pos::setTemplateImpl(const Mat& edges, const Mat& dx, const Mat& dy, Point templCenter_)
111     {
112         templSize = edges.size();
113         templCenter = templCenter_;
114         edges.copyTo(templEdges);
115         dx.copyTo(templDx);
116         dy.copyTo(templDy);
117
118         processTempl();
119     }
120
121     void GHT_Pos::detectImpl(const Mat& edges, const Mat& dx, const Mat& dy, OutputArray positions, OutputArray votes)
122     {
123         imageSize = edges.size();
124         edges.copyTo(imageEdges);
125         dx.copyTo(imageDx);
126         dy.copyTo(imageDy);
127
128         posOutBuf.clear();
129         voteOutBuf.clear();
130
131         processImage();
132
133         if (!posOutBuf.empty())
134         {
135             if (minDist > 1)
136                 filterMinDist();
137             convertTo(positions, votes);
138         }
139         else
140         {
141             positions.release();
142             if (votes.needed())
143                 votes.release();
144         }
145     }
146
147     void GHT_Pos::releaseImpl()
148     {
149         templSize = Size();
150         templCenter = Point(-1, -1);
151         templEdges.release();
152         templDx.release();
153         templDy.release();
154
155         imageSize = Size();
156         imageEdges.release();
157         imageDx.release();
158         imageDy.release();
159
160         releaseVector(posOutBuf);
161         releaseVector(voteOutBuf);
162     }
163
164     #define votes_cmp_gt(l1, l2) (aux[l1][0] > aux[l2][0])
165     static CV_IMPLEMENT_QSORT_EX( sortIndexies, size_t, votes_cmp_gt, const Vec3i* )
166
167     void GHT_Pos::filterMinDist()
168     {
169         size_t oldSize = posOutBuf.size();
170         const bool hasVotes = !voteOutBuf.empty();
171
172         CV_Assert(!hasVotes || voteOutBuf.size() == oldSize);
173
174         vector<Vec4f> oldPosBuf(posOutBuf);
175         vector<Vec3i> oldVoteBuf(voteOutBuf);
176
177         vector<size_t> indexies(oldSize);
178         for (size_t i = 0; i < oldSize; ++i)
179             indexies[i] = i;
180         sortIndexies(&indexies[0], oldSize, &oldVoteBuf[0]);
181
182         posOutBuf.clear();
183         voteOutBuf.clear();
184
185         const int cellSize = cvRound(minDist);
186         const int gridWidth = (imageSize.width + cellSize - 1) / cellSize;
187         const int gridHeight = (imageSize.height + cellSize - 1) / cellSize;
188
189         vector< vector<Point2f> > grid(gridWidth * gridHeight);
190
191         const double minDist2 = minDist * minDist;
192
193         for (size_t i = 0; i < oldSize; ++i)
194         {
195             const size_t ind = indexies[i];
196
197             Point2f p(oldPosBuf[ind][0], oldPosBuf[ind][1]);
198
199             bool good = true;
200
201             const int xCell = static_cast<int>(p.x / cellSize);
202             const int yCell = static_cast<int>(p.y / cellSize);
203
204             int x1 = xCell - 1;
205             int y1 = yCell - 1;
206             int x2 = xCell + 1;
207             int y2 = yCell + 1;
208
209             // boundary check
210             x1 = std::max(0, x1);
211             y1 = std::max(0, y1);
212             x2 = std::min(gridWidth - 1, x2);
213             y2 = std::min(gridHeight - 1, y2);
214
215             for (int yy = y1; yy <= y2; ++yy)
216             {
217                 for (int xx = x1; xx <= x2; ++xx)
218                 {
219                     const vector<Point2f>& m = grid[yy * gridWidth + xx];
220
221                     for(size_t j = 0; j < m.size(); ++j)
222                     {
223                         const Point2f d = p - m[j];
224
225                         if (d.ddot(d) < minDist2)
226                         {
227                             good = false;
228                             goto break_out;
229                         }
230                     }
231                 }
232             }
233
234             break_out:
235
236             if(good)
237             {
238                 grid[yCell * gridWidth + xCell].push_back(p);
239
240                 posOutBuf.push_back(oldPosBuf[ind]);
241                 if (hasVotes)
242                     voteOutBuf.push_back(oldVoteBuf[ind]);
243             }
244         }
245     }
246
247     void GHT_Pos::convertTo(OutputArray _positions, OutputArray _votes)
248     {
249         const int total = static_cast<int>(posOutBuf.size());
250         const bool hasVotes = !voteOutBuf.empty();
251
252         CV_Assert(!hasVotes || voteOutBuf.size() == posOutBuf.size());
253
254         _positions.create(1, total, CV_32FC4);
255         Mat positions = _positions.getMat();
256         Mat(1, total, CV_32FC4, &posOutBuf[0]).copyTo(positions);
257
258         if (_votes.needed())
259         {
260             if (!hasVotes)
261                 _votes.release();
262             else
263             {
264                 _votes.create(1, total, CV_32SC3);
265                 Mat votes = _votes.getMat();
266                 Mat(1, total, CV_32SC3, &voteOutBuf[0]).copyTo(votes);
267             }
268         }
269     }
270
271     /////////////////////////////////////
272     // POSITION Ballard
273
274     class GHT_Ballard_Pos : public GHT_Pos
275     {
276     public:
277         AlgorithmInfo* info() const;
278
279         GHT_Ballard_Pos();
280
281     protected:
282         void releaseImpl();
283
284         void processTempl();
285         void processImage();
286
287         virtual void calcHist();
288         virtual void findPosInHist();
289
290         int levels;
291         int votesThreshold;
292         double dp;
293
294         vector< vector<Point> > r_table;
295         Mat hist;
296     };
297
298     CV_INIT_ALGORITHM(GHT_Ballard_Pos, "GeneralizedHough.POSITION",
299                       obj.info()->addParam(obj, "minDist", obj.minDist, false, 0, 0,
300                                            "Minimum distance between the centers of the detected objects.");
301                       obj.info()->addParam(obj, "levels", obj.levels, false, 0, 0,
302                                            "R-Table levels.");
303                       obj.info()->addParam(obj, "votesThreshold", obj.votesThreshold, false, 0, 0,
304                                            "The accumulator threshold for the template centers at the detection stage. The smaller it is, the more false positions may be detected.");
305                       obj.info()->addParam(obj, "dp", obj.dp, false, 0, 0,
306                                            "Inverse ratio of the accumulator resolution to the image resolution."));
307
308     GHT_Ballard_Pos::GHT_Ballard_Pos()
309     {
310         levels = 360;
311         votesThreshold = 100;
312         dp = 1.0;
313     }
314
315     void GHT_Ballard_Pos::releaseImpl()
316     {
317         GHT_Pos::releaseImpl();
318
319         releaseVector(r_table);
320         hist.release();
321     }
322
323     void GHT_Ballard_Pos::processTempl()
324     {
325         CV_Assert(templEdges.type() == CV_8UC1);
326         CV_Assert(templDx.type() == CV_32FC1 && templDx.size() == templSize);
327         CV_Assert(templDy.type() == templDx.type() && templDy.size() == templSize);
328         CV_Assert(levels > 0);
329
330         const double thetaScale = levels / 360.0;
331
332         r_table.resize(levels + 1);
333         for_each(r_table.begin(), r_table.end(), mem_fun_ref(&vector<Point>::clear));
334
335         for (int y = 0; y < templSize.height; ++y)
336         {
337             const uchar* edgesRow = templEdges.ptr(y);
338             const float* dxRow = templDx.ptr<float>(y);
339             const float* dyRow = templDy.ptr<float>(y);
340
341             for (int x = 0; x < templSize.width; ++x)
342             {
343                 const Point p(x, y);
344
345                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
346                 {
347                     const float theta = fastAtan2(dyRow[x], dxRow[x]);
348                     const int n = cvRound(theta * thetaScale);
349                     r_table[n].push_back(p - templCenter);
350                 }
351             }
352         }
353     }
354
355     void GHT_Ballard_Pos::processImage()
356     {
357         calcHist();
358         findPosInHist();
359     }
360
361     void GHT_Ballard_Pos::calcHist()
362     {
363         CV_Assert(imageEdges.type() == CV_8UC1);
364         CV_Assert(imageDx.type() == CV_32FC1 && imageDx.size() == imageSize);
365         CV_Assert(imageDy.type() == imageDx.type() && imageDy.size() == imageSize);
366         CV_Assert(levels > 0 && r_table.size() == static_cast<size_t>(levels + 1));
367         CV_Assert(dp > 0.0);
368
369         const double thetaScale = levels / 360.0;
370         const double idp = 1.0 / dp;
371
372         hist.create(cvCeil(imageSize.height * idp) + 2, cvCeil(imageSize.width * idp) + 2, CV_32SC1);
373         hist.setTo(0);
374
375         const int rows = hist.rows - 2;
376         const int cols = hist.cols - 2;
377
378         for (int y = 0; y < imageSize.height; ++y)
379         {
380             const uchar* edgesRow = imageEdges.ptr(y);
381             const float* dxRow = imageDx.ptr<float>(y);
382             const float* dyRow = imageDy.ptr<float>(y);
383
384             for (int x = 0; x < imageSize.width; ++x)
385             {
386                 const Point p(x, y);
387
388                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
389                 {
390                     const float theta = fastAtan2(dyRow[x], dxRow[x]);
391                     const int n = cvRound(theta * thetaScale);
392
393                     const vector<Point>& r_row = r_table[n];
394
395                     for (size_t j = 0; j < r_row.size(); ++j)
396                     {
397                         Point c = p - r_row[j];
398
399                         c.x = cvRound(c.x * idp);
400                         c.y = cvRound(c.y * idp);
401
402                         if (c.x >= 0 && c.x < cols && c.y >= 0 && c.y < rows)
403                             ++hist.at<int>(c.y + 1, c.x + 1);
404                     }
405                 }
406             }
407         }
408     }
409
410     void GHT_Ballard_Pos::findPosInHist()
411     {
412         CV_Assert(votesThreshold > 0);
413
414         const int histRows = hist.rows - 2;
415         const int histCols = hist.cols - 2;
416
417         for(int y = 0; y < histRows; ++y)
418         {
419             const int* prevRow = hist.ptr<int>(y);
420             const int* curRow = hist.ptr<int>(y + 1);
421             const int* nextRow = hist.ptr<int>(y + 2);
422
423             for(int x = 0; x < histCols; ++x)
424             {
425                 const int votes = curRow[x + 1];
426
427                 if (votes > votesThreshold && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
428                 {
429                     posOutBuf.push_back(Vec4f(static_cast<float>(x * dp), static_cast<float>(y * dp), 1.0f, 0.0f));
430                     voteOutBuf.push_back(Vec3i(votes, 0, 0));
431                 }
432             }
433         }
434     }
435
436     /////////////////////////////////////
437     // POSITION & SCALE
438
439     class GHT_Ballard_PosScale : public GHT_Ballard_Pos
440     {
441     public:
442         AlgorithmInfo* info() const;
443
444         GHT_Ballard_PosScale();
445
446     protected:
447         void calcHist();
448         void findPosInHist();
449
450         double minScale;
451         double maxScale;
452         double scaleStep;
453
454         class Worker;
455         friend class Worker;
456     };
457
458     CV_INIT_ALGORITHM(GHT_Ballard_PosScale, "GeneralizedHough.POSITION_SCALE",
459                       obj.info()->addParam(obj, "minDist", obj.minDist, false, 0, 0,
460                                            "Minimum distance between the centers of the detected objects.");
461                       obj.info()->addParam(obj, "levels", obj.levels, false, 0, 0,
462                                            "R-Table levels.");
463                       obj.info()->addParam(obj, "votesThreshold", obj.votesThreshold, false, 0, 0,
464                                            "The accumulator threshold for the template centers at the detection stage. The smaller it is, the more false positions may be detected.");
465                       obj.info()->addParam(obj, "dp", obj.dp, false, 0, 0,
466                                            "Inverse ratio of the accumulator resolution to the image resolution.");
467                       obj.info()->addParam(obj, "minScale", obj.minScale, false, 0, 0,
468                                            "Minimal scale to detect.");
469                       obj.info()->addParam(obj, "maxScale", obj.maxScale, false, 0, 0,
470                                            "Maximal scale to detect.");
471                       obj.info()->addParam(obj, "scaleStep", obj.scaleStep, false, 0, 0,
472                                            "Scale step."));
473
474     GHT_Ballard_PosScale::GHT_Ballard_PosScale()
475     {
476         minScale = 0.5;
477         maxScale = 2.0;
478         scaleStep = 0.05;
479     }
480
481     class GHT_Ballard_PosScale::Worker : public ParallelLoopBody
482     {
483     public:
484         explicit Worker(GHT_Ballard_PosScale* base_) : base(base_) {}
485
486         void operator ()(const Range& range) const;
487
488     private:
489         GHT_Ballard_PosScale* base;
490     };
491
492     void GHT_Ballard_PosScale::Worker::operator ()(const Range& range) const
493     {
494         const double thetaScale = base->levels / 360.0;
495         const double idp = 1.0 / base->dp;
496
497         for (int s = range.start; s < range.end; ++s)
498         {
499             const double scale = base->minScale + s * base->scaleStep;
500
501             Mat curHist(base->hist.size[1], base->hist.size[2], CV_32SC1, base->hist.ptr(s + 1), base->hist.step[1]);
502
503             for (int y = 0; y < base->imageSize.height; ++y)
504             {
505                 const uchar* edgesRow = base->imageEdges.ptr(y);
506                 const float* dxRow = base->imageDx.ptr<float>(y);
507                 const float* dyRow = base->imageDy.ptr<float>(y);
508
509                 for (int x = 0; x < base->imageSize.width; ++x)
510                 {
511                     const Point2d p(x, y);
512
513                     if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
514                     {
515                         const float theta = fastAtan2(dyRow[x], dxRow[x]);
516                         const int n = cvRound(theta * thetaScale);
517
518                         const vector<Point>& r_row = base->r_table[n];
519
520                         for (size_t j = 0; j < r_row.size(); ++j)
521                         {
522                             Point2d d = r_row[j];
523                             Point2d c = p - d * scale;
524
525                             c.x *= idp;
526                             c.y *= idp;
527
528                             if (c.x >= 0 && c.x < base->hist.size[2] - 2 && c.y >= 0 && c.y < base->hist.size[1] - 2)
529                                 ++curHist.at<int>(cvRound(c.y + 1), cvRound(c.x + 1));
530                         }
531                     }
532                 }
533             }
534         }
535     }
536
537     void GHT_Ballard_PosScale::calcHist()
538     {
539         CV_Assert(imageEdges.type() == CV_8UC1);
540         CV_Assert(imageDx.type() == CV_32FC1 && imageDx.size() == imageSize);
541         CV_Assert(imageDy.type() == imageDx.type() && imageDy.size() == imageSize);
542         CV_Assert(levels > 0 && r_table.size() == static_cast<size_t>(levels + 1));
543         CV_Assert(dp > 0.0);
544         CV_Assert(minScale > 0.0 && minScale < maxScale);
545         CV_Assert(scaleStep > 0.0);
546
547         const double idp = 1.0 / dp;
548         const int scaleRange = cvCeil((maxScale - minScale) / scaleStep);
549
550         const int sizes[] = {scaleRange + 2, cvCeil(imageSize.height * idp) + 2, cvCeil(imageSize.width * idp) + 2};
551         hist.create(3, sizes, CV_32SC1);
552         hist.setTo(0);
553
554         parallel_for_(Range(0, scaleRange), Worker(this));
555     }
556
557     void GHT_Ballard_PosScale::findPosInHist()
558     {
559         CV_Assert(votesThreshold > 0);
560
561         const int scaleRange = hist.size[0] - 2;
562         const int histRows = hist.size[1] - 2;
563         const int histCols = hist.size[2] - 2;
564
565         for (int s = 0; s < scaleRange; ++s)
566         {
567             const float scale = static_cast<float>(minScale + s * scaleStep);
568
569             const Mat prevHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(s), hist.step[1]);
570             const Mat curHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(s + 1), hist.step[1]);
571             const Mat nextHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(s + 2), hist.step[1]);
572
573             for(int y = 0; y < histRows; ++y)
574             {
575                 const int* prevHistRow = prevHist.ptr<int>(y + 1);
576                 const int* prevRow = curHist.ptr<int>(y);
577                 const int* curRow = curHist.ptr<int>(y + 1);
578                 const int* nextRow = curHist.ptr<int>(y + 2);
579                 const int* nextHistRow = nextHist.ptr<int>(y + 1);
580
581                 for(int x = 0; x < histCols; ++x)
582                 {
583                     const int votes = curRow[x + 1];
584
585                     if (votes > votesThreshold &&
586                         votes > curRow[x] &&
587                         votes >= curRow[x + 2] &&
588                         votes > prevRow[x + 1] &&
589                         votes >= nextRow[x + 1] &&
590                         votes > prevHistRow[x + 1] &&
591                         votes >= nextHistRow[x + 1])
592                     {
593                         posOutBuf.push_back(Vec4f(static_cast<float>(x * dp), static_cast<float>(y * dp), scale, 0.0f));
594                         voteOutBuf.push_back(Vec3i(votes, votes, 0));
595                     }
596                 }
597             }
598         }
599     }
600
601     /////////////////////////////////////
602     // POSITION & ROTATION
603
604     class GHT_Ballard_PosRotation : public GHT_Ballard_Pos
605     {
606     public:
607         AlgorithmInfo* info() const;
608
609         GHT_Ballard_PosRotation();
610
611     protected:
612         void calcHist();
613         void findPosInHist();
614
615         double minAngle;
616         double maxAngle;
617         double angleStep;
618
619         class Worker;
620         friend class Worker;
621     };
622
623     CV_INIT_ALGORITHM(GHT_Ballard_PosRotation, "GeneralizedHough.POSITION_ROTATION",
624                       obj.info()->addParam(obj, "minDist", obj.minDist, false, 0, 0,
625                                            "Minimum distance between the centers of the detected objects.");
626                       obj.info()->addParam(obj, "levels", obj.levels, false, 0, 0,
627                                            "R-Table levels.");
628                       obj.info()->addParam(obj, "votesThreshold", obj.votesThreshold, false, 0, 0,
629                                            "The accumulator threshold for the template centers at the detection stage. The smaller it is, the more false positions may be detected.");
630                       obj.info()->addParam(obj, "dp", obj.dp, false, 0, 0,
631                                            "Inverse ratio of the accumulator resolution to the image resolution.");
632                       obj.info()->addParam(obj, "minAngle", obj.minAngle, false, 0, 0,
633                                            "Minimal rotation angle to detect in degrees.");
634                       obj.info()->addParam(obj, "maxAngle", obj.maxAngle, false, 0, 0,
635                                            "Maximal rotation angle to detect in degrees.");
636                       obj.info()->addParam(obj, "angleStep", obj.angleStep, false, 0, 0,
637                                            "Angle step in degrees."));
638
639     GHT_Ballard_PosRotation::GHT_Ballard_PosRotation()
640     {
641         minAngle = 0.0;
642         maxAngle = 360.0;
643         angleStep = 1.0;
644     }
645
646     class GHT_Ballard_PosRotation::Worker : public ParallelLoopBody
647     {
648     public:
649         explicit Worker(GHT_Ballard_PosRotation* base_) : base(base_) {}
650
651         void operator ()(const Range& range) const;
652
653     private:
654         GHT_Ballard_PosRotation* base;
655     };
656
657     void GHT_Ballard_PosRotation::Worker::operator ()(const Range& range) const
658     {
659         const double thetaScale = base->levels / 360.0;
660         const double idp = 1.0 / base->dp;
661
662         for (int a = range.start; a < range.end; ++a)
663         {
664             const double angle = base->minAngle + a * base->angleStep;
665
666             const double sinA = ::sin(toRad(angle));
667             const double cosA = ::cos(toRad(angle));
668
669             Mat curHist(base->hist.size[1], base->hist.size[2], CV_32SC1, base->hist.ptr(a + 1), base->hist.step[1]);
670
671             for (int y = 0; y < base->imageSize.height; ++y)
672             {
673                 const uchar* edgesRow = base->imageEdges.ptr(y);
674                 const float* dxRow = base->imageDx.ptr<float>(y);
675                 const float* dyRow = base->imageDy.ptr<float>(y);
676
677                 for (int x = 0; x < base->imageSize.width; ++x)
678                 {
679                     const Point2d p(x, y);
680
681                     if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
682                     {
683                         double theta = fastAtan2(dyRow[x], dxRow[x]) - angle;
684                         if (theta < 0)
685                             theta += 360.0;
686                         const int n = cvRound(theta * thetaScale);
687
688                         const vector<Point>& r_row = base->r_table[n];
689
690                         for (size_t j = 0; j < r_row.size(); ++j)
691                         {
692                             Point2d d = r_row[j];
693                             Point2d c = p - Point2d(d.x * cosA - d.y * sinA, d.x * sinA + d.y * cosA);
694
695                             c.x *= idp;
696                             c.y *= idp;
697
698                             if (c.x >= 0 && c.x < base->hist.size[2] - 2 && c.y >= 0 && c.y < base->hist.size[1] - 2)
699                                 ++curHist.at<int>(cvRound(c.y + 1), cvRound(c.x + 1));
700                         }
701                     }
702                 }
703             }
704         }
705     }
706
707     void GHT_Ballard_PosRotation::calcHist()
708     {
709         CV_Assert(imageEdges.type() == CV_8UC1);
710         CV_Assert(imageDx.type() == CV_32FC1 && imageDx.size() == imageSize);
711         CV_Assert(imageDy.type() == imageDx.type() && imageDy.size() == imageSize);
712         CV_Assert(levels > 0 && r_table.size() == static_cast<size_t>(levels + 1));
713         CV_Assert(dp > 0.0);
714         CV_Assert(minAngle >= 0.0 && minAngle < maxAngle && maxAngle <= 360.0);
715         CV_Assert(angleStep > 0.0 && angleStep < 360.0);
716
717         const double idp = 1.0 / dp;
718         const int angleRange = cvCeil((maxAngle - minAngle) / angleStep);
719
720         const int sizes[] = {angleRange + 2, cvCeil(imageSize.height * idp) + 2, cvCeil(imageSize.width * idp) + 2};
721         hist.create(3, sizes, CV_32SC1);
722         hist.setTo(0);
723
724         parallel_for_(Range(0, angleRange), Worker(this));
725     }
726
727     void GHT_Ballard_PosRotation::findPosInHist()
728     {
729         CV_Assert(votesThreshold > 0);
730
731         const int angleRange = hist.size[0] - 2;
732         const int histRows = hist.size[1] - 2;
733         const int histCols = hist.size[2] - 2;
734
735         for (int a = 0; a < angleRange; ++a)
736         {
737             const float angle = static_cast<float>(minAngle + a * angleStep);
738
739             const Mat prevHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(a), hist.step[1]);
740             const Mat curHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(a + 1), hist.step[1]);
741             const Mat nextHist(histRows + 2, histCols + 2, CV_32SC1, hist.ptr(a + 2), hist.step[1]);
742
743             for(int y = 0; y < histRows; ++y)
744             {
745                 const int* prevHistRow = prevHist.ptr<int>(y + 1);
746                 const int* prevRow = curHist.ptr<int>(y);
747                 const int* curRow = curHist.ptr<int>(y + 1);
748                 const int* nextRow = curHist.ptr<int>(y + 2);
749                 const int* nextHistRow = nextHist.ptr<int>(y + 1);
750
751                 for(int x = 0; x < histCols; ++x)
752                 {
753                     const int votes = curRow[x + 1];
754
755                     if (votes > votesThreshold &&
756                         votes > curRow[x] &&
757                         votes >= curRow[x + 2] &&
758                         votes > prevRow[x + 1] &&
759                         votes >= nextRow[x + 1] &&
760                         votes > prevHistRow[x + 1] &&
761                         votes >= nextHistRow[x + 1])
762                     {
763                         posOutBuf.push_back(Vec4f(static_cast<float>(x * dp), static_cast<float>(y * dp), 1.0f, angle));
764                         voteOutBuf.push_back(Vec3i(votes, 0, votes));
765                     }
766                 }
767             }
768         }
769     }
770
771     /////////////////////////////////////////
772     // POSITION & SCALE & ROTATION
773
774     double clampAngle(double a)
775     {
776         double res = a;
777
778         while (res > 360.0)
779             res -= 360.0;
780         while (res < 0)
781             res += 360.0;
782
783         return res;
784     }
785
786     bool angleEq(double a, double b, double eps = 1.0)
787     {
788         return (fabs(clampAngle(a - b)) <= eps);
789     }
790
791     class GHT_Guil_Full : public GHT_Pos
792     {
793     public:
794         AlgorithmInfo* info() const;
795
796         GHT_Guil_Full();
797
798     protected:
799         void releaseImpl();
800
801         void processTempl();
802         void processImage();
803
804         struct ContourPoint
805         {
806             Point2d pos;
807             double theta;
808         };
809
810         struct Feature
811         {
812             ContourPoint p1;
813             ContourPoint p2;
814
815             double alpha12;
816             double d12;
817
818             Point2d r1;
819             Point2d r2;
820         };
821
822         void buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, vector< vector<Feature> >& features, Point2d center = Point2d());
823         void getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, vector<ContourPoint>& points);
824
825         void calcOrientation();
826         void calcScale(double angle);
827         void calcPosition(double angle, int angleVotes, double scale, int scaleVotes);
828
829         int maxSize;
830         double xi;
831         int levels;
832         double angleEpsilon;
833
834         double minAngle;
835         double maxAngle;
836         double angleStep;
837         int angleThresh;
838
839         double minScale;
840         double maxScale;
841         double scaleStep;
842         int scaleThresh;
843
844         double dp;
845         int posThresh;
846
847         vector< vector<Feature> > templFeatures;
848         vector< vector<Feature> > imageFeatures;
849
850         vector< pair<double, int> > angles;
851         vector< pair<double, int> > scales;
852     };
853
854     CV_INIT_ALGORITHM(GHT_Guil_Full, "GeneralizedHough.POSITION_SCALE_ROTATION",
855                       obj.info()->addParam(obj, "minDist", obj.minDist, false, 0, 0,
856                                            "Minimum distance between the centers of the detected objects.");
857                       obj.info()->addParam(obj, "maxSize", obj.maxSize, false, 0, 0,
858                                            "Maximal size of inner buffers.");
859                       obj.info()->addParam(obj, "xi", obj.xi, false, 0, 0,
860                                            "Angle difference in degrees between two points in feature.");
861                       obj.info()->addParam(obj, "levels", obj.levels, false, 0, 0,
862                                            "Feature table levels.");
863                       obj.info()->addParam(obj, "angleEpsilon", obj.angleEpsilon, false, 0, 0,
864                                            "Maximal difference between angles that treated as equal.");
865                       obj.info()->addParam(obj, "minAngle", obj.minAngle, false, 0, 0,
866                                            "Minimal rotation angle to detect in degrees.");
867                       obj.info()->addParam(obj, "maxAngle", obj.maxAngle, false, 0, 0,
868                                            "Maximal rotation angle to detect in degrees.");
869                       obj.info()->addParam(obj, "angleStep", obj.angleStep, false, 0, 0,
870                                            "Angle step in degrees.");
871                       obj.info()->addParam(obj, "angleThresh", obj.angleThresh, false, 0, 0,
872                                            "Angle threshold.");
873                       obj.info()->addParam(obj, "minScale", obj.minScale, false, 0, 0,
874                                            "Minimal scale to detect.");
875                       obj.info()->addParam(obj, "maxScale", obj.maxScale, false, 0, 0,
876                                            "Maximal scale to detect.");
877                       obj.info()->addParam(obj, "scaleStep", obj.scaleStep, false, 0, 0,
878                                            "Scale step.");
879                       obj.info()->addParam(obj, "scaleThresh", obj.scaleThresh, false, 0, 0,
880                                            "Scale threshold.");
881                       obj.info()->addParam(obj, "dp", obj.dp, false, 0, 0,
882                                            "Inverse ratio of the accumulator resolution to the image resolution.");
883                       obj.info()->addParam(obj, "posThresh", obj.posThresh, false, 0, 0,
884                                            "Position threshold."));
885
886     GHT_Guil_Full::GHT_Guil_Full()
887     {
888         maxSize = 1000;
889         xi = 90.0;
890         levels = 360;
891         angleEpsilon = 1.0;
892
893         minAngle = 0.0;
894         maxAngle = 360.0;
895         angleStep = 1.0;
896         angleThresh = 15000;
897
898         minScale = 0.5;
899         maxScale = 2.0;
900         scaleStep = 0.05;
901         scaleThresh = 1000;
902
903         dp = 1.0;
904         posThresh = 100;
905     }
906
907     void GHT_Guil_Full::releaseImpl()
908     {
909         GHT_Pos::releaseImpl();
910
911         releaseVector(templFeatures);
912         releaseVector(imageFeatures);
913
914         releaseVector(angles);
915         releaseVector(scales);
916     }
917
918     void GHT_Guil_Full::processTempl()
919     {
920         buildFeatureList(templEdges, templDx, templDy, templFeatures, templCenter);
921     }
922
923     void GHT_Guil_Full::processImage()
924     {
925         buildFeatureList(imageEdges, imageDx, imageDy, imageFeatures);
926
927         calcOrientation();
928
929         for (size_t i = 0; i < angles.size(); ++i)
930         {
931             const double angle = angles[i].first;
932             const int angleVotes = angles[i].second;
933
934             calcScale(angle);
935
936             for (size_t j = 0; j < scales.size(); ++j)
937             {
938                 const double scale = scales[j].first;
939                 const int scaleVotes = scales[j].second;
940
941                 calcPosition(angle, angleVotes, scale, scaleVotes);
942             }
943         }
944     }
945
946     void GHT_Guil_Full::buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, vector< vector<Feature> >& features, Point2d center)
947     {
948         CV_Assert(levels > 0);
949
950         const double maxDist = sqrt((double) templSize.width * templSize.width + templSize.height * templSize.height) * maxScale;
951
952         const double alphaScale = levels / 360.0;
953
954         vector<ContourPoint> points;
955         getContourPoints(edges, dx, dy, points);
956
957         features.resize(levels + 1);
958         for_each(features.begin(), features.end(), mem_fun_ref(&vector<Feature>::clear));
959         for_each(features.begin(), features.end(), bind2nd(mem_fun_ref(&vector<Feature>::reserve), maxSize));
960
961         for (size_t i = 0; i < points.size(); ++i)
962         {
963             ContourPoint p1 = points[i];
964
965             for (size_t j = 0; j < points.size(); ++j)
966             {
967                 ContourPoint p2 = points[j];
968
969                 if (angleEq(p1.theta - p2.theta, xi, angleEpsilon))
970                 {
971                     const Point2d d = p1.pos - p2.pos;
972
973                     Feature f;
974
975                     f.p1 = p1;
976                     f.p2 = p2;
977
978                     f.alpha12 = clampAngle(fastAtan2(d.y, d.x) - p1.theta);
979                     f.d12 = norm(d);
980
981                     if (f.d12 > maxDist)
982                         continue;
983
984                     f.r1 = p1.pos - center;
985                     f.r2 = p2.pos - center;
986
987                     const int n = cvRound(f.alpha12 * alphaScale);
988
989                     if (features[n].size() < static_cast<size_t>(maxSize))
990                         features[n].push_back(f);
991                 }
992             }
993         }
994     }
995
996     void GHT_Guil_Full::getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, vector<ContourPoint>& points)
997     {
998         CV_Assert(edges.type() == CV_8UC1);
999         CV_Assert(dx.type() == CV_32FC1 && dx.size == edges.size);
1000         CV_Assert(dy.type() == dx.type() && dy.size == edges.size);
1001
1002         points.clear();
1003         points.reserve(edges.size().area());
1004
1005         for (int y = 0; y < edges.rows; ++y)
1006         {
1007             const uchar* edgesRow = edges.ptr(y);
1008             const float* dxRow = dx.ptr<float>(y);
1009             const float* dyRow = dy.ptr<float>(y);
1010
1011             for (int x = 0; x < edges.cols; ++x)
1012             {
1013                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
1014                 {
1015                     ContourPoint p;
1016
1017                     p.pos = Point2d(x, y);
1018                     p.theta = fastAtan2(dyRow[x], dxRow[x]);
1019
1020                     points.push_back(p);
1021                 }
1022             }
1023         }
1024     }
1025
1026     void GHT_Guil_Full::calcOrientation()
1027     {
1028         CV_Assert(levels > 0);
1029         CV_Assert(templFeatures.size() == static_cast<size_t>(levels + 1));
1030         CV_Assert(imageFeatures.size() == templFeatures.size());
1031         CV_Assert(minAngle >= 0.0 && minAngle < maxAngle && maxAngle <= 360.0);
1032         CV_Assert(angleStep > 0.0 && angleStep < 360.0);
1033         CV_Assert(angleThresh > 0);
1034
1035         const double iAngleStep = 1.0 / angleStep;
1036         const int angleRange = cvCeil((maxAngle - minAngle) * iAngleStep);
1037
1038         vector<int> OHist(angleRange + 1, 0);
1039         for (int i = 0; i <= levels; ++i)
1040         {
1041             const vector<Feature>& templRow = templFeatures[i];
1042             const vector<Feature>& imageRow = imageFeatures[i];
1043
1044             for (size_t j = 0; j < templRow.size(); ++j)
1045             {
1046                 Feature templF = templRow[j];
1047
1048                 for (size_t k = 0; k < imageRow.size(); ++k)
1049                 {
1050                     Feature imF = imageRow[k];
1051
1052                     const double angle = clampAngle(imF.p1.theta - templF.p1.theta);
1053                     if (angle >= minAngle && angle <= maxAngle)
1054                     {
1055                         const int n = cvRound((angle - minAngle) * iAngleStep);
1056                         ++OHist[n];
1057                     }
1058                 }
1059             }
1060         }
1061
1062         angles.clear();
1063
1064         for (int n = 0; n < angleRange; ++n)
1065         {
1066             if (OHist[n] >= angleThresh)
1067             {
1068                 const double angle = minAngle + n * angleStep;
1069                 angles.push_back(make_pair(angle, OHist[n]));
1070             }
1071         }
1072     }
1073
1074     void GHT_Guil_Full::calcScale(double angle)
1075     {
1076         CV_Assert(levels > 0);
1077         CV_Assert(templFeatures.size() == static_cast<size_t>(levels + 1));
1078         CV_Assert(imageFeatures.size() == templFeatures.size());
1079         CV_Assert(minScale > 0.0 && minScale < maxScale);
1080         CV_Assert(scaleStep > 0.0);
1081         CV_Assert(scaleThresh > 0);
1082
1083         const double iScaleStep = 1.0 / scaleStep;
1084         const int scaleRange = cvCeil((maxScale - minScale) * iScaleStep);
1085
1086         vector<int> SHist(scaleRange + 1, 0);
1087
1088         for (int i = 0; i <= levels; ++i)
1089         {
1090             const vector<Feature>& templRow = templFeatures[i];
1091             const vector<Feature>& imageRow = imageFeatures[i];
1092
1093             for (size_t j = 0; j < templRow.size(); ++j)
1094             {
1095                 Feature templF = templRow[j];
1096
1097                 templF.p1.theta += angle;
1098
1099                 for (size_t k = 0; k < imageRow.size(); ++k)
1100                 {
1101                     Feature imF = imageRow[k];
1102
1103                     if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon))
1104                     {
1105                         const double scale = imF.d12 / templF.d12;
1106                         if (scale >= minScale && scale <= maxScale)
1107                         {
1108                             const int s = cvRound((scale - minScale) * iScaleStep);
1109                             ++SHist[s];
1110                         }
1111                     }
1112                 }
1113             }
1114         }
1115
1116         scales.clear();
1117
1118         for (int s = 0; s < scaleRange; ++s)
1119         {
1120             if (SHist[s] >= scaleThresh)
1121             {
1122                 const double scale = minScale + s * scaleStep;
1123                 scales.push_back(make_pair(scale, SHist[s]));
1124             }
1125         }
1126     }
1127
1128     void GHT_Guil_Full::calcPosition(double angle, int angleVotes, double scale, int scaleVotes)
1129     {
1130         CV_Assert(levels > 0);
1131         CV_Assert(templFeatures.size() == static_cast<size_t>(levels + 1));
1132         CV_Assert(imageFeatures.size() == templFeatures.size());
1133         CV_Assert(dp > 0.0);
1134         CV_Assert(posThresh > 0);
1135
1136         const double sinVal = sin(toRad(angle));
1137         const double cosVal = cos(toRad(angle));
1138         const double idp = 1.0 / dp;
1139
1140         const int histRows = cvCeil(imageSize.height * idp);
1141         const int histCols = cvCeil(imageSize.width * idp);
1142
1143         Mat DHist(histRows + 2, histCols + 2, CV_32SC1, Scalar::all(0));
1144
1145         for (int i = 0; i <= levels; ++i)
1146         {
1147             const vector<Feature>& templRow = templFeatures[i];
1148             const vector<Feature>& imageRow = imageFeatures[i];
1149
1150             for (size_t j = 0; j < templRow.size(); ++j)
1151             {
1152                 Feature templF = templRow[j];
1153
1154                 templF.p1.theta += angle;
1155
1156                 templF.r1 *= scale;
1157                 templF.r2 *= scale;
1158
1159                 templF.r1 = Point2d(cosVal * templF.r1.x - sinVal * templF.r1.y, sinVal * templF.r1.x + cosVal * templF.r1.y);
1160                 templF.r2 = Point2d(cosVal * templF.r2.x - sinVal * templF.r2.y, sinVal * templF.r2.x + cosVal * templF.r2.y);
1161
1162                 for (size_t k = 0; k < imageRow.size(); ++k)
1163                 {
1164                     Feature imF = imageRow[k];
1165
1166                     if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon))
1167                     {
1168                         Point2d c1, c2;
1169
1170                         c1 = imF.p1.pos - templF.r1;
1171                         c1 *= idp;
1172
1173                         c2 = imF.p2.pos - templF.r2;
1174                         c2 *= idp;
1175
1176                         if (fabs(c1.x - c2.x) > 1 || fabs(c1.y - c2.y) > 1)
1177                             continue;
1178
1179                         if (c1.y >= 0 && c1.y < histRows && c1.x >= 0 && c1.x < histCols)
1180                             ++DHist.at<int>(cvRound(c1.y) + 1, cvRound(c1.x) + 1);
1181                     }
1182                 }
1183             }
1184         }
1185
1186         for(int y = 0; y < histRows; ++y)
1187         {
1188             const int* prevRow = DHist.ptr<int>(y);
1189             const int* curRow = DHist.ptr<int>(y + 1);
1190             const int* nextRow = DHist.ptr<int>(y + 2);
1191
1192             for(int x = 0; x < histCols; ++x)
1193             {
1194                 const int votes = curRow[x + 1];
1195
1196                 if (votes > posThresh && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
1197                 {
1198                     posOutBuf.push_back(Vec4f(static_cast<float>(x * dp), static_cast<float>(y * dp), static_cast<float>(scale), static_cast<float>(angle)));
1199                     voteOutBuf.push_back(Vec3i(votes, scaleVotes, angleVotes));
1200                 }
1201             }
1202         }
1203     }
1204 }
1205
1206 Ptr<GeneralizedHough> cv::GeneralizedHough::create(int method)
1207 {
1208     switch (method)
1209     {
1210     case GHT_POSITION:
1211         CV_Assert( !GHT_Ballard_Pos_info_auto.name().empty() );
1212         return new GHT_Ballard_Pos();
1213
1214     case (GHT_POSITION | GHT_SCALE):
1215         CV_Assert( !GHT_Ballard_PosScale_info_auto.name().empty() );
1216         return new GHT_Ballard_PosScale();
1217
1218     case (GHT_POSITION | GHT_ROTATION):
1219         CV_Assert( !GHT_Ballard_PosRotation_info_auto.name().empty() );
1220         return new GHT_Ballard_PosRotation();
1221
1222     case (GHT_POSITION | GHT_SCALE | GHT_ROTATION):
1223         CV_Assert( !GHT_Guil_Full_info_auto.name().empty() );
1224         return new GHT_Guil_Full();
1225     }
1226
1227     CV_Error(CV_StsBadArg, "Unsupported method");
1228     return Ptr<GeneralizedHough>();
1229 }
1230
1231 cv::GeneralizedHough::~GeneralizedHough()
1232 {
1233 }
1234
1235 void cv::GeneralizedHough::setTemplate(InputArray _templ, int cannyThreshold, Point templCenter)
1236 {
1237     Mat templ = _templ.getMat();
1238
1239     CV_Assert(templ.type() == CV_8UC1);
1240     CV_Assert(cannyThreshold > 0);
1241
1242     Canny(templ, edges_, cannyThreshold / 2, cannyThreshold);
1243     Sobel(templ, dx_, CV_32F, 1, 0);
1244     Sobel(templ, dy_, CV_32F, 0, 1);
1245
1246     if (templCenter == Point(-1, -1))
1247         templCenter = Point(templ.cols / 2, templ.rows / 2);
1248
1249     setTemplateImpl(edges_, dx_, dy_, templCenter);
1250 }
1251
1252 void cv::GeneralizedHough::setTemplate(InputArray _edges, InputArray _dx, InputArray _dy, Point templCenter)
1253 {
1254     Mat edges = _edges.getMat();
1255     Mat dx = _dx.getMat();
1256     Mat dy = _dy.getMat();
1257
1258     if (templCenter == Point(-1, -1))
1259         templCenter = Point(edges.cols / 2, edges.rows / 2);
1260
1261     setTemplateImpl(edges, dx, dy, templCenter);
1262 }
1263
1264 void cv::GeneralizedHough::detect(InputArray _image, OutputArray positions, OutputArray votes, int cannyThreshold)
1265 {
1266     Mat image = _image.getMat();
1267
1268     CV_Assert(image.type() == CV_8UC1);
1269     CV_Assert(cannyThreshold > 0);
1270
1271     Canny(image, edges_, cannyThreshold / 2, cannyThreshold);
1272     Sobel(image, dx_, CV_32F, 1, 0);
1273     Sobel(image, dy_, CV_32F, 0, 1);
1274
1275     detectImpl(edges_, dx_, dy_, positions, votes);
1276 }
1277
1278 void cv::GeneralizedHough::detect(InputArray _edges, InputArray _dx, InputArray _dy, OutputArray positions, OutputArray votes)
1279 {
1280     cv::Mat edges = _edges.getMat();
1281     cv::Mat dx = _dx.getMat();
1282     cv::Mat dy = _dy.getMat();
1283
1284     detectImpl(edges, dx, dy, positions, votes);
1285 }
1286
1287 void cv::GeneralizedHough::release()
1288 {
1289     edges_.release();
1290     dx_.release();
1291     dy_.release();
1292     releaseImpl();
1293 }