Added detection of cirlces' grid pattern

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

@@ -543,6 +543,11 @@ 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

+/*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

+/*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.hpp

+/*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_ */