Added detection of cirlces' grid pattern

modules/calib3d/include/opencv2/calib3d/calib3d.hpp

@@ -542,7 +542,12 @@ CV_EXPORTS_W void drawChessboardCorners( Mat& image, Size patternSize,
 CV_EXPORTS void drawChessboardCorners( Mat& image, Size patternSize,
                                        const vector<Point2f>& corners,
                                        bool patternWasFound );    
-    
+
+//! finds circles' grid pattern of the specified size in the image
+CV_EXPORTS_W bool findCirclesGrid( const Mat& image, Size patternSize,
+                                   CV_OUT vector<Point2f>& centers,
+                                   int flags=0 );
+
 enum
 {
     CALIB_USE_INTRINSIC_GUESS = CV_CALIB_USE_INTRINSIC_GUESS,

modules/calib3d/src/blobdetector.cpp

@@ -0,0 +1,288 @@
+/*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.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., 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 the copyright holders 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 "blobdetector.hpp"
+
+using namespace cv;
+
+BlobDetectorParameters::BlobDetectorParameters()
+{
+  thresholdStep = 10;
+  minThreshold = 50;
+  maxThreshold = 220;
+  maxCentersDist = 10;
+  defaultKeypointSize = 1;
+  minRepeatability = 2;
+  filterByColor = true;
+  computeRadius = true;
+
+  isGrayscaleCentroid = false;
+  centroidROIMargin = 2;
+
+  filterByArea = true;
+  minArea = 25;
+  maxArea = 5000;
+
+  filterByInertia = true;
+  //minInertiaRatio = 0.6;
+  minInertiaRatio = 0.1;
+
+  filterByConvexity = true;
+  //minConvexity = 0.8;
+  minConvexity = 0.95;
+
+  filterByCircularity = false;
+  minCircularity = 0.8;
+}
+
+BlobDetector::BlobDetector(const BlobDetectorParameters &parameters) :
+  params(parameters)
+{
+}
+
+void BlobDetector::detect(const cv::Mat& image, vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const
+{
+  detectImpl(image, keypoints, mask);
+}
+
+Point2d BlobDetector::computeGrayscaleCentroid(const Mat &image, const vector<Point> &contour) const
+{
+  Rect rect = boundingRect(Mat(contour));
+  rect.x -= params.centroidROIMargin;
+  rect.y -= params.centroidROIMargin;
+  rect.width += 2 * params.centroidROIMargin;
+  rect.height += 2 * params.centroidROIMargin;
+
+  rect.x = rect.x < 0 ? 0 : rect.x;
+  rect.y = rect.y < 0 ? 0 : rect.y;
+  rect.width = rect.x + rect.width < image.cols ? rect.width : image.cols - rect.x;
+  rect.height = rect.y + rect.height < image.rows ? rect.height : image.rows - rect.y;
+
+  Mat roi = image(rect);
+  assert( roi.type() == CV_8UC1 );
+
+  Mat invRoi = 255 - roi;
+  invRoi.convertTo(invRoi, CV_32FC1);
+  invRoi = invRoi.mul(invRoi);
+
+  Moments moms = moments(invRoi);
+
+  Point2d tl = rect.tl();
+  Point2d roiCentroid(moms.m10 / moms.m00, moms.m01 / moms.m00);
+
+  Point2d centroid = tl + roiCentroid;
+  return centroid;
+}
+
+void BlobDetector::findBlobs(const cv::Mat &image, const cv::Mat &binaryImage, vector<Center> &centers) const
+{
+  centers.clear();
+
+  vector<vector<Point> > contours;
+  Mat tmpBinaryImage = binaryImage.clone();
+  findContours(tmpBinaryImage, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+
+  //Mat keypointsImage;
+  //cvtColor( binaryImage, keypointsImage, CV_GRAY2RGB );
+
+  //Mat contoursImage;
+  //cvtColor( binaryImage, contoursImage, CV_GRAY2RGB );
+  //drawContours( contoursImage, contours, -1, Scalar(0,255,0) );
+  //imshow("contours", contoursImage );
+
+  for (size_t contourIdx = 0; contourIdx < contours.size(); contourIdx++)
+  {
+    Center center;
+    center.confidence = 1;
+    Moments moms = moments(Mat(contours[contourIdx]));
+    if (params.filterByArea)
+    {
+      double area = moms.m00;
+      if (area < params.minArea || area > params.maxArea)
+        continue;
+    }
+
+    if (params.filterByCircularity)
+    {
+      double area = moms.m00;
+      double perimeter = arcLength(Mat(contours[contourIdx]), true);
+      double ratio = 4 * M_PI * area / (perimeter * perimeter);
+      if (ratio < params.minCircularity)
+        continue;
+    }
+
+    if (params.filterByInertia)
+    {
+      double denominator = sqrt(pow(2 * moms.mu11, 2) + pow(moms.mu20 - moms.mu02, 2));
+      const double eps = 1e-2;
+      double ratio;
+      if (denominator > eps)
+      {
+        double cosmin = (moms.mu20 - moms.mu02) / denominator;
+        double sinmin = 2 * moms.mu11 / denominator;
+        double cosmax = -cosmin;
+        double sinmax = -sinmin;
+
+        double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
+        double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
+        ratio = imin / imax;
+      }
+      else
+      {
+        ratio = 1;
+      }
+
+      if (ratio < params.minInertiaRatio)
+        continue;
+
+      center.confidence = ratio * ratio;
+    }
+
+    if (params.filterByConvexity)
+    {
+      vector<Point> hull;
+      convexHull(Mat(contours[contourIdx]), hull);
+      double area = contourArea(Mat(contours[contourIdx]));
+      double hullArea = contourArea(Mat(hull));
+      double ratio = area / hullArea;
+      if (ratio < params.minConvexity)
+        continue;
+    }
+
+    if (params.isGrayscaleCentroid)
+      center.location = computeGrayscaleCentroid(image, contours[contourIdx]);
+    else
+      center.location = Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
+
+    if (params.filterByColor)
+    {
+      if (binaryImage.at<uchar> (center.location.y, center.location.x) == 255)
+        continue;
+    }
+
+    if (params.computeRadius)
+    {
+      vector<double> dists;
+      for (size_t pointIdx = 0; pointIdx < contours[contourIdx].size(); pointIdx++)
+      {
+        Point2d pt = contours[contourIdx][pointIdx];
+        dists.push_back(norm(center.location - pt));
+      }
+      std::sort(dists.begin(), dists.end());
+      center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
+    }
+
+    centers.push_back(center);
+
+    //circle( keypointsImage, center.location, 1, Scalar(0,0,255), 1 );
+  }
+  //imshow("bk", keypointsImage );
+  //waitKey();
+}
+
+void BlobDetector::detectImpl(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const
+{
+  keypoints.clear();
+  Mat grayscaleImage;
+  if (image.channels() == 3)
+    cvtColor(image, grayscaleImage, CV_BGR2GRAY);
+  else
+    grayscaleImage = image;
+
+  vector<vector<Center> > centers;
+  for (double thresh = params.minThreshold; thresh < params.maxThreshold; thresh += params.thresholdStep)
+  {
+    Mat binarizedImage;
+    threshold(grayscaleImage, binarizedImage, thresh, 255, THRESH_BINARY);
+
+    //Mat keypointsImage;
+    //cvtColor( binarizedImage, keypointsImage, CV_GRAY2RGB );
+
+    vector<Center> curCenters;
+    findBlobs(grayscaleImage, binarizedImage, curCenters);
+    for (size_t i = 0; i < curCenters.size(); i++)
+    {
+      //circle(keypointsImage, curCenters[i].location, 1, Scalar(0,0,255),-1);
+
+      bool isNew = true;
+      for (size_t j = 0; j < centers.size(); j++)
+      {
+        double dist = norm(centers[j][0].location - curCenters[i].location);
+        if (params.computeRadius)
+          isNew = dist >= centers[j][0].radius && dist >= curCenters[i].radius && dist >= params.maxCentersDist;
+        else
+          isNew = dist >= params.maxCentersDist;
+        if (!isNew)
+        {
+          centers[j].push_back(curCenters[i]);
+          //          if( centers[j][0].radius < centers[j][ centers[j].size()-1 ].radius )
+          //          {
+          //            std::swap( centers[j][0], centers[j][ centers[j].size()-1 ] );
+          //          }
+          break;
+        }
+      }
+      if (isNew)
+      {
+        centers.push_back(vector<Center> (1, curCenters[i]));
+      }
+    }
+    //imshow("binarized", keypointsImage );
+    //waitKey();
+  }
+
+  for (size_t i = 0; i < centers.size(); i++)
+  {
+    if (centers[i].size() < params.minRepeatability)
+      continue;
+    Point2d sumPoint(0, 0);
+    double normalizer = 0;
+    for (size_t j = 0; j < centers[i].size(); j++)
+    {
+      sumPoint += centers[i][j].confidence * centers[i][j].location;
+      normalizer += centers[i][j].confidence;
+    }
+    sumPoint *= (1. / normalizer);
+    KeyPoint kpt(sumPoint, params.defaultKeypointSize);
+    keypoints.push_back(kpt);
+  }
+}

modules/calib3d/src/blobdetector.hpp

@@ -0,0 +1,92 @@
+/*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.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., 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 the copyright holders 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*/
+
+#ifndef BLOBDETECTOR_HPP_
+#define BLOBDETECTOR_HPP_
+
+#include "precomp.hpp"
+#include "../../features2d/include/opencv2/features2d/features2d.hpp"
+
+struct BlobDetectorParameters
+{
+  BlobDetectorParameters();
+
+  float thresholdStep;
+  float minThreshold;
+  float maxThreshold;
+  float maxCentersDist;
+  int defaultKeypointSize;
+  size_t minRepeatability;
+  bool computeRadius;
+  bool isGrayscaleCentroid;
+  int centroidROIMargin;
+
+  bool filterByArea, filterByInertia, filterByCircularity, filterByColor, filterByConvexity;
+  float minArea;
+  float maxArea;
+  float minCircularity;
+  float minInertiaRatio;
+  float minConvexity;
+};
+
+class BlobDetector //: public cv::FeatureDetector
+{
+public:
+  BlobDetector(const BlobDetectorParameters &parameters = BlobDetectorParameters());
+  void detect(const cv::Mat& image, vector<cv::KeyPoint>& keypoints, const cv::Mat& mask = cv::Mat()) const;
+protected:
+  struct Center
+  {
+    cv::Point2d location;
+    double radius;
+    double confidence;
+  };
+
+  virtual void detectImpl(const cv::Mat& image, vector<cv::KeyPoint>& keypoints, const cv::Mat& mask = cv::Mat()) const;
+  virtual void findBlobs(const cv::Mat &image, const cv::Mat &binaryImage, vector<Center> &centers) const;
+
+  cv::Point2d computeGrayscaleCentroid(const cv::Mat &image, const vector<cv::Point> &contour) const;
+
+  BlobDetectorParameters params;
+};
+
+#endif /* BLOBDETECTOR_HPP_ */

modules/calib3d/src/calibinit.cpp

@@ -60,6 +60,8 @@
 \************************************************************************************/
 
 #include "precomp.hpp"
+#include "circlesgrid.hpp"
+#include "blobdetector.hpp"
 #include <stdarg.h>
 
 //#define ENABLE_TRIM_COL_ROW
@@ -1933,6 +1935,62 @@ void drawChessboardCorners( Mat& image, Size patternSize,
                             (int)corners.size(), patternWasFound );
 }
 
+bool findCirclesGrid( const Mat& image, Size patternSize,
+                      vector<Point2f>& centers, int flags )
+{
+    Ptr<BlobDetector> detector = new BlobDetector();
+    //Ptr<FeatureDetector> detector = new MserFeatureDetector();
+    vector<KeyPoint> keypoints;
+    detector->detect(image, keypoints);
+
+    CirclesGridFinderParameters parameters;
+    parameters.vertexPenalty = -0.6;
+    parameters.vertexGain = 1;
+    parameters.existingVertexGain = 10000;
+    parameters.edgeGain = 1;
+    parameters.edgePenalty = -0.6;
+
+    const int attempts = 2;
+    const int minHomographyPoints = 4;
+    Mat H;
+    for (int i = 0; i < attempts; i++)
+    {
+      centers.clear();
+      CirclesGridFinder boxFinder(patternSize, keypoints, parameters);
+      bool isFound = false;
+      try
+      {
+        isFound = boxFinder.findHoles();
+      }
+      catch (cv::Exception &e)
+      {
+      }
+
+      boxFinder.getHoles(centers);
+
+      if (isFound)
+      {
+        if (i != 0)
+        {
+          Mat orgPointsMat;
+          transform(Mat(centers), orgPointsMat, H.inv());
+          convertPointsHomogeneous(orgPointsMat, centers);
+        }
+
+        return true;
+      }
+
+      if (i != attempts - 1)
+      {
+        if (centers.size() < minHomographyPoints)
+          break;
+        H = CirclesGridFinder::rectifyGrid(boxFinder.getDetectedGridSize(), centers, keypoints, keypoints);
+      }
+    }
+
+    return false;
+}
+
 }
 
 /* End of file. */

modules/calib3d/src/circlesgrid.cpp

@@ -0,0 +1,850 @@
+/*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.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., 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 the copyright holders 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 "circlesgrid.hpp"
+
+using namespace cv;
+
+Graph::Graph(int n)
+{
+  for (int i = 0; i < n; i++)
+  {
+    addVertex(i);
+  }
+}
+
+bool Graph::doesVertexExist(int id) const
+{
+  return (vertices.find(id) != vertices.end());
+}
+
+void Graph::addVertex(int id)
+{
+  assert( !doesVertexExist( id ) );
+
+  vertices.insert(pair<int, Vertex> (id, Vertex()));
+}
+
+void Graph::addEdge(int id1, int id2)
+{
+  assert( doesVertexExist( id1 ) );
+  assert( doesVertexExist( id2 ) );
+
+  vertices[id1].neighbors.insert(id2);
+  vertices[id2].neighbors.insert(id1);
+}
+
+bool Graph::areVerticesAdjacent(int id1, int id2) const
+{
+  assert( doesVertexExist( id1 ) );
+  assert( doesVertexExist( id2 ) );
+
+  Vertices::const_iterator it = vertices.find(id1);
+  return it->second.neighbors.find(id2) != it->second.neighbors.end();
+}
+
+size_t Graph::getVerticesCount() const
+{
+  return vertices.size();
+}
+
+size_t Graph::getDegree(int id) const
+{
+  assert( doesVertexExist(id) );
+
+  Vertices::const_iterator it = vertices.find(id);
+  return it->second.neighbors.size();
+}
+
+void Graph::floydWarshall(cv::Mat &distanceMatrix, int infinity) const
+{
+  const int edgeWeight = 1;
+
+  const size_t n = getVerticesCount();
+  distanceMatrix.create(n, n, CV_32SC1);
+  distanceMatrix.setTo(infinity);
+  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end(); it1++)
+  {
+    distanceMatrix.at<int> (it1->first, it1->first) = 0;
+    for (Neighbors::iterator it2 = it1->second.neighbors.begin(); it2 != it1->second.neighbors.end(); it2++)
+    {
+      assert( it1->first != *it2 );
+      distanceMatrix.at<int> (it1->first, *it2) = edgeWeight;
+    }
+  }
+
+  for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end(); it1++)
+  {
+    for (Vertices::const_iterator it2 = vertices.begin(); it2 != vertices.end(); it2++)
+    {
+      for (Vertices::const_iterator it3 = vertices.begin(); it3 != vertices.end(); it3++)
+      {
+        int val1 = distanceMatrix.at<int> (it2->first, it3->first);
+        int val2;
+        if (distanceMatrix.at<int> (it2->first, it1->first) == infinity || distanceMatrix.at<int> (it1->first,
+                                                                                                   it3->first)
+            == infinity)
+          val2 = val1;
+        else
+          val2 = distanceMatrix.at<int> (it2->first, it1->first) + distanceMatrix.at<int> (it1->first, it3->first);
+        distanceMatrix.at<int> (it2->first, it3->first) = std::min(val1, val2);
+      }
+    }
+  }
+}
+
+void computeShortestPath(Mat &predecessorMatrix, int v1, int v2, vector<int> &path);
+void computePredecessorMatrix(const Mat &dm, int verticesCount, Mat &predecessorMatrix);
+
+CirclesGridFinderParameters::CirclesGridFinderParameters()
+{
+  minDensity = 10;
+  densityNeighborhoodSize = Size2f(16, 16);
+  minDistanceToAddKeypoint = 20;
+  kmeansAttempts = 100;
+  convexHullFactor = 1.1;
+  keypointScale = 1;
+
+  minGraphConfidence = 9;
+  vertexGain = 2;
+  vertexPenalty = -5;
+  edgeGain = 1;
+  edgePenalty = -5;
+  existingVertexGain = 0;
+}
+
+CirclesGridFinder::CirclesGridFinder(Size _patternSize, const vector<KeyPoint> &testKeypoints,
+                                     const CirclesGridFinderParameters &_parameters) :
+  patternSize(_patternSize)
+{
+  keypoints = testKeypoints;
+  parameters = _parameters;
+}
+
+bool CirclesGridFinder::findHoles()
+{
+  vector<Point2f> vectors, filteredVectors, basis;
+  computeEdgeVectorsOfRNG(vectors);
+  filterOutliersByDensity(vectors, filteredVectors);
+  vector<Graph> basisGraphs;
+  findBasis(filteredVectors, basis, basisGraphs);
+  findMCS(basis, basisGraphs);
+
+  return (isDetectionCorrect());
+  //CV_Error( 0, "Detection is not correct" );
+}
+
+bool CirclesGridFinder::isDetectionCorrect()
+{
+  if (holes.size() != patternSize.height)
+    return false;
+
+  set<int> vertices;
+  for (size_t i = 0; i < holes.size(); i++)
+  {
+    if (holes[i].size() != patternSize.width)
+      return false;
+
+    for (size_t j = 0; j < holes[i].size(); j++)
+    {
+      vertices.insert(holes[i][j]);
+    }
+  }
+
+  return vertices.size() == patternSize.area();
+}
+
+void CirclesGridFinder::findMCS(const vector<Point2f> &basis, vector<Graph> &basisGraphs)
+{
+  Path longestPath;
+  size_t bestGraphIdx = findLongestPath(basisGraphs, longestPath);
+  vector<int> holesRow = longestPath.vertices;
+
+  while (holesRow.size() > std::max(patternSize.width, patternSize.height))
+  {
+    holesRow.pop_back();
+    holesRow.erase(holesRow.begin());
+  }
+
+  if (bestGraphIdx == 0)
+  {
+    holes.push_back(holesRow);
+    int w = holes[0].size();
+    int h = holes.size();
+
+    //parameters.minGraphConfidence = holes[0].size() * parameters.vertexGain + (holes[0].size() - 1) * parameters.edgeGain;
+    //parameters.minGraphConfidence = holes[0].size() * parameters.vertexGain + (holes[0].size() / 2) * parameters.edgeGain;
+    //parameters.minGraphConfidence = holes[0].size() * parameters.existingVertexGain + (holes[0].size() / 2) * parameters.edgeGain;
+    parameters.minGraphConfidence = holes[0].size() * parameters.existingVertexGain;
+    for (int i = h; i < patternSize.height; i++)
+    {
+      addHolesByGraph(basisGraphs, true, basis[1]);
+    }
+
+    //parameters.minGraphConfidence = holes.size() * parameters.existingVertexGain + (holes.size() / 2) * parameters.edgeGain;
+    parameters.minGraphConfidence = holes.size() * parameters.existingVertexGain;
+
+    for (int i = w; i < patternSize.width; i++)
+    {
+      addHolesByGraph(basisGraphs, false, basis[0]);
+    }
+  }
+  else
+  {
+    holes.resize(holesRow.size());
+    for (size_t i = 0; i < holesRow.size(); i++)
+      holes[i].push_back(holesRow[i]);
+
+    int w = holes[0].size();
+    int h = holes.size();
+
+    parameters.minGraphConfidence = holes.size() * parameters.existingVertexGain;
+    for (int i = w; i < patternSize.width; i++)
+    {
+      addHolesByGraph(basisGraphs, false, basis[0]);
+    }
+
+    parameters.minGraphConfidence = holes[0].size() * parameters.existingVertexGain;
+    for (int i = h; i < patternSize.height; i++)
+    {
+      addHolesByGraph(basisGraphs, true, basis[1]);
+    }
+  }
+}
+
+Mat CirclesGridFinder::rectifyGrid(Size detectedGridSize, const vector<Point2f>& centers,
+                                   const vector<KeyPoint> &keypoints, vector<KeyPoint> &warpedKeypoints)
+{
+  assert( !centers.empty() );
+  const float edgeLength = 30;
+  const Point2f offset(150, 150);
+  const int keypointScale = 1;
+
+  vector<Point2f> dstPoints;
+  for (int i = 0; i < detectedGridSize.height; i++)
+  {
+    for (int j = 0; j < detectedGridSize.width; j++)
+    {
+      dstPoints.push_back(offset + Point2f(edgeLength * j, edgeLength * i));
+    }
+  }
+
+  Mat H = findHomography(Mat(centers), Mat(dstPoints), CV_RANSAC);
+  //Mat H = findHomography( Mat( corners ), Mat( dstPoints ) );
+
+  vector<Point2f> srcKeypoints;
+  for (size_t i = 0; i < keypoints.size(); i++)
+  {
+    srcKeypoints.push_back(keypoints[i].pt);
+  }
+
+  Mat dstKeypointsMat;
+  transform(Mat(srcKeypoints), dstKeypointsMat, H);
+  vector<Point2f> dstKeypoints;
+  convertPointsHomogeneous(dstKeypointsMat, dstKeypoints);
+
+  warpedKeypoints.clear();
+  for (size_t i = 0; i < dstKeypoints.size(); i++)
+  {
+    Point2f pt = dstKeypoints[i];
+    warpedKeypoints.push_back(KeyPoint(pt, keypointScale));
+  }
+
+  return H;
+}
+
+int CirclesGridFinder::findNearestKeypoint(Point2f pt) const
+{
+  int bestIdx = -1;
+  float minDist = std::numeric_limits<float>::max();
+  for (size_t i = 0; i < keypoints.size(); i++)
+  {
+    float dist = norm(pt - keypoints[i].pt);
+    if (dist < minDist)
+    {
+      minDist = dist;
+      bestIdx = i;
+    }
+  }
+  return bestIdx;
+}
+
+void CirclesGridFinder::addPoint(Point2f pt, vector<int> &points)
+{
+  int ptIdx = findNearestKeypoint(pt);
+  if (norm(keypoints[ptIdx].pt - pt) > parameters.minDistanceToAddKeypoint)
+  {
+    KeyPoint kpt = KeyPoint(pt, parameters.keypointScale);
+    keypoints.push_back(kpt);
+    points.push_back(keypoints.size() - 1);
+  }
+  else
+  {
+    points.push_back(ptIdx);
+  }
+}
+
+void CirclesGridFinder::findCandidateLine(vector<int> &line, int seedLineIdx, bool addRow, Point2f basisVec,
+                                          vector<int> &seeds)
+{
+  line.clear();
+  seeds.clear();
+
+  if (addRow)
+  {
+    for (size_t i = 0; i < holes[seedLineIdx].size(); i++)
+    {
+      Point2f pt = keypoints[holes[seedLineIdx][i]].pt + basisVec;
+      addPoint(pt, line);
+      seeds.push_back(holes[seedLineIdx][i]);
+    }
+  }
+  else
+  {
+    for (size_t i = 0; i < holes.size(); i++)
+    {
+      Point2f pt = keypoints[holes[i][seedLineIdx]].pt + basisVec;
+      addPoint(pt, line);
+      seeds.push_back(holes[i][seedLineIdx]);
+    }
+  }
+
+  assert( line.size() == seeds.size() );
+}
+
+void CirclesGridFinder::findCandidateHoles(vector<int> &above, vector<int> &below, bool addRow, Point2f basisVec,
+                                           vector<int> &aboveSeeds, vector<int> &belowSeeds)
+{
+  above.clear();
+  below.clear();
+  aboveSeeds.clear();
+  belowSeeds.clear();
+
+  findCandidateLine(above, 0, addRow, -basisVec, aboveSeeds);
+  int lastIdx = addRow ? holes.size() - 1 : holes[0].size() - 1;
+  findCandidateLine(below, lastIdx, addRow, basisVec, belowSeeds);
+
+  assert( below.size() == above.size() );
+  assert( belowSeeds.size() == aboveSeeds.size() );
+  assert( below.size() == belowSeeds.size() );
+}
+
+bool CirclesGridFinder::areCentersNew(const vector<int> &newCenters, const vector<vector<int> > &holes)
+{
+  for (size_t i = 0; i < newCenters.size(); i++)
+  {
+    for (size_t j = 0; j < holes.size(); j++)
+    {
+      if (holes[j].end() != std::find(holes[j].begin(), holes[j].end(), newCenters[i]))
+      {
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
+void CirclesGridFinder::insertWinner(float aboveConfidence, float belowConfidence, float minConfidence, bool addRow,
+                                     const vector<int> &above, const vector<int> &below, vector<vector<int> > &holes)
+{
+  if (aboveConfidence < minConfidence && belowConfidence < minConfidence)
+    return;
+
+  if (addRow)
+  {
+    if (aboveConfidence >= belowConfidence)
+    {
+      if (!areCentersNew(above, holes))
+        CV_Error( 0, "Centers are not new" );
+
+      holes.insert(holes.begin(), above);
+    }
+    else
+    {
+      if (!areCentersNew(below, holes))
+        CV_Error( 0, "Centers are not new" );
+
+      holes.insert(holes.end(), below);
+    }
+  }
+  else
+  {
+    if (aboveConfidence >= belowConfidence)
+    {
+      if (!areCentersNew(above, holes))
+        CV_Error( 0, "Centers are not new" );
+
+      for (size_t i = 0; i < holes.size(); i++)
+      {
+        holes[i].insert(holes[i].begin(), above[i]);
+      }
+    }
+    else
+    {
+      if (!areCentersNew(below, holes))
+        CV_Error( 0, "Centers are not new" );
+
+      for (size_t i = 0; i < holes.size(); i++)
+      {
+        holes[i].insert(holes[i].end(), below[i]);
+      }
+    }
+  }
+}
+
+/*
+ bool CirclesGridFinder::areVerticesAdjacent(const Graph &graph, int vertex1, int vertex2)
+ {
+ property_map<Graph, vertex_index_t>::type index = get(vertex_index, graph);
+
+ bool areAdjacent = false;
+ graph_traits<Graph>::adjacency_iterator ai;
+ graph_traits<Graph>::adjacency_iterator ai_end;
+
+ for (tie(ai, ai_end) = adjacent_vertices(vertex1, graph); ai != ai_end; ++ai)
+ {
+ if (*ai == index[vertex2])
+ areAdjacent = true;
+ }
+
+ return areAdjacent;
+ }*/
+
+float CirclesGridFinder::computeGraphConfidence(const vector<Graph> &basisGraphs, bool addRow,
+                                                const vector<int> &points, const vector<int> &seeds)
+{
+  assert( points.size() == seeds.size() );
+  float confidence = 0;
+  const int vCount = basisGraphs[0].getVerticesCount();
+  assert( basisGraphs[0].getVerticesCount() == basisGraphs[1].getVerticesCount() );
+
+  for (size_t i = 0; i < seeds.size(); i++)
+  {
+    if (seeds[i] < vCount && points[i] < vCount)
+    {
+      if (!basisGraphs[addRow].areVerticesAdjacent(seeds[i], points[i]))
+      {
+        confidence += parameters.vertexPenalty;
+      }
+      else
+      {
+        confidence += parameters.vertexGain;
+      }
+    }
+
+    if (points[i] < vCount)
+    {
+      confidence += parameters.existingVertexGain;
+    }
+  }
+
+  for (size_t i = 1; i < points.size(); i++)
+  {
+    if (points[i - 1] < vCount && points[i] < vCount)
+    {
+      if (!basisGraphs[!addRow].areVerticesAdjacent(points[i - 1], points[i]))
+      {
+        confidence += parameters.edgePenalty;
+      }
+      else
+      {
+        confidence += parameters.edgeGain;
+      }
+    }
+  }
+  return confidence;
+
+}
+
+void CirclesGridFinder::addHolesByGraph(const vector<Graph> &basisGraphs, bool addRow, Point2f basisVec)
+{
+  vector<int> above, below, aboveSeeds, belowSeeds;
+  findCandidateHoles(above, below, addRow, basisVec, aboveSeeds, belowSeeds);
+  float aboveConfidence = computeGraphConfidence(basisGraphs, addRow, above, aboveSeeds);
+  float belowConfidence = computeGraphConfidence(basisGraphs, addRow, below, belowSeeds);
+
+  insertWinner(aboveConfidence, belowConfidence, parameters.minGraphConfidence, addRow, above, below, holes);
+}
+
+void CirclesGridFinder::filterOutliersByDensity(const vector<Point2f> &samples, vector<Point2f> &filteredSamples)
+{
+  if (samples.empty())
+    CV_Error( 0, "samples is empty" );
+
+  filteredSamples.clear();
+
+  for (size_t i = 0; i < samples.size(); i++)
+  {
+    Rect_<float> rect(samples[i] - Point2f(parameters.densityNeighborhoodSize) * 0.5,
+                      parameters.densityNeighborhoodSize);
+    int neighborsCount = 0;
+    for (size_t j = 0; j < samples.size(); j++)
+    {
+      if (rect.contains(samples[j]))
+        neighborsCount++;
+    }
+    if (neighborsCount >= parameters.minDensity)
+      filteredSamples.push_back(samples[i]);
+  }
+
+  if (filteredSamples.empty())
+    CV_Error( 0, "filteredSamples is empty" );
+}
+
+void CirclesGridFinder::findBasis(const vector<Point2f> &samples, vector<Point2f> &basis, vector<Graph> &basisGraphs)
+{
+  basis.clear();
+  Mat bestLabels;
+  TermCriteria termCriteria;
+  Mat centers;
+  int clustersCount = 4;
+  kmeans(Mat(samples).reshape(1, 0), clustersCount, bestLabels, termCriteria, parameters.kmeansAttempts,
+         KMEANS_RANDOM_CENTERS, &centers);
+  assert( centers.type() == CV_32FC1 );
+
+  vector<int> basisIndices;
+  //TODO: only remove duplicate
+  for (int i = 0; i < clustersCount; i++)
+  {
+    int maxIdx = (fabs(centers.at<float> (i, 0)) < fabs(centers.at<float> (i, 1)));
+    if (centers.at<float> (i, maxIdx) > 0)
+    {
+      Point2f vec(centers.at<float> (i, 0), centers.at<float> (i, 1));
+      basis.push_back(vec);
+      basisIndices.push_back(i);
+    }
+  }
+  if (basis.size() != 2)
+    CV_Error( 0, "Basis size is not 2");
+
+  if (basis[1].x > basis[0].x)
+  {
+    std::swap(basis[0], basis[1]);
+    std::swap(basisIndices[0], basisIndices[1]);
+  }
+
+  const float minBasisDif = 2;
+  if (norm(basis[0] - basis[1]) < minBasisDif)
+    CV_Error( 0, "degenerate basis" );
+
+  vector<vector<Point2f> > clusters(2), hulls(2);
+  for (size_t k = 0; k < samples.size(); k++)
+  {
+    int label = bestLabels.at<int> (k, 0);
+    int idx = -1;
+    if (label == basisIndices[0])
+      idx = 0;
+    if (label == basisIndices[1])
+      idx = 1;
+    if (idx >= 0)
+    {
+      clusters[idx].push_back(basis[idx] + parameters.convexHullFactor * (samples[k] - basis[idx]));
+    }
+  }
+  for (size_t i = 0; i < basis.size(); i++)
+  {
+    convexHull(Mat(clusters[i]), hulls[i]);
+  }
+
+  basisGraphs.resize(basis.size(), Graph(keypoints.size()));
+  for (size_t i = 0; i < keypoints.size(); i++)
+  {
+    for (size_t j = 0; j < keypoints.size(); j++)
+    {
+      if (i == j)
+        continue;
+
+      Point2f vec = keypoints[i].pt - keypoints[j].pt;
+
+      for (size_t k = 0; k < hulls.size(); k++)
+      {
+        if (pointPolygonTest(Mat(hulls[k]), vec, false) >= 0)
+        {
+          basisGraphs[k].addEdge(i, j);
+        }
+      }
+    }
+  }
+}
+
+void CirclesGridFinder::computeEdgeVectorsOfRNG(vector<Point2f> &vectors, Mat *drawImage) const
+{
+  vectors.clear();
+
+  //TODO: use more fast algorithm instead of naive N^3
+  for (size_t i = 0; i < keypoints.size(); i++)
+  {
+    for (size_t j = 0; j < keypoints.size(); j++)
+    {
+      if (i == j)
+        continue;
+
+      Point2f vec = keypoints[i].pt - keypoints[j].pt;
+      float dist = norm(vec);
+
+      bool isNeighbors = true;
+      for (size_t k = 0; k < keypoints.size(); k++)
+      {
+        if (k == i || k == j)
+          continue;
+
+        float dist1 = norm(keypoints[i].pt - keypoints[k].pt);
+        float dist2 = norm(keypoints[j].pt - keypoints[k].pt);
+        if (dist1 < dist && dist2 < dist)
+        {
+          isNeighbors = false;
+          break;
+        }
+      }
+
+      if (isNeighbors)
+      {
+        vectors.push_back(keypoints[i].pt - keypoints[j].pt);
+        if (drawImage != 0)
+        {
+          line(*drawImage, keypoints[i].pt, keypoints[j].pt, Scalar(255, 0, 0), 2);
+          circle(*drawImage, keypoints[i].pt, 3, Scalar(0, 0, 255), -1);
+          circle(*drawImage, keypoints[j].pt, 3, Scalar(0, 0, 255), -1);
+        }
+      }
+    }
+  }
+}
+
+void computePredecessorMatrix(const Mat &dm, int verticesCount, Mat &predecessorMatrix)
+{
+  assert( dm.type() == CV_32SC1 );
+  predecessorMatrix.create(verticesCount, verticesCount, CV_32SC1);
+  predecessorMatrix = -1;
+  for (int i = 0; i < predecessorMatrix.rows; i++)
+  {
+    for (int j = 0; j < predecessorMatrix.cols; j++)
+    {
+      int dist = dm.at<int> (i, j);
+      for (int k = 0; k < verticesCount; k++)
+      {
+        if (dm.at<int> (i, k) == dist - 1 && dm.at<int> (k, j) == 1)
+        {
+          predecessorMatrix.at<int> (i, j) = k;
+          break;
+        }
+      }
+    }
+  }
+}
+
+void computeShortestPath(Mat &predecessorMatrix, int v1, int v2, vector<int> &path)
+{
+  if (predecessorMatrix.at<int> (v1, v2) < 0)
+  {
+    path.push_back(v1);
+    return;
+  }
+
+  computeShortestPath(predecessorMatrix, v1, predecessorMatrix.at<int> (v1, v2), path);
+  path.push_back(v2);
+}
+
+size_t CirclesGridFinder::findLongestPath(vector<Graph> &basisGraphs, Path &bestPath)
+{
+  vector<Path> longestPaths(1);
+  vector<int> confidences;
+
+  size_t bestGraphIdx = 0;
+  const int infinity = -1;
+  for (size_t graphIdx = 0; graphIdx < basisGraphs.size(); graphIdx++)
+  {
+    const Graph &g = basisGraphs[graphIdx];
+    Mat distanceMatrix;
+    g.floydWarshall(distanceMatrix, infinity);
+    Mat predecessorMatrix;
+    computePredecessorMatrix(distanceMatrix, g.getVerticesCount(), predecessorMatrix);
+
+    double maxVal;
+    Point maxLoc;
+    assert (infinity < 0);
+    minMaxLoc(distanceMatrix, 0, &maxVal, 0, &maxLoc);
+
+    if (maxVal > longestPaths[0].length)
+    {
+      longestPaths.clear();
+      confidences.clear();
+      bestGraphIdx = graphIdx;
+    }
+    if (longestPaths.empty() || (maxVal == longestPaths[0].length && graphIdx == bestGraphIdx))
+    {
+      Path path = Path(maxLoc.x, maxLoc.y, maxVal);
+      computeShortestPath(predecessorMatrix, maxLoc.x, maxLoc.y, path.vertices);
+      longestPaths.push_back(path);
+
+      int conf = 0;
+      for (size_t v2 = 0; v2 < path.vertices.size(); v2++)
+      {
+        conf += basisGraphs[1 - (int)graphIdx].getDegree(v2);
+      }
+      confidences.push_back(conf);
+    }
+  }
+  //if( bestGraphIdx != 0 )
+  //CV_Error( 0, "" );
+
+  int maxConf = -1;
+  int bestPathIdx = -1;
+  for (size_t i = 0; i < confidences.size(); i++)
+  {
+    if (confidences[i] > maxConf)
+    {
+      maxConf = confidences[i];
+      bestPathIdx = i;
+    }
+  }
+
+  //int bestPathIdx = rand() % longestPaths.size();
+  bestPath = longestPaths.at(bestPathIdx);
+  bool needReverse = (bestGraphIdx == 0 && keypoints[bestPath.lastVertex].pt.x < keypoints[bestPath.firstVertex].pt.x)
+      || (bestGraphIdx == 1 && keypoints[bestPath.lastVertex].pt.y < keypoints[bestPath.firstVertex].pt.y);
+  if (needReverse)
+  {
+    std::swap(bestPath.lastVertex, bestPath.firstVertex);
+    std::reverse(bestPath.vertices.begin(), bestPath.vertices.end());
+  }
+  return bestGraphIdx;
+}
+
+void CirclesGridFinder::drawBasis(const vector<Point2f> &basis, Point2f origin, Mat &drawImg) const
+{
+  for (size_t i = 0; i < basis.size(); i++)
+  {
+    Point2f pt(basis[i]);
+    line(drawImg, origin, origin + pt, Scalar(0, i * 255, 0), 2);
+  }
+}
+
+void CirclesGridFinder::drawBasisGraphs(const vector<Graph> &basisGraphs, Mat &drawImage, bool drawEdges,
+                                        bool drawVertices) const
+{
+  //const int vertexRadius = 1;
+  const int vertexRadius = 3;
+  const Scalar vertexColor = Scalar(0, 0, 255);
+  const int vertexThickness = -1;
+
+  const Scalar edgeColor = Scalar(255, 0, 0);
+  //const int edgeThickness = 1;
+  const int edgeThickness = 2;
+
+  if (drawEdges)
+  {
+    for (size_t i = 0; i < basisGraphs.size(); i++)
+    {
+      for (size_t v1 = 0; v1 < basisGraphs[i].getVerticesCount(); v1++)
+      {
+        for (size_t v2 = 0; v2 < basisGraphs[i].getVerticesCount(); v2++)
+        {
+          if (basisGraphs[i].areVerticesAdjacent(v1, v2))
+          {
+            line(drawImage, keypoints[v1].pt, keypoints[v2].pt, edgeColor, edgeThickness);
+          }
+        }
+      }
+    }
+  }
+  if (drawVertices)
+  {
+    for (size_t v = 0; v < basisGraphs[0].getVerticesCount(); v++)
+    {
+      circle(drawImage, keypoints[v].pt, vertexRadius, vertexColor, vertexThickness);
+    }
+  }
+}
+
+void CirclesGridFinder::drawHoles(const Mat &srcImage, Mat &drawImage) const
+{
+  //const int holeRadius = 4;
+  //const int holeRadius = 2;
+  //const int holeThickness = 1;
+  const int holeRadius = 3;
+  const int holeThickness = -1;
+
+  //const Scalar holeColor = Scalar(0, 0, 255);
+  const Scalar holeColor = Scalar(0, 255, 0);
+
+  if (srcImage.channels() == 1)
+    cvtColor(srcImage, drawImage, CV_GRAY2RGB);
+  else
+    srcImage.copyTo(drawImage);
+
+  for (size_t i = 0; i < holes.size(); i++)
+  {
+    for (size_t j = 0; j < holes[i].size(); j++)
+    {
+      if (j != holes[i].size() - 1)
+        line(drawImage, keypoints[holes[i][j]].pt, keypoints[holes[i][j + 1]].pt, Scalar(255, 0, 0), 2);
+      if (i != holes.size() - 1)
+        line(drawImage, keypoints[holes[i][j]].pt, keypoints[holes[i + 1][j]].pt, Scalar(255, 0, 0), 2);
+
+      //circle(drawImage, keypoints[holes[i][j]].pt, holeRadius, holeColor, holeThickness);
+      circle(drawImage, keypoints[holes[i][j]].pt, holeRadius, holeColor, holeThickness);
+    }
+  }
+}
+
+Size CirclesGridFinder::getDetectedGridSize() const
+{
+  if (holes.size() == 0)
+    return Size(0, 0);
+
+  return Size(holes[0].size(), holes.size());
+}
+
+void CirclesGridFinder::getHoles(vector<Point2f> &outHoles) const
+{
+  outHoles.clear();
+
+  for (size_t i = 0; i < holes.size(); i++)
+  {
+    for (size_t j = 0; j < holes[i].size(); j++)
+    {
+      outHoles.push_back(keypoints[holes[i][j]].pt);
+    }
+  }
+}

modules/calib3d/src/circlesgrid.hpp

@@ -0,0 +1,156 @@
+/*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.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., 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 the copyright holders 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*/
+
+#ifndef CIRCLESGRID_HPP_
+#define CIRCLESGRID_HPP_
+
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <set>
+
+#include "precomp.hpp"
+#include "../../features2d/include/opencv2/features2d/features2d.hpp"
+
+class Graph
+{
+public:
+  typedef set<int> Neighbors;
+  struct Vertex
+  {
+    Neighbors neighbors;
+  };
+  typedef map<int, Vertex> Vertices;
+
+  Graph( int n);
+  bool doesVertexExist( int id ) const;
+  void addVertex( int id );
+  void addEdge( int id1, int id2 );
+  bool areVerticesAdjacent( int id1, int id2 ) const;
+  size_t getVerticesCount() const;
+  size_t getDegree( int id ) const;
+  void floydWarshall(cv::Mat &distanceMatrix, int infinity = -1) const;
+
+private:
+  Vertices vertices;
+};
+
+struct Path
+{
+  int firstVertex;
+  int lastVertex;
+  int length;
+
+  vector<int> vertices;
+
+  Path(int first = -1, int last = -1, int len = -1)
+  {
+    firstVertex = first;
+    lastVertex = last;
+    length = len;
+  }
+};
+
+struct CirclesGridFinderParameters
+{
+  CirclesGridFinderParameters();
+  cv::Size2f densityNeighborhoodSize;
+  float minDensity;
+  int kmeansAttempts;
+  int minDistanceToAddKeypoint;
+  int keypointScale;
+  int minGraphConfidence;
+  float vertexGain;
+  float vertexPenalty;
+  float existingVertexGain;
+  float edgeGain;
+  float edgePenalty;
+  float convexHullFactor;
+};
+
+class CirclesGridFinder
+{
+public:
+  CirclesGridFinder(cv::Size patternSize, const vector<cv::KeyPoint> &testKeypoints,
+                    const CirclesGridFinderParameters &parameters = CirclesGridFinderParameters());
+  bool findHoles();
+  static cv::Mat rectifyGrid(cv::Size detectedGridSize, const vector<cv::Point2f>& centers,
+                          const vector<cv::KeyPoint> &keypoint, vector<cv::KeyPoint> &warpedKeypoints);
+
+  void getHoles(vector<cv::Point2f> &holes) const;
+  cv::Size getDetectedGridSize() const;
+
+  void drawBasis(const vector<cv::Point2f> &basis, cv::Point2f origin, cv::Mat &drawImg) const;
+  void drawBasisGraphs(const vector<Graph> &basisGraphs, cv::Mat &drawImg, bool drawEdges = true, bool drawVertices =
+      true) const;
+  void drawHoles(const cv::Mat &srcImage, cv::Mat &drawImage) const;
+private:
+  void computeEdgeVectorsOfRNG(vector<cv::Point2f> &vectors, cv::Mat *drawImage = 0) const;
+  void filterOutliersByDensity(const vector<cv::Point2f> &samples, vector<cv::Point2f> &filteredSamples);
+  void findBasis(const vector<cv::Point2f> &samples, vector<cv::Point2f> &basis, vector<Graph> &basisGraphs);
+  void findMCS(const vector<cv::Point2f> &basis, vector<Graph> &basisGraphs);
+  size_t findLongestPath(vector<Graph> &basisGraphs, Path &bestPath);
+  float computeGraphConfidence(const vector<Graph> &basisGraphs, bool addRow, const vector<int> &points, const vector<
+      int> &seeds);
+  void addHolesByGraph(const vector<Graph> &basisGraphs, bool addRow, cv::Point2f basisVec);
+
+  int findNearestKeypoint(cv::Point2f pt) const;
+  void addPoint(cv::Point2f pt, vector<int> &points);
+  void findCandidateLine(vector<int> &line, int seedLineIdx, bool addRow, cv::Point2f basisVec, vector<int> &seeds);
+  void findCandidateHoles(vector<int> &above, vector<int> &below, bool addRow, cv::Point2f basisVec,
+                          vector<int> &aboveSeeds, vector<int> &belowSeeds);
+  static bool areCentersNew( const vector<int> &newCenters, const vector<vector<int> > &holes );
+  bool isDetectionCorrect();
+
+  static void insertWinner(float aboveConfidence, float belowConfidence, float minConfidence,
+                           bool addRow,
+                           const vector<int> &above, const vector<int> &below, vector<vector<int> > &holes);
+  static bool areVerticesAdjacent(const Graph &graph, int vertex1, int vertex2);
+
+  vector<cv::KeyPoint> keypoints;
+
+  vector<vector<int> > holes;
+  const cv::Size patternSize;
+  CirclesGridFinderParameters parameters;
+};
+
+#endif /* CIRCLESGRID_HPP_ */