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