root/modules/imgproc/src/generalized_hough.cpp

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. toRad
  2. notNull
  3. calcEdges
  4. setTemplateImpl
  5. setTemplateImpl
  6. detectImpl
  7. detectImpl
  8. filterMinDist
  9. convertTo
  10. setTemplate
  11. setTemplate
  12. detect
  13. detect
  14. setCannyLowThresh
  15. getCannyLowThresh
  16. setCannyHighThresh
  17. getCannyHighThresh
  18. setMinDist
  19. getMinDist
  20. setDp
  21. getDp
  22. setMaxBufferSize
  23. getMaxBufferSize
  24. setLevels
  25. getLevels
  26. setVotesThreshold
  27. getVotesThreshold
  28. processTempl
  29. processImage
  30. calcHist
  31. findPosInHist
  32. createGeneralizedHoughBallard
  33. setTemplate
  34. setTemplate
  35. detect
  36. detect
  37. setCannyLowThresh
  38. getCannyLowThresh
  39. setCannyHighThresh
  40. getCannyHighThresh
  41. setMinDist
  42. getMinDist
  43. setDp
  44. getDp
  45. setMaxBufferSize
  46. getMaxBufferSize
  47. setXi
  48. getXi
  49. setLevels
  50. getLevels
  51. setAngleEpsilon
  52. getAngleEpsilon
  53. setMinAngle
  54. getMinAngle
  55. setMaxAngle
  56. getMaxAngle
  57. setAngleStep
  58. getAngleStep
  59. setAngleThresh
  60. getAngleThresh
  61. setMinScale
  62. getMinScale
  63. setMaxScale
  64. getMaxScale
  65. setScaleStep
  66. getScaleStep
  67. setScaleThresh
  68. getScaleThresh
  69. setPosThresh
  70. getPosThresh
  71. clampAngle
  72. angleEq
  73. processTempl
  74. processImage
  75. buildFeatureList
  76. getContourPoints
  77. calcOrientation
  78. calcScale
  79. calcPosition
  80. createGeneralizedHoughGuil
/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <functional>
#include <limits>

using namespace cv;

// common

namespace
{
    double toRad(double a)
    {
        return a * CV_PI / 180.0;
    }

    bool notNull(float v)
    {
        return fabs(v) > std::numeric_limits<float>::epsilon();
    }

    class GeneralizedHoughBase
    {
    protected:
        GeneralizedHoughBase();
        virtual ~GeneralizedHoughBase() {}

        void setTemplateImpl(InputArray templ, Point templCenter);
        void setTemplateImpl(InputArray edges, InputArray dx, InputArray dy, Point templCenter);

        void detectImpl(InputArray image, OutputArray positions, OutputArray votes);
        void detectImpl(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes);

        virtual void processTempl() = 0;
        virtual void processImage() = 0;

        int cannyLowThresh_;
        int cannyHighThresh_;
        double minDist_;
        double dp_;

        Size templSize_;
        Point templCenter_;
        Mat templEdges_;
        Mat templDx_;
        Mat templDy_;

        Size imageSize_;
        Mat imageEdges_;
        Mat imageDx_;
        Mat imageDy_;

        std::vector<Vec4f> posOutBuf_;
        std::vector<Vec3i> voteOutBuf_;

    private:
        void calcEdges(InputArray src, Mat& edges, Mat& dx, Mat& dy);
        void filterMinDist();
        void convertTo(OutputArray positions, OutputArray votes);
    };

    GeneralizedHoughBase::GeneralizedHoughBase()
    {
        cannyLowThresh_ = 50;
        cannyHighThresh_ = 100;
        minDist_ = 1.0;
        dp_ = 1.0;
    }

    void GeneralizedHoughBase::calcEdges(InputArray _src, Mat& edges, Mat& dx, Mat& dy)
    {
        Mat src = _src.getMat();

        CV_Assert( src.type() == CV_8UC1 );
        CV_Assert( cannyLowThresh_ > 0 && cannyLowThresh_ < cannyHighThresh_ );

        Canny(src, edges, cannyLowThresh_, cannyHighThresh_);
        Sobel(src, dx, CV_32F, 1, 0);
        Sobel(src, dy, CV_32F, 0, 1);
    }

    void GeneralizedHoughBase::setTemplateImpl(InputArray templ, Point templCenter)
    {
        calcEdges(templ, templEdges_, templDx_, templDy_);

        if (templCenter == Point(-1, -1))
            templCenter = Point(templEdges_.cols / 2, templEdges_.rows / 2);

        templSize_ = templEdges_.size();
        templCenter_ = templCenter;

        processTempl();
    }

    void GeneralizedHoughBase::setTemplateImpl(InputArray edges, InputArray dx, InputArray dy, Point templCenter)
    {
        edges.getMat().copyTo(templEdges_);
        dx.getMat().copyTo(templDx_);
        dy.getMat().copyTo(templDy_);

        CV_Assert( templEdges_.type() == CV_8UC1 );
        CV_Assert( templDx_.type() == CV_32FC1 && templDx_.size() == templEdges_.size() );
        CV_Assert( templDy_.type() == templDx_.type() && templDy_.size() == templEdges_.size() );

        if (templCenter == Point(-1, -1))
            templCenter = Point(templEdges_.cols / 2, templEdges_.rows / 2);

        templSize_ = templEdges_.size();
        templCenter_ = templCenter;

        processTempl();
    }

    void GeneralizedHoughBase::detectImpl(InputArray image, OutputArray positions, OutputArray votes)
    {
        calcEdges(image, imageEdges_, imageDx_, imageDy_);

        imageSize_ = imageEdges_.size();

        posOutBuf_.clear();
        voteOutBuf_.clear();

        processImage();

        if (!posOutBuf_.empty())
        {
            if (minDist_ > 1)
                filterMinDist();
            convertTo(positions, votes);
        }
        else
        {
            positions.release();
            if (votes.needed())
                votes.release();
        }
    }

    void GeneralizedHoughBase::detectImpl(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes)
    {
        edges.getMat().copyTo(imageEdges_);
        dx.getMat().copyTo(imageDx_);
        dy.getMat().copyTo(imageDy_);

        CV_Assert( imageEdges_.type() == CV_8UC1 );
        CV_Assert( imageDx_.type() == CV_32FC1 && imageDx_.size() == imageEdges_.size() );
        CV_Assert( imageDy_.type() == imageDx_.type() && imageDy_.size() == imageEdges_.size() );

        imageSize_ = imageEdges_.size();

        posOutBuf_.clear();
        voteOutBuf_.clear();

        processImage();

        if (!posOutBuf_.empty())
        {
            if (minDist_ > 1)
                filterMinDist();
            convertTo(positions, votes);
        }
        else
        {
            positions.release();
            if (votes.needed())
                votes.release();
        }
    }

    class Vec3iGreaterThanIdx
    {
    public:
        Vec3iGreaterThanIdx( const Vec3i* _arr ) : arr(_arr) {}
        bool operator()(size_t a, size_t b) const { return arr[a][0] > arr[b][0]; }
        const Vec3i* arr;
    };

    void GeneralizedHoughBase::filterMinDist()
    {
        size_t oldSize = posOutBuf_.size();
        const bool hasVotes = !voteOutBuf_.empty();

        CV_Assert( !hasVotes || voteOutBuf_.size() == oldSize );

        std::vector<Vec4f> oldPosBuf(posOutBuf_);
        std::vector<Vec3i> oldVoteBuf(voteOutBuf_);

        std::vector<size_t> indexies(oldSize);
        for (size_t i = 0; i < oldSize; ++i)
            indexies[i] = i;
        std::sort(indexies.begin(), indexies.end(), Vec3iGreaterThanIdx(&oldVoteBuf[0]));

        posOutBuf_.clear();
        voteOutBuf_.clear();

        const int cellSize = cvRound(minDist_);
        const int gridWidth = (imageSize_.width + cellSize - 1) / cellSize;
        const int gridHeight = (imageSize_.height + cellSize - 1) / cellSize;

        std::vector< std::vector<Point2f> > grid(gridWidth * gridHeight);

        const double minDist2 = minDist_ * minDist_;

        for (size_t i = 0; i < oldSize; ++i)
        {
            const size_t ind = indexies[i];

            Point2f p(oldPosBuf[ind][0], oldPosBuf[ind][1]);

            bool good = true;

            const int xCell = static_cast<int>(p.x / cellSize);
            const int yCell = static_cast<int>(p.y / cellSize);

            int x1 = xCell - 1;
            int y1 = yCell - 1;
            int x2 = xCell + 1;
            int y2 = yCell + 1;

            // boundary check
            x1 = std::max(0, x1);
            y1 = std::max(0, y1);
            x2 = std::min(gridWidth - 1, x2);
            y2 = std::min(gridHeight - 1, y2);

            for (int yy = y1; yy <= y2; ++yy)
            {
                for (int xx = x1; xx <= x2; ++xx)
                {
                    const std::vector<Point2f>& m = grid[yy * gridWidth + xx];

                    for(size_t j = 0; j < m.size(); ++j)
                    {
                        const Point2f d = p - m[j];

                        if (d.ddot(d) < minDist2)
                        {
                            good = false;
                            goto break_out;
                        }
                    }
                }
            }

            break_out:

            if(good)
            {
                grid[yCell * gridWidth + xCell].push_back(p);

                posOutBuf_.push_back(oldPosBuf[ind]);
                if (hasVotes)
                    voteOutBuf_.push_back(oldVoteBuf[ind]);
            }
        }
    }

    void GeneralizedHoughBase::convertTo(OutputArray _positions, OutputArray _votes)
    {
        const int total = static_cast<int>(posOutBuf_.size());
        const bool hasVotes = !voteOutBuf_.empty();

        CV_Assert( !hasVotes || voteOutBuf_.size() == posOutBuf_.size() );

        _positions.create(1, total, CV_32FC4);
        Mat positions = _positions.getMat();
        Mat(1, total, CV_32FC4, &posOutBuf_[0]).copyTo(positions);

        if (_votes.needed())
        {
            if (!hasVotes)
            {
                _votes.release();
            }
            else
            {
                _votes.create(1, total, CV_32SC3);
                Mat votes = _votes.getMat();
                Mat(1, total, CV_32SC3, &voteOutBuf_[0]).copyTo(votes);
            }
        }
    }
}

// GeneralizedHoughBallard

namespace
{
    class GeneralizedHoughBallardImpl : public GeneralizedHoughBallard, private GeneralizedHoughBase
    {
    public:
        GeneralizedHoughBallardImpl();

        void setTemplate(InputArray templ, Point templCenter) { setTemplateImpl(templ, templCenter); }
        void setTemplate(InputArray edges, InputArray dx, InputArray dy, Point templCenter) { setTemplateImpl(edges, dx, dy, templCenter); }

        void detect(InputArray image, OutputArray positions, OutputArray votes) { detectImpl(image, positions, votes); }
        void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }

        void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }
        int getCannyLowThresh() const { return cannyLowThresh_; }

        void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }
        int getCannyHighThresh() const { return cannyHighThresh_; }

        void setMinDist(double minDist) { minDist_ = minDist; }
        double getMinDist() const { return minDist_; }

        void setDp(double dp) { dp_ = dp; }
        double getDp() const { return dp_; }

        void setMaxBufferSize(int) {  }
        int getMaxBufferSize() const { return 0; }

        void setLevels(int levels) { levels_ = levels; }
        int getLevels() const { return levels_; }

        void setVotesThreshold(int votesThreshold) { votesThreshold_ = votesThreshold; }
        int getVotesThreshold() const { return votesThreshold_; }

    private:
        void processTempl();
        void processImage();

        void calcHist();
        void findPosInHist();

        int levels_;
        int votesThreshold_;

        std::vector< std::vector<Point> > r_table_;
        Mat hist_;
    };

    GeneralizedHoughBallardImpl::GeneralizedHoughBallardImpl()
    {
        levels_ = 360;
        votesThreshold_ = 100;
    }

    void GeneralizedHoughBallardImpl::processTempl()
    {
        CV_Assert( levels_ > 0 );

        const double thetaScale = levels_ / 360.0;

        r_table_.resize(levels_ + 1);
        std::for_each(r_table_.begin(), r_table_.end(), std::mem_fun_ref(&std::vector<Point>::clear));

        for (int y = 0; y < templSize_.height; ++y)
        {
            const uchar* edgesRow = templEdges_.ptr(y);
            const float* dxRow = templDx_.ptr<float>(y);
            const float* dyRow = templDy_.ptr<float>(y);

            for (int x = 0; x < templSize_.width; ++x)
            {
                const Point p(x, y);

                if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
                {
                    const float theta = fastAtan2(dyRow[x], dxRow[x]);
                    const int n = cvRound(theta * thetaScale);
                    r_table_[n].push_back(p - templCenter_);
                }
            }
        }
    }

    void GeneralizedHoughBallardImpl::processImage()
    {
        calcHist();
        findPosInHist();
    }

    void GeneralizedHoughBallardImpl::calcHist()
    {
        CV_Assert( imageEdges_.type() == CV_8UC1 );
        CV_Assert( imageDx_.type() == CV_32FC1 && imageDx_.size() == imageSize_);
        CV_Assert( imageDy_.type() == imageDx_.type() && imageDy_.size() == imageSize_);
        CV_Assert( levels_ > 0 && r_table_.size() == static_cast<size_t>(levels_ + 1) );
        CV_Assert( dp_ > 0.0 );

        const double thetaScale = levels_ / 360.0;
        const double idp = 1.0 / dp_;

        hist_.create(cvCeil(imageSize_.height * idp) + 2, cvCeil(imageSize_.width * idp) + 2, CV_32SC1);
        hist_.setTo(0);

        const int rows = hist_.rows - 2;
        const int cols = hist_.cols - 2;

        for (int y = 0; y < imageSize_.height; ++y)
        {
            const uchar* edgesRow = imageEdges_.ptr(y);
            const float* dxRow = imageDx_.ptr<float>(y);
            const float* dyRow = imageDy_.ptr<float>(y);

            for (int x = 0; x < imageSize_.width; ++x)
            {
                const Point p(x, y);

                if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
                {
                    const float theta = fastAtan2(dyRow[x], dxRow[x]);
                    const int n = cvRound(theta * thetaScale);

                    const std::vector<Point>& r_row = r_table_[n];

                    for (size_t j = 0; j < r_row.size(); ++j)
                    {
                        Point c = p - r_row[j];

                        c.x = cvRound(c.x * idp);
                        c.y = cvRound(c.y * idp);

                        if (c.x >= 0 && c.x < cols && c.y >= 0 && c.y < rows)
                            ++hist_.at<int>(c.y + 1, c.x + 1);
                    }
                }
            }
        }
    }

    void GeneralizedHoughBallardImpl::findPosInHist()
    {
        CV_Assert( votesThreshold_ > 0 );

        const int histRows = hist_.rows - 2;
        const int histCols = hist_.cols - 2;

        for(int y = 0; y < histRows; ++y)
        {
            const int* prevRow = hist_.ptr<int>(y);
            const int* curRow = hist_.ptr<int>(y + 1);
            const int* nextRow = hist_.ptr<int>(y + 2);

            for(int x = 0; x < histCols; ++x)
            {
                const int votes = curRow[x + 1];

                if (votes > votesThreshold_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
                {
                    posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), 1.0f, 0.0f));
                    voteOutBuf_.push_back(Vec3i(votes, 0, 0));
                }
            }
        }
    }
}

Ptr<GeneralizedHoughBallard> cv::createGeneralizedHoughBallard()
{
    return makePtr<GeneralizedHoughBallardImpl>();
}

// GeneralizedHoughGuil

namespace
{
    class GeneralizedHoughGuilImpl : public GeneralizedHoughGuil, private GeneralizedHoughBase
    {
    public:
        GeneralizedHoughGuilImpl();

        void setTemplate(InputArray templ, Point templCenter) { setTemplateImpl(templ, templCenter); }
        void setTemplate(InputArray edges, InputArray dx, InputArray dy, Point templCenter) { setTemplateImpl(edges, dx, dy, templCenter); }

        void detect(InputArray image, OutputArray positions, OutputArray votes) { detectImpl(image, positions, votes); }
        void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }

        void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }
        int getCannyLowThresh() const { return cannyLowThresh_; }

        void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }
        int getCannyHighThresh() const { return cannyHighThresh_; }

        void setMinDist(double minDist) { minDist_ = minDist; }
        double getMinDist() const { return minDist_; }

        void setDp(double dp) { dp_ = dp; }
        double getDp() const { return dp_; }

        void setMaxBufferSize(int maxBufferSize) { maxBufferSize_ = maxBufferSize; }
        int getMaxBufferSize() const { return maxBufferSize_; }

        void setXi(double xi) { xi_ = xi; }
        double getXi() const { return xi_; }

        void setLevels(int levels) { levels_ = levels; }
        int getLevels() const { return levels_; }

        void setAngleEpsilon(double angleEpsilon) { angleEpsilon_ = angleEpsilon; }
        double getAngleEpsilon() const { return angleEpsilon_; }

        void setMinAngle(double minAngle) { minAngle_ = minAngle; }
        double getMinAngle() const { return minAngle_; }

        void setMaxAngle(double maxAngle) { maxAngle_ = maxAngle; }
        double getMaxAngle() const { return maxAngle_; }

        void setAngleStep(double angleStep) { angleStep_ = angleStep; }
        double getAngleStep() const { return angleStep_; }

        void setAngleThresh(int angleThresh) { angleThresh_ = angleThresh; }
        int getAngleThresh() const { return angleThresh_; }

        void setMinScale(double minScale) { minScale_ = minScale; }
        double getMinScale() const { return minScale_; }

        void setMaxScale(double maxScale) { maxScale_ = maxScale; }
        double getMaxScale() const { return maxScale_; }

        void setScaleStep(double scaleStep) { scaleStep_ = scaleStep; }
        double getScaleStep() const { return scaleStep_; }

        void setScaleThresh(int scaleThresh) { scaleThresh_ = scaleThresh; }
        int getScaleThresh() const { return scaleThresh_; }

        void setPosThresh(int posThresh) { posThresh_ = posThresh; }
        int getPosThresh() const { return posThresh_; }

    private:
        void processTempl();
        void processImage();

        int maxBufferSize_;
        double xi_;
        int levels_;
        double angleEpsilon_;

        double minAngle_;
        double maxAngle_;
        double angleStep_;
        int angleThresh_;

        double minScale_;
        double maxScale_;
        double scaleStep_;
        int scaleThresh_;

        int posThresh_;

        struct ContourPoint
        {
            Point2d pos;
            double theta;
        };

        struct Feature
        {
            ContourPoint p1;
            ContourPoint p2;

            double alpha12;
            double d12;

            Point2d r1;
            Point2d r2;
        };

        void buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center = Point2d());
        void getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points);

        void calcOrientation();
        void calcScale(double angle);
        void calcPosition(double angle, int angleVotes, double scale, int scaleVotes);

        std::vector< std::vector<Feature> > templFeatures_;
        std::vector< std::vector<Feature> > imageFeatures_;

        std::vector< std::pair<double, int> > angles_;
        std::vector< std::pair<double, int> > scales_;
    };

    double clampAngle(double a)
    {
        double res = a;

        while (res > 360.0)
            res -= 360.0;
        while (res < 0)
            res += 360.0;

        return res;
    }

    bool angleEq(double a, double b, double eps = 1.0)
    {
        return (fabs(clampAngle(a - b)) <= eps);
    }

    GeneralizedHoughGuilImpl::GeneralizedHoughGuilImpl()
    {
        maxBufferSize_ = 1000;
        xi_ = 90.0;
        levels_ = 360;
        angleEpsilon_ = 1.0;

        minAngle_ = 0.0;
        maxAngle_ = 360.0;
        angleStep_ = 1.0;
        angleThresh_ = 15000;

        minScale_ = 0.5;
        maxScale_ = 2.0;
        scaleStep_ = 0.05;
        scaleThresh_ = 1000;

        posThresh_ = 100;
    }

    void GeneralizedHoughGuilImpl::processTempl()
    {
        buildFeatureList(templEdges_, templDx_, templDy_, templFeatures_, templCenter_);
    }

    void GeneralizedHoughGuilImpl::processImage()
    {
        buildFeatureList(imageEdges_, imageDx_, imageDy_, imageFeatures_);

        calcOrientation();

        for (size_t i = 0; i < angles_.size(); ++i)
        {
            const double angle = angles_[i].first;
            const int angleVotes = angles_[i].second;

            calcScale(angle);

            for (size_t j = 0; j < scales_.size(); ++j)
            {
                const double scale = scales_[j].first;
                const int scaleVotes = scales_[j].second;

                calcPosition(angle, angleVotes, scale, scaleVotes);
            }
        }
    }

    void GeneralizedHoughGuilImpl::buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center)
    {
        CV_Assert( levels_ > 0 );

        const double maxDist = sqrt((double) templSize_.width * templSize_.width + templSize_.height * templSize_.height) * maxScale_;

        const double alphaScale = levels_ / 360.0;

        std::vector<ContourPoint> points;
        getContourPoints(edges, dx, dy, points);

        features.resize(levels_ + 1);
        std::for_each(features.begin(), features.end(), std::mem_fun_ref(&std::vector<Feature>::clear));
        std::for_each(features.begin(), features.end(), std::bind2nd(std::mem_fun_ref(&std::vector<Feature>::reserve), maxBufferSize_));

        for (size_t i = 0; i < points.size(); ++i)
        {
            ContourPoint p1 = points[i];

            for (size_t j = 0; j < points.size(); ++j)
            {
                ContourPoint p2 = points[j];

                if (angleEq(p1.theta - p2.theta, xi_, angleEpsilon_))
                {
                    const Point2d d = p1.pos - p2.pos;

                    Feature f;

                    f.p1 = p1;
                    f.p2 = p2;

                    f.alpha12 = clampAngle(fastAtan2((float)d.y, (float)d.x) - p1.theta);
                    f.d12 = norm(d);

                    if (f.d12 > maxDist)
                        continue;

                    f.r1 = p1.pos - center;
                    f.r2 = p2.pos - center;

                    const int n = cvRound(f.alpha12 * alphaScale);

                    if (features[n].size() < static_cast<size_t>(maxBufferSize_))
                        features[n].push_back(f);
                }
            }
        }
    }

    void GeneralizedHoughGuilImpl::getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points)
    {
        CV_Assert( edges.type() == CV_8UC1 );
        CV_Assert( dx.type() == CV_32FC1 && dx.size == edges.size );
        CV_Assert( dy.type() == dx.type() && dy.size == edges.size );

        points.clear();
        points.reserve(edges.size().area());

        for (int y = 0; y < edges.rows; ++y)
        {
            const uchar* edgesRow = edges.ptr(y);
            const float* dxRow = dx.ptr<float>(y);
            const float* dyRow = dy.ptr<float>(y);

            for (int x = 0; x < edges.cols; ++x)
            {
                if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
                {
                    ContourPoint p;

                    p.pos = Point2d(x, y);
                    p.theta = fastAtan2(dyRow[x], dxRow[x]);

                    points.push_back(p);
                }
            }
        }
    }

    void GeneralizedHoughGuilImpl::calcOrientation()
    {
        CV_Assert( levels_ > 0 );
        CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
        CV_Assert( imageFeatures_.size() == templFeatures_.size() );
        CV_Assert( minAngle_ >= 0.0 && minAngle_ < maxAngle_ && maxAngle_ <= 360.0 );
        CV_Assert( angleStep_ > 0.0 && angleStep_ < 360.0 );
        CV_Assert( angleThresh_ > 0 );

        const double iAngleStep = 1.0 / angleStep_;
        const int angleRange = cvCeil((maxAngle_ - minAngle_) * iAngleStep);

        std::vector<int> OHist(angleRange + 1, 0);
        for (int i = 0; i <= levels_; ++i)
        {
            const std::vector<Feature>& templRow = templFeatures_[i];
            const std::vector<Feature>& imageRow = imageFeatures_[i];

            for (size_t j = 0; j < templRow.size(); ++j)
            {
                Feature templF = templRow[j];

                for (size_t k = 0; k < imageRow.size(); ++k)
                {
                    Feature imF = imageRow[k];

                    const double angle = clampAngle(imF.p1.theta - templF.p1.theta);
                    if (angle >= minAngle_ && angle <= maxAngle_)
                    {
                        const int n = cvRound((angle - minAngle_) * iAngleStep);
                        ++OHist[n];
                    }
                }
            }
        }

        angles_.clear();

        for (int n = 0; n < angleRange; ++n)
        {
            if (OHist[n] >= angleThresh_)
            {
                const double angle = minAngle_ + n * angleStep_;
                angles_.push_back(std::make_pair(angle, OHist[n]));
            }
        }
    }

    void GeneralizedHoughGuilImpl::calcScale(double angle)
    {
        CV_Assert( levels_ > 0 );
        CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
        CV_Assert( imageFeatures_.size() == templFeatures_.size() );
        CV_Assert( minScale_ > 0.0 && minScale_ < maxScale_ );
        CV_Assert( scaleStep_ > 0.0 );
        CV_Assert( scaleThresh_ > 0 );

        const double iScaleStep = 1.0 / scaleStep_;
        const int scaleRange = cvCeil((maxScale_ - minScale_) * iScaleStep);

        std::vector<int> SHist(scaleRange + 1, 0);

        for (int i = 0; i <= levels_; ++i)
        {
            const std::vector<Feature>& templRow = templFeatures_[i];
            const std::vector<Feature>& imageRow = imageFeatures_[i];

            for (size_t j = 0; j < templRow.size(); ++j)
            {
                Feature templF = templRow[j];

                templF.p1.theta += angle;

                for (size_t k = 0; k < imageRow.size(); ++k)
                {
                    Feature imF = imageRow[k];

                    if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
                    {
                        const double scale = imF.d12 / templF.d12;
                        if (scale >= minScale_ && scale <= maxScale_)
                        {
                            const int s = cvRound((scale - minScale_) * iScaleStep);
                            ++SHist[s];
                        }
                    }
                }
            }
        }

        scales_.clear();

        for (int s = 0; s < scaleRange; ++s)
        {
            if (SHist[s] >= scaleThresh_)
            {
                const double scale = minScale_ + s * scaleStep_;
                scales_.push_back(std::make_pair(scale, SHist[s]));
            }
        }
    }

    void GeneralizedHoughGuilImpl::calcPosition(double angle, int angleVotes, double scale, int scaleVotes)
    {
        CV_Assert( levels_ > 0 );
        CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
        CV_Assert( imageFeatures_.size() == templFeatures_.size() );
        CV_Assert( dp_ > 0.0 );
        CV_Assert( posThresh_ > 0 );

        const double sinVal = sin(toRad(angle));
        const double cosVal = cos(toRad(angle));
        const double idp = 1.0 / dp_;

        const int histRows = cvCeil(imageSize_.height * idp);
        const int histCols = cvCeil(imageSize_.width * idp);

        Mat DHist(histRows + 2, histCols + 2, CV_32SC1, Scalar::all(0));

        for (int i = 0; i <= levels_; ++i)
        {
            const std::vector<Feature>& templRow = templFeatures_[i];
            const std::vector<Feature>& imageRow = imageFeatures_[i];

            for (size_t j = 0; j < templRow.size(); ++j)
            {
                Feature templF = templRow[j];

                templF.p1.theta += angle;

                templF.r1 *= scale;
                templF.r2 *= scale;

                templF.r1 = Point2d(cosVal * templF.r1.x - sinVal * templF.r1.y, sinVal * templF.r1.x + cosVal * templF.r1.y);
                templF.r2 = Point2d(cosVal * templF.r2.x - sinVal * templF.r2.y, sinVal * templF.r2.x + cosVal * templF.r2.y);

                for (size_t k = 0; k < imageRow.size(); ++k)
                {
                    Feature imF = imageRow[k];

                    if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
                    {
                        Point2d c1, c2;

                        c1 = imF.p1.pos - templF.r1;
                        c1 *= idp;

                        c2 = imF.p2.pos - templF.r2;
                        c2 *= idp;

                        if (fabs(c1.x - c2.x) > 1 || fabs(c1.y - c2.y) > 1)
                            continue;

                        if (c1.y >= 0 && c1.y < histRows && c1.x >= 0 && c1.x < histCols)
                            ++DHist.at<int>(cvRound(c1.y) + 1, cvRound(c1.x) + 1);
                    }
                }
            }
        }

        for(int y = 0; y < histRows; ++y)
        {
            const int* prevRow = DHist.ptr<int>(y);
            const int* curRow = DHist.ptr<int>(y + 1);
            const int* nextRow = DHist.ptr<int>(y + 2);

            for(int x = 0; x < histCols; ++x)
            {
                const int votes = curRow[x + 1];

                if (votes > posThresh_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
                {
                    posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), static_cast<float>(scale), static_cast<float>(angle)));
                    voteOutBuf_.push_back(Vec3i(votes, scaleVotes, angleVotes));
                }
            }
        }
    }
}

Ptr<GeneralizedHoughGuil> cv::createGeneralizedHoughGuil()
{
    return makePtr<GeneralizedHoughGuilImpl>();
}
/* [<][>][^][v][top][bottom][index][help] */