generalized_hough.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 
00042 #include "precomp.hpp"
00043 #include <functional>
00044 #include <limits>
00045 
00046 using namespace cv;
00047 
00048 // common
00049 
00050 namespace
00051 {
00052     double toRad(double a)
00053     {
00054         return a * CV_PI / 180.0;
00055     }
00056 
00057     bool notNull(float v)
00058     {
00059         return fabs(v) > std::numeric_limits<float>::epsilon();
00060     }
00061 
00062     class GeneralizedHoughBase
00063     {
00064     protected:
00065         GeneralizedHoughBase();
00066         virtual ~GeneralizedHoughBase() {}
00067 
00068         void setTemplateImpl(InputArray templ, Point  templCenter);
00069         void setTemplateImpl(InputArray edges, InputArray dx, InputArray dy, Point  templCenter);
00070 
00071         void detectImpl(InputArray image, OutputArray positions, OutputArray votes);
00072         void detectImpl(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes);
00073 
00074         virtual void processTempl() = 0;
00075         virtual void processImage() = 0;
00076 
00077         int cannyLowThresh_;
00078         int cannyHighThresh_;
00079         double minDist_;
00080         double dp_;
00081 
00082         Size templSize_;
00083         Point  templCenter_;
00084         Mat templEdges_;
00085         Mat templDx_;
00086         Mat templDy_;
00087 
00088         Size imageSize_;
00089         Mat imageEdges_;
00090         Mat imageDx_;
00091         Mat imageDy_;
00092 
00093         std::vector<Vec4f> posOutBuf_;
00094         std::vector<Vec3i> voteOutBuf_;
00095 
00096     private:
00097         void calcEdges(InputArray src, Mat& edges, Mat& dx, Mat& dy);
00098         void filterMinDist();
00099         void convertTo(OutputArray positions, OutputArray votes);
00100     };
00101 
00102     GeneralizedHoughBase::GeneralizedHoughBase()
00103     {
00104         cannyLowThresh_ = 50;
00105         cannyHighThresh_ = 100;
00106         minDist_ = 1.0;
00107         dp_ = 1.0;
00108     }
00109 
00110     void GeneralizedHoughBase::calcEdges(InputArray _src, Mat& edges, Mat& dx, Mat& dy)
00111     {
00112         Mat src = _src.getMat();
00113 
00114         CV_Assert( src.type() == CV_8UC1 );
00115         CV_Assert( cannyLowThresh_ > 0 && cannyLowThresh_ < cannyHighThresh_ );
00116 
00117         Canny(src, edges, cannyLowThresh_, cannyHighThresh_);
00118         Sobel(src, dx, CV_32F, 1, 0);
00119         Sobel(src, dy, CV_32F, 0, 1);
00120     }
00121 
00122     void GeneralizedHoughBase::setTemplateImpl(InputArray templ, Point  templCenter)
00123     {
00124         calcEdges(templ, templEdges_, templDx_, templDy_);
00125 
00126         if (templCenter == Point (-1, -1))
00127             templCenter = Point (templEdges_.cols / 2, templEdges_.rows / 2);
00128 
00129         templSize_ = templEdges_.size();
00130         templCenter_ = templCenter;
00131 
00132         processTempl();
00133     }
00134 
00135     void GeneralizedHoughBase::setTemplateImpl(InputArray edges, InputArray dx, InputArray dy, Point  templCenter)
00136     {
00137         edges.getMat().copyTo(templEdges_);
00138         dx.getMat().copyTo(templDx_);
00139         dy.getMat().copyTo(templDy_);
00140 
00141         CV_Assert( templEdges_.type() == CV_8UC1 );
00142         CV_Assert( templDx_.type() == CV_32FC1 && templDx_.size() == templEdges_.size() );
00143         CV_Assert( templDy_.type() == templDx_.type() && templDy_.size() == templEdges_.size() );
00144 
00145         if (templCenter == Point (-1, -1))
00146             templCenter = Point (templEdges_.cols / 2, templEdges_.rows / 2);
00147 
00148         templSize_ = templEdges_.size();
00149         templCenter_ = templCenter;
00150 
00151         processTempl();
00152     }
00153 
00154     void GeneralizedHoughBase::detectImpl(InputArray image, OutputArray positions, OutputArray votes)
00155     {
00156         calcEdges(image, imageEdges_, imageDx_, imageDy_);
00157 
00158         imageSize_ = imageEdges_.size();
00159 
00160         posOutBuf_.clear();
00161         voteOutBuf_.clear();
00162 
00163         processImage();
00164 
00165         if (!posOutBuf_.empty())
00166         {
00167             if (minDist_ > 1)
00168                 filterMinDist();
00169             convertTo(positions, votes);
00170         }
00171         else
00172         {
00173             positions.release();
00174             if (votes.needed())
00175                 votes.release();
00176         }
00177     }
00178 
00179     void GeneralizedHoughBase::detectImpl(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes)
00180     {
00181         edges.getMat().copyTo(imageEdges_);
00182         dx.getMat().copyTo(imageDx_);
00183         dy.getMat().copyTo(imageDy_);
00184 
00185         CV_Assert( imageEdges_.type() == CV_8UC1 );
00186         CV_Assert( imageDx_.type() == CV_32FC1 && imageDx_.size() == imageEdges_.size() );
00187         CV_Assert( imageDy_.type() == imageDx_.type() && imageDy_.size() == imageEdges_.size() );
00188 
00189         imageSize_ = imageEdges_.size();
00190 
00191         posOutBuf_.clear();
00192         voteOutBuf_.clear();
00193 
00194         processImage();
00195 
00196         if (!posOutBuf_.empty())
00197         {
00198             if (minDist_ > 1)
00199                 filterMinDist();
00200             convertTo(positions, votes);
00201         }
00202         else
00203         {
00204             positions.release();
00205             if (votes.needed())
00206                 votes.release();
00207         }
00208     }
00209 
00210     class Vec3iGreaterThanIdx
00211     {
00212     public:
00213         Vec3iGreaterThanIdx( const Vec3i* _arr ) : arr(_arr) {}
00214         bool operator()(size_t a, size_t b) const { return arr[a][0] > arr[b][0]; }
00215         const Vec3i* arr;
00216     };
00217 
00218     void GeneralizedHoughBase::filterMinDist()
00219     {
00220         size_t oldSize = posOutBuf_.size();
00221         const bool hasVotes = !voteOutBuf_.empty();
00222 
00223         CV_Assert( !hasVotes || voteOutBuf_.size() == oldSize );
00224 
00225         std::vector<Vec4f> oldPosBuf(posOutBuf_);
00226         std::vector<Vec3i> oldVoteBuf(voteOutBuf_);
00227 
00228         std::vector<size_t> indexies(oldSize);
00229         for (size_t i = 0; i < oldSize; ++i)
00230             indexies[i] = i;
00231         std::sort(indexies.begin(), indexies.end(), Vec3iGreaterThanIdx(&oldVoteBuf[0]));
00232 
00233         posOutBuf_.clear();
00234         voteOutBuf_.clear();
00235 
00236         const int cellSize = cvRound(minDist_);
00237         const int gridWidth = (imageSize_.width + cellSize - 1) / cellSize;
00238         const int gridHeight = (imageSize_.height + cellSize - 1) / cellSize;
00239 
00240         std::vector< std::vector<Point2f> > grid(gridWidth * gridHeight);
00241 
00242         const double minDist2 = minDist_ * minDist_;
00243 
00244         for (size_t i = 0; i < oldSize; ++i)
00245         {
00246             const size_t ind = indexies[i];
00247 
00248             Point2f  p(oldPosBuf[ind][0], oldPosBuf[ind][1]);
00249 
00250             bool good = true;
00251 
00252             const int xCell = static_cast<int>(p.x / cellSize);
00253             const int yCell = static_cast<int>(p.y / cellSize);
00254 
00255             int x1 = xCell - 1;
00256             int y1 = yCell - 1;
00257             int x2 = xCell + 1;
00258             int y2 = yCell + 1;
00259 
00260             // boundary check
00261             x1 = std::max(0, x1);
00262             y1 = std::max(0, y1);
00263             x2 = std::min(gridWidth - 1, x2);
00264             y2 = std::min(gridHeight - 1, y2);
00265 
00266             for (int yy = y1; yy <= y2; ++yy)
00267             {
00268                 for (int xx = x1; xx <= x2; ++xx)
00269                 {
00270                     const std::vector<Point2f>& m = grid[yy * gridWidth + xx];
00271 
00272                     for(size_t j = 0; j < m.size(); ++j)
00273                     {
00274                         const Point2f  d = p - m[j];
00275 
00276                         if (d.ddot(d) < minDist2)
00277                         {
00278                             good = false;
00279                             goto break_out;
00280                         }
00281                     }
00282                 }
00283             }
00284 
00285             break_out:
00286 
00287             if(good)
00288             {
00289                 grid[yCell * gridWidth + xCell].push_back(p);
00290 
00291                 posOutBuf_.push_back(oldPosBuf[ind]);
00292                 if (hasVotes)
00293                     voteOutBuf_.push_back(oldVoteBuf[ind]);
00294             }
00295         }
00296     }
00297 
00298     void GeneralizedHoughBase::convertTo(OutputArray _positions, OutputArray _votes)
00299     {
00300         const int total = static_cast<int>(posOutBuf_.size());
00301         const bool hasVotes = !voteOutBuf_.empty();
00302 
00303         CV_Assert( !hasVotes || voteOutBuf_.size() == posOutBuf_.size() );
00304 
00305         _positions.create(1, total, CV_32FC4);
00306         Mat positions = _positions.getMat();
00307         Mat(1, total, CV_32FC4, &posOutBuf_[0]).copyTo(positions);
00308 
00309         if (_votes.needed())
00310         {
00311             if (!hasVotes)
00312             {
00313                 _votes.release();
00314             }
00315             else
00316             {
00317                 _votes.create(1, total, CV_32SC3);
00318                 Mat votes = _votes.getMat();
00319                 Mat(1, total, CV_32SC3, &voteOutBuf_[0]).copyTo(votes);
00320             }
00321         }
00322     }
00323 }
00324 
00325 // GeneralizedHoughBallard
00326 
00327 namespace
00328 {
00329     class GeneralizedHoughBallardImpl : public GeneralizedHoughBallard, private GeneralizedHoughBase
00330     {
00331     public:
00332         GeneralizedHoughBallardImpl();
00333 
00334         void setTemplate(InputArray templ, Point  templCenter) { setTemplateImpl(templ, templCenter); }
00335         void setTemplate(InputArray edges, InputArray dx, InputArray dy, Point  templCenter) { setTemplateImpl(edges, dx, dy, templCenter); }
00336 
00337         void detect(InputArray image, OutputArray positions, OutputArray votes) { detectImpl(image, positions, votes); }
00338         void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }
00339 
00340         void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }
00341         int getCannyLowThresh() const { return cannyLowThresh_; }
00342 
00343         void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }
00344         int getCannyHighThresh() const { return cannyHighThresh_; }
00345 
00346         void setMinDist(double minDist) { minDist_ = minDist; }
00347         double getMinDist() const { return minDist_; }
00348 
00349         void setDp(double dp) { dp_ = dp; }
00350         double getDp() const { return dp_; }
00351 
00352         void setMaxBufferSize(int) {  }
00353         int getMaxBufferSize() const { return 0; }
00354 
00355         void setLevels(int levels) { levels_ = levels; }
00356         int getLevels() const { return levels_; }
00357 
00358         void setVotesThreshold(int votesThreshold) { votesThreshold_ = votesThreshold; }
00359         int getVotesThreshold() const { return votesThreshold_; }
00360 
00361     private:
00362         void processTempl();
00363         void processImage();
00364 
00365         void calcHist();
00366         void findPosInHist();
00367 
00368         int levels_;
00369         int votesThreshold_;
00370 
00371         std::vector< std::vector<Point> > r_table_;
00372         Mat hist_;
00373     };
00374 
00375     GeneralizedHoughBallardImpl::GeneralizedHoughBallardImpl()
00376     {
00377         levels_ = 360;
00378         votesThreshold_ = 100;
00379     }
00380 
00381     void GeneralizedHoughBallardImpl::processTempl()
00382     {
00383         CV_Assert( levels_ > 0 );
00384 
00385         const double thetaScale = levels_ / 360.0;
00386 
00387         r_table_.resize(levels_ + 1);
00388         std::for_each(r_table_.begin(), r_table_.end(), std::mem_fun_ref(&std::vector<Point>::clear));
00389 
00390         for (int y = 0; y < templSize_.height; ++y)
00391         {
00392             const uchar* edgesRow = templEdges_.ptr(y);
00393             const float* dxRow = templDx_.ptr<float>(y);
00394             const float* dyRow = templDy_.ptr<float>(y);
00395 
00396             for (int x = 0; x < templSize_.width; ++x)
00397             {
00398                 const Point  p(x, y);
00399 
00400                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
00401                 {
00402                     const float theta = fastAtan2(dyRow[x], dxRow[x]);
00403                     const int n = cvRound(theta * thetaScale);
00404                     r_table_[n].push_back(p - templCenter_);
00405                 }
00406             }
00407         }
00408     }
00409 
00410     void GeneralizedHoughBallardImpl::processImage()
00411     {
00412         calcHist();
00413         findPosInHist();
00414     }
00415 
00416     void GeneralizedHoughBallardImpl::calcHist()
00417     {
00418         CV_Assert( imageEdges_.type() == CV_8UC1 );
00419         CV_Assert( imageDx_.type() == CV_32FC1 && imageDx_.size() == imageSize_);
00420         CV_Assert( imageDy_.type() == imageDx_.type() && imageDy_.size() == imageSize_);
00421         CV_Assert( levels_ > 0 && r_table_.size() == static_cast<size_t>(levels_ + 1) );
00422         CV_Assert( dp_ > 0.0 );
00423 
00424         const double thetaScale = levels_ / 360.0;
00425         const double idp = 1.0 / dp_;
00426 
00427         hist_.create(cvCeil(imageSize_.height * idp) + 2, cvCeil(imageSize_.width * idp) + 2, CV_32SC1);
00428         hist_.setTo(0);
00429 
00430         const int rows = hist_.rows - 2;
00431         const int cols = hist_.cols - 2;
00432 
00433         for (int y = 0; y < imageSize_.height; ++y)
00434         {
00435             const uchar* edgesRow = imageEdges_.ptr(y);
00436             const float* dxRow = imageDx_.ptr<float>(y);
00437             const float* dyRow = imageDy_.ptr<float>(y);
00438 
00439             for (int x = 0; x < imageSize_.width; ++x)
00440             {
00441                 const Point  p(x, y);
00442 
00443                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
00444                 {
00445                     const float theta = fastAtan2(dyRow[x], dxRow[x]);
00446                     const int n = cvRound(theta * thetaScale);
00447 
00448                     const std::vector<Point>& r_row = r_table_[n];
00449 
00450                     for (size_t j = 0; j < r_row.size(); ++j)
00451                     {
00452                         Point  c = p - r_row[j];
00453 
00454                         c.x = cvRound(c.x * idp);
00455                         c.y = cvRound(c.y * idp);
00456 
00457                         if (c.x >= 0 && c.x < cols && c.y >= 0 && c.y < rows)
00458                             ++hist_.at<int>(c.y + 1, c.x + 1);
00459                     }
00460                 }
00461             }
00462         }
00463     }
00464 
00465     void GeneralizedHoughBallardImpl::findPosInHist()
00466     {
00467         CV_Assert( votesThreshold_ > 0 );
00468 
00469         const int histRows = hist_.rows - 2;
00470         const int histCols = hist_.cols - 2;
00471 
00472         for(int y = 0; y < histRows; ++y)
00473         {
00474             const int* prevRow = hist_.ptr<int>(y);
00475             const int* curRow = hist_.ptr<int>(y + 1);
00476             const int* nextRow = hist_.ptr<int>(y + 2);
00477 
00478             for(int x = 0; x < histCols; ++x)
00479             {
00480                 const int votes = curRow[x + 1];
00481 
00482                 if (votes > votesThreshold_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
00483                 {
00484                     posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), 1.0f, 0.0f));
00485                     voteOutBuf_.push_back(Vec3i(votes, 0, 0));
00486                 }
00487             }
00488         }
00489     }
00490 }
00491 
00492 Ptr<GeneralizedHoughBallard> cv::createGeneralizedHoughBallard()
00493 {
00494     return makePtr<GeneralizedHoughBallardImpl>();
00495 }
00496 
00497 // GeneralizedHoughGuil
00498 
00499 namespace
00500 {
00501     class GeneralizedHoughGuilImpl : public GeneralizedHoughGuil, private GeneralizedHoughBase
00502     {
00503     public:
00504         GeneralizedHoughGuilImpl();
00505 
00506         void setTemplate(InputArray templ, Point  templCenter) { setTemplateImpl(templ, templCenter); }
00507         void setTemplate(InputArray edges, InputArray dx, InputArray dy, Point  templCenter) { setTemplateImpl(edges, dx, dy, templCenter); }
00508 
00509         void detect(InputArray image, OutputArray positions, OutputArray votes) { detectImpl(image, positions, votes); }
00510         void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }
00511 
00512         void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }
00513         int getCannyLowThresh() const { return cannyLowThresh_; }
00514 
00515         void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }
00516         int getCannyHighThresh() const { return cannyHighThresh_; }
00517 
00518         void setMinDist(double minDist) { minDist_ = minDist; }
00519         double getMinDist() const { return minDist_; }
00520 
00521         void setDp(double dp) { dp_ = dp; }
00522         double getDp() const { return dp_; }
00523 
00524         void setMaxBufferSize(int maxBufferSize) { maxBufferSize_ = maxBufferSize; }
00525         int getMaxBufferSize() const { return maxBufferSize_; }
00526 
00527         void setXi(double xi) { xi_ = xi; }
00528         double getXi() const { return xi_; }
00529 
00530         void setLevels(int levels) { levels_ = levels; }
00531         int getLevels() const { return levels_; }
00532 
00533         void setAngleEpsilon(double angleEpsilon) { angleEpsilon_ = angleEpsilon; }
00534         double getAngleEpsilon() const { return angleEpsilon_; }
00535 
00536         void setMinAngle(double minAngle) { minAngle_ = minAngle; }
00537         double getMinAngle() const { return minAngle_; }
00538 
00539         void setMaxAngle(double maxAngle) { maxAngle_ = maxAngle; }
00540         double getMaxAngle() const { return maxAngle_; }
00541 
00542         void setAngleStep(double angleStep) { angleStep_ = angleStep; }
00543         double getAngleStep() const { return angleStep_; }
00544 
00545         void setAngleThresh(int angleThresh) { angleThresh_ = angleThresh; }
00546         int getAngleThresh() const { return angleThresh_; }
00547 
00548         void setMinScale(double minScale) { minScale_ = minScale; }
00549         double getMinScale() const { return minScale_; }
00550 
00551         void setMaxScale(double maxScale) { maxScale_ = maxScale; }
00552         double getMaxScale() const { return maxScale_; }
00553 
00554         void setScaleStep(double scaleStep) { scaleStep_ = scaleStep; }
00555         double getScaleStep() const { return scaleStep_; }
00556 
00557         void setScaleThresh(int scaleThresh) { scaleThresh_ = scaleThresh; }
00558         int getScaleThresh() const { return scaleThresh_; }
00559 
00560         void setPosThresh(int posThresh) { posThresh_ = posThresh; }
00561         int getPosThresh() const { return posThresh_; }
00562 
00563     private:
00564         void processTempl();
00565         void processImage();
00566 
00567         int maxBufferSize_;
00568         double xi_;
00569         int levels_;
00570         double angleEpsilon_;
00571 
00572         double minAngle_;
00573         double maxAngle_;
00574         double angleStep_;
00575         int angleThresh_;
00576 
00577         double minScale_;
00578         double maxScale_;
00579         double scaleStep_;
00580         int scaleThresh_;
00581 
00582         int posThresh_;
00583 
00584         struct ContourPoint
00585         {
00586             Point2d pos;
00587             double theta;
00588         };
00589 
00590         struct Feature
00591         {
00592             ContourPoint p1;
00593             ContourPoint p2;
00594 
00595             double alpha12;
00596             double d12;
00597 
00598             Point2d r1;
00599             Point2d r2;
00600         };
00601 
00602         void buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center = Point2d());
00603         void getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points);
00604 
00605         void calcOrientation();
00606         void calcScale(double angle);
00607         void calcPosition(double angle, int angleVotes, double scale, int scaleVotes);
00608 
00609         std::vector< std::vector<Feature> > templFeatures_;
00610         std::vector< std::vector<Feature> > imageFeatures_;
00611 
00612         std::vector< std::pair<double, int> > angles_;
00613         std::vector< std::pair<double, int> > scales_;
00614     };
00615 
00616     double clampAngle(double a)
00617     {
00618         double res = a;
00619 
00620         while (res > 360.0)
00621             res -= 360.0;
00622         while (res < 0)
00623             res += 360.0;
00624 
00625         return res;
00626     }
00627 
00628     bool angleEq(double a, double b, double eps = 1.0)
00629     {
00630         return (fabs(clampAngle(a - b)) <= eps);
00631     }
00632 
00633     GeneralizedHoughGuilImpl::GeneralizedHoughGuilImpl()
00634     {
00635         maxBufferSize_ = 1000;
00636         xi_ = 90.0;
00637         levels_ = 360;
00638         angleEpsilon_ = 1.0;
00639 
00640         minAngle_ = 0.0;
00641         maxAngle_ = 360.0;
00642         angleStep_ = 1.0;
00643         angleThresh_ = 15000;
00644 
00645         minScale_ = 0.5;
00646         maxScale_ = 2.0;
00647         scaleStep_ = 0.05;
00648         scaleThresh_ = 1000;
00649 
00650         posThresh_ = 100;
00651     }
00652 
00653     void GeneralizedHoughGuilImpl::processTempl()
00654     {
00655         buildFeatureList(templEdges_, templDx_, templDy_, templFeatures_, templCenter_);
00656     }
00657 
00658     void GeneralizedHoughGuilImpl::processImage()
00659     {
00660         buildFeatureList(imageEdges_, imageDx_, imageDy_, imageFeatures_);
00661 
00662         calcOrientation();
00663 
00664         for (size_t i = 0; i < angles_.size(); ++i)
00665         {
00666             const double angle = angles_[i].first;
00667             const int angleVotes = angles_[i].second;
00668 
00669             calcScale(angle);
00670 
00671             for (size_t j = 0; j < scales_.size(); ++j)
00672             {
00673                 const double scale = scales_[j].first;
00674                 const int scaleVotes = scales_[j].second;
00675 
00676                 calcPosition(angle, angleVotes, scale, scaleVotes);
00677             }
00678         }
00679     }
00680 
00681     void GeneralizedHoughGuilImpl::buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center)
00682     {
00683         CV_Assert( levels_ > 0 );
00684 
00685         const double maxDist = sqrt((double) templSize_.width * templSize_.width + templSize_.height * templSize_.height) * maxScale_;
00686 
00687         const double alphaScale = levels_ / 360.0;
00688 
00689         std::vector<ContourPoint> points;
00690         getContourPoints(edges, dx, dy, points);
00691 
00692         features.resize(levels_ + 1);
00693         std::for_each(features.begin(), features.end(), std::mem_fun_ref(&std::vector<Feature>::clear));
00694         std::for_each(features.begin(), features.end(), std::bind2nd(std::mem_fun_ref(&std::vector<Feature>::reserve), maxBufferSize_));
00695 
00696         for (size_t i = 0; i < points.size(); ++i)
00697         {
00698             ContourPoint p1 = points[i];
00699 
00700             for (size_t j = 0; j < points.size(); ++j)
00701             {
00702                 ContourPoint p2 = points[j];
00703 
00704                 if (angleEq(p1.theta - p2.theta, xi_, angleEpsilon_))
00705                 {
00706                     const Point2d d = p1.pos - p2.pos;
00707 
00708                     Feature f;
00709 
00710                     f.p1 = p1;
00711                     f.p2 = p2;
00712 
00713                     f.alpha12 = clampAngle(fastAtan2((float)d.y, (float)d.x) - p1.theta);
00714                     f.d12 = norm(d);
00715 
00716                     if (f.d12 > maxDist)
00717                         continue;
00718 
00719                     f.r1 = p1.pos - center;
00720                     f.r2 = p2.pos - center;
00721 
00722                     const int n = cvRound(f.alpha12 * alphaScale);
00723 
00724                     if (features[n].size() < static_cast<size_t>(maxBufferSize_))
00725                         features[n].push_back(f);
00726                 }
00727             }
00728         }
00729     }
00730 
00731     void GeneralizedHoughGuilImpl::getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points)
00732     {
00733         CV_Assert( edges.type() == CV_8UC1 );
00734         CV_Assert( dx.type() == CV_32FC1 && dx.size == edges.size );
00735         CV_Assert( dy.type() == dx.type() && dy.size == edges.size );
00736 
00737         points.clear();
00738         points.reserve(edges.size().area());
00739 
00740         for (int y = 0; y < edges.rows; ++y)
00741         {
00742             const uchar* edgesRow = edges.ptr(y);
00743             const float* dxRow = dx.ptr<float>(y);
00744             const float* dyRow = dy.ptr<float>(y);
00745 
00746             for (int x = 0; x < edges.cols; ++x)
00747             {
00748                 if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
00749                 {
00750                     ContourPoint p;
00751 
00752                     p.pos = Point2d(x, y);
00753                     p.theta = fastAtan2(dyRow[x], dxRow[x]);
00754 
00755                     points.push_back(p);
00756                 }
00757             }
00758         }
00759     }
00760 
00761     void GeneralizedHoughGuilImpl::calcOrientation()
00762     {
00763         CV_Assert( levels_ > 0 );
00764         CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
00765         CV_Assert( imageFeatures_.size() == templFeatures_.size() );
00766         CV_Assert( minAngle_ >= 0.0 && minAngle_ < maxAngle_ && maxAngle_ <= 360.0 );
00767         CV_Assert( angleStep_ > 0.0 && angleStep_ < 360.0 );
00768         CV_Assert( angleThresh_ > 0 );
00769 
00770         const double iAngleStep = 1.0 / angleStep_;
00771         const int angleRange = cvCeil((maxAngle_ - minAngle_) * iAngleStep);
00772 
00773         std::vector<int> OHist(angleRange + 1, 0);
00774         for (int i = 0; i <= levels_; ++i)
00775         {
00776             const std::vector<Feature>& templRow = templFeatures_[i];
00777             const std::vector<Feature>& imageRow = imageFeatures_[i];
00778 
00779             for (size_t j = 0; j < templRow.size(); ++j)
00780             {
00781                 Feature templF = templRow[j];
00782 
00783                 for (size_t k = 0; k < imageRow.size(); ++k)
00784                 {
00785                     Feature imF = imageRow[k];
00786 
00787                     const double angle = clampAngle(imF.p1.theta - templF.p1.theta);
00788                     if (angle >= minAngle_ && angle <= maxAngle_)
00789                     {
00790                         const int n = cvRound((angle - minAngle_) * iAngleStep);
00791                         ++OHist[n];
00792                     }
00793                 }
00794             }
00795         }
00796 
00797         angles_.clear();
00798 
00799         for (int n = 0; n < angleRange; ++n)
00800         {
00801             if (OHist[n] >= angleThresh_)
00802             {
00803                 const double angle = minAngle_ + n * angleStep_;
00804                 angles_.push_back(std::make_pair(angle, OHist[n]));
00805             }
00806         }
00807     }
00808 
00809     void GeneralizedHoughGuilImpl::calcScale(double angle)
00810     {
00811         CV_Assert( levels_ > 0 );
00812         CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
00813         CV_Assert( imageFeatures_.size() == templFeatures_.size() );
00814         CV_Assert( minScale_ > 0.0 && minScale_ < maxScale_ );
00815         CV_Assert( scaleStep_ > 0.0 );
00816         CV_Assert( scaleThresh_ > 0 );
00817 
00818         const double iScaleStep = 1.0 / scaleStep_;
00819         const int scaleRange = cvCeil((maxScale_ - minScale_) * iScaleStep);
00820 
00821         std::vector<int> SHist(scaleRange + 1, 0);
00822 
00823         for (int i = 0; i <= levels_; ++i)
00824         {
00825             const std::vector<Feature>& templRow = templFeatures_[i];
00826             const std::vector<Feature>& imageRow = imageFeatures_[i];
00827 
00828             for (size_t j = 0; j < templRow.size(); ++j)
00829             {
00830                 Feature templF = templRow[j];
00831 
00832                 templF.p1.theta += angle;
00833 
00834                 for (size_t k = 0; k < imageRow.size(); ++k)
00835                 {
00836                     Feature imF = imageRow[k];
00837 
00838                     if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
00839                     {
00840                         const double scale = imF.d12 / templF.d12;
00841                         if (scale >= minScale_ && scale <= maxScale_)
00842                         {
00843                             const int s = cvRound((scale - minScale_) * iScaleStep);
00844                             ++SHist[s];
00845                         }
00846                     }
00847                 }
00848             }
00849         }
00850 
00851         scales_.clear();
00852 
00853         for (int s = 0; s < scaleRange; ++s)
00854         {
00855             if (SHist[s] >= scaleThresh_)
00856             {
00857                 const double scale = minScale_ + s * scaleStep_;
00858                 scales_.push_back(std::make_pair(scale, SHist[s]));
00859             }
00860         }
00861     }
00862 
00863     void GeneralizedHoughGuilImpl::calcPosition(double angle, int angleVotes, double scale, int scaleVotes)
00864     {
00865         CV_Assert( levels_ > 0 );
00866         CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );
00867         CV_Assert( imageFeatures_.size() == templFeatures_.size() );
00868         CV_Assert( dp_ > 0.0 );
00869         CV_Assert( posThresh_ > 0 );
00870 
00871         const double sinVal = sin(toRad(angle));
00872         const double cosVal = cos(toRad(angle));
00873         const double idp = 1.0 / dp_;
00874 
00875         const int histRows = cvCeil(imageSize_.height * idp);
00876         const int histCols = cvCeil(imageSize_.width * idp);
00877 
00878         Mat DHist(histRows + 2, histCols + 2, CV_32SC1, Scalar::all(0));
00879 
00880         for (int i = 0; i <= levels_; ++i)
00881         {
00882             const std::vector<Feature>& templRow = templFeatures_[i];
00883             const std::vector<Feature>& imageRow = imageFeatures_[i];
00884 
00885             for (size_t j = 0; j < templRow.size(); ++j)
00886             {
00887                 Feature templF = templRow[j];
00888 
00889                 templF.p1.theta += angle;
00890 
00891                 templF.r1 *= scale;
00892                 templF.r2 *= scale;
00893 
00894                 templF.r1 = Point2d(cosVal * templF.r1.x - sinVal * templF.r1.y, sinVal * templF.r1.x + cosVal * templF.r1.y);
00895                 templF.r2 = Point2d(cosVal * templF.r2.x - sinVal * templF.r2.y, sinVal * templF.r2.x + cosVal * templF.r2.y);
00896 
00897                 for (size_t k = 0; k < imageRow.size(); ++k)
00898                 {
00899                     Feature imF = imageRow[k];
00900 
00901                     if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
00902                     {
00903                         Point2d c1, c2;
00904 
00905                         c1 = imF.p1.pos - templF.r1;
00906                         c1 *= idp;
00907 
00908                         c2 = imF.p2.pos - templF.r2;
00909                         c2 *= idp;
00910 
00911                         if (fabs(c1.x - c2.x) > 1 || fabs(c1.y - c2.y) > 1)
00912                             continue;
00913 
00914                         if (c1.y >= 0 && c1.y < histRows && c1.x >= 0 && c1.x < histCols)
00915                             ++DHist.at<int>(cvRound(c1.y) + 1, cvRound(c1.x) + 1);
00916                     }
00917                 }
00918             }
00919         }
00920 
00921         for(int y = 0; y < histRows; ++y)
00922         {
00923             const int* prevRow = DHist.ptr<int>(y);
00924             const int* curRow = DHist.ptr<int>(y + 1);
00925             const int* nextRow = DHist.ptr<int>(y + 2);
00926 
00927             for(int x = 0; x < histCols; ++x)
00928             {
00929                 const int votes = curRow[x + 1];
00930 
00931                 if (votes > posThresh_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
00932                 {
00933                     posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), static_cast<float>(scale), static_cast<float>(angle)));
00934                     voteOutBuf_.push_back(Vec3i(votes, scaleVotes, angleVotes));
00935                 }
00936             }
00937         }
00938     }
00939 }
00940 
00941 Ptr<GeneralizedHoughGuil> cv::createGeneralizedHoughGuil()
00942 {
00943     return makePtr<GeneralizedHoughGuilImpl>();
00944 }
00945