From 37cd2b6f2599b1124e8d5fdf86f40f6fc88fd8cd Mon Sep 17 00:00:00 2001 From: Ilya Lysenkov Date: Fri, 15 Apr 2011 15:33:11 +0000 Subject: [PATCH] Implemented new algorithm for asymmetric circles pattern detection. Use flag CALIB_CB_CLUSTERING. --- .../include/opencv2/calib3d/calib3d.hpp | 3 +- modules/calib3d/src/calibinit.cpp | 15 +- modules/calib3d/src/circlesgrid.cpp | 250 ++++++++++++++++++ modules/calib3d/src/circlesgrid.hpp | 24 ++ 4 files changed, 290 insertions(+), 2 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 3680ed08e3..16fcd2bb33 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -560,7 +560,8 @@ CV_EXPORTS void drawChessboardCorners( Mat& image, Size patternSize, const vector& corners, bool patternWasFound ); -enum { CALIB_CB_SYMMETRIC_GRID = 1, CALIB_CB_ASYMMETRIC_GRID = 2 }; +enum { CALIB_CB_SYMMETRIC_GRID = 1, CALIB_CB_ASYMMETRIC_GRID = 2, + CALIB_CB_CLUSTERING = 4, CALIB_CB_WHITE_CIRCLES = 8 }; //! finds circles' grid pattern of the specified size in the image CV_EXPORTS_W bool findCirclesGrid( const Mat& image, Size patternSize, diff --git a/modules/calib3d/src/calibinit.cpp b/modules/calib3d/src/calibinit.cpp index 50a056c562..afb223186d 100644 --- a/modules/calib3d/src/calibinit.cpp +++ b/modules/calib3d/src/calibinit.cpp @@ -1937,7 +1937,13 @@ void drawChessboardCorners( Mat& image, Size patternSize, bool findCirclesGrid( const Mat& image, Size patternSize, vector& centers, int flags ) { - Ptr detector = new SimpleBlobDetector(); + SimpleBlobDetector::Params params; + if(flags & CALIB_CB_WHITE_CIRCLES) + { + params.filterByColor = true; + params.blobColor = 255; + } + Ptr detector = new SimpleBlobDetector(params); //Ptr detector = new MserFeatureDetector(); vector keypoints; detector->detect(image, keypoints); @@ -1947,6 +1953,13 @@ bool findCirclesGrid( const Mat& image, Size patternSize, points.push_back (keypoints[i].pt); } + if((flags & CALIB_CB_CLUSTERING) && (flags & CALIB_CB_ASYMMETRIC_GRID)) + { + CirclesGridClusterFinder circlesGridClusterFinder; + circlesGridClusterFinder.findGrid(points, patternSize, centers); + return !centers.empty(); + } + CirclesGridFinderParameters parameters; parameters.vertexPenalty = -0.6f; parameters.vertexGain = 1; diff --git a/modules/calib3d/src/circlesgrid.cpp b/modules/calib3d/src/circlesgrid.cpp index d4c8a01a2c..2ad00b4e99 100644 --- a/modules/calib3d/src/circlesgrid.cpp +++ b/modules/calib3d/src/circlesgrid.cpp @@ -43,9 +43,259 @@ #include "circlesgrid.hpp" //#define DEBUG_CIRCLES +#ifdef DEBUG_CIRCLES +#include +#endif + using namespace cv; using namespace std; +void CirclesGridClusterFinder::hierarchicalClustering(const vector points, const Size &patternSize, vector &patternPoints) +{ + Mat dists(points.size(), points.size(), CV_32FC1, Scalar(0)); + Mat distsMask(dists.size(), CV_8UC1, Scalar(0)); + for(size_t i=0; i(i, j) = norm(points[i] - points[j]); + distsMask.at(i, j) = 255; + //TODO: use symmetry + distsMask.at(j, i) = distsMask.at(i, j); + dists.at(j, i) = dists.at(i, j); + } + } + + vector > clusters(points.size()); + for(size_t i=0; i 0) + { + Point minLoc; + minMaxLoc(dists, 0, 0, &minLoc, 0, distsMask); + int minIdx = std::min(minLoc.x, minLoc.y); + int maxIdx = std::max(minLoc.x, minLoc.y); + + distsMask.row(maxIdx).setTo(0); + distsMask.col(maxIdx).setTo(0); + Mat newDists = cv::min(dists.row(minLoc.x), dists.row(minLoc.y)); + Mat tmpLine = dists.row(minIdx); + newDists.copyTo(tmpLine); + tmpLine = dists.col(minIdx); + newDists.copyTo(tmpLine); + + clusters[minIdx].splice(clusters[minIdx].end(), clusters[maxIdx]); + patternClusterIdx = minIdx; + } + + patternPoints.clear(); + if(clusters[patternClusterIdx].size() != patternSize.area()) + { + return; + } + patternPoints.reserve(clusters[patternClusterIdx].size()); + for(std::list::iterator it = clusters[patternClusterIdx].begin(); it != clusters[patternClusterIdx].end(); it++) + { + patternPoints.push_back(points[*it]); + } +} + +void CirclesGridClusterFinder::findGrid(const std::vector points, cv::Size patternSize, vector& centers) +{ + centers.clear(); + + vector patternPoints; + hierarchicalClustering(points, patternSize, patternPoints); + if(patternPoints.empty()) + { + return; + } + + vector hull2f; + convexHull(Mat(patternPoints), hull2f, false); + + vector corners; + findCorners(hull2f, corners); + + vector outsideCorners; + findOutsideCorners(corners, outsideCorners); + + vector sortedCorners; + getSortedCorners(hull2f, corners, outsideCorners, sortedCorners); + + vector rectifiedPatternPoints; + rectifyPatternPoints(patternSize, patternPoints, sortedCorners, rectifiedPatternPoints); + + parsePatternPoints(patternSize, patternPoints, rectifiedPatternPoints, centers); +} + +void CirclesGridClusterFinder::findCorners(const std::vector &hull2f, std::vector &corners) +{ + //find angles (cosines) of vertices in convex hull + vector angles; + for(size_t i=0; i(hull2f.size())) % hull2f.size()] - hull2f[i % hull2f.size()]; + float angle = vec1.ddot(vec2) / (norm(vec1) * norm(vec2)); + angles.push_back(angle); + } + + //sort angles by cosine + //corners are the most sharp angles (6) + Mat anglesMat = Mat(angles); + Mat sortedIndices; + sortIdx(anglesMat, sortedIndices, CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING); + CV_Assert(sortedIndices.type() == CV_32SC1); + const int cornersCount = 6; + corners.clear(); + for(int i=0; i(i, 0)]); + } +} + +void CirclesGridClusterFinder::findOutsideCorners(const std::vector &corners, std::vector &outsideCorners) +{ + //find two pairs of the most nearest corners + double min1 = std::numeric_limits::max(); + double min2 = std::numeric_limits::max(); + Point minLoc1, minLoc2; + + for(size_t i=0; i outsideCornersIndices; + for(size_t i=0; i::iterator it = outsideCornersIndices.begin(); it != outsideCornersIndices.end(); it++) + { + outsideCorners.push_back(corners[*it]); + } +} + +void CirclesGridClusterFinder::getSortedCorners(const std::vector &hull2f, const std::vector &corners, const std::vector &outsideCorners, std::vector &sortedCorners) +{ + Point2f center = std::accumulate(corners.begin(), corners.end(), Point2f(0.0f, 0.0f)); + center *= 1.0 / corners.size(); + + vector centerToCorners; + for(size_t i=0; i 0 + bool isClockwise = crossProduct > 0; + Point2f firstCorner = isClockwise ? outsideCorners[1] : outsideCorners[0]; + + std::vector::const_iterator firstCornerIterator = std::find(hull2f.begin(), hull2f.end(), firstCorner); + sortedCorners.clear(); + for(vector::const_iterator it = firstCornerIterator; it != hull2f.end(); it++) + { + vector::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it); + if(itCorners != corners.end()) + { + sortedCorners.push_back(*it); + } + } + for(vector::const_iterator it = hull2f.begin(); it != firstCornerIterator; it++) + { + vector::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it); + if(itCorners != corners.end()) + { + sortedCorners.push_back(*it); + } + } +} + +void CirclesGridClusterFinder::rectifyPatternPoints(const cv::Size &patternSize, const std::vector &patternPoints, const std::vector &sortedCorners, std::vector &rectifiedPatternPoints) +{ + //indices of corner points in pattern + vector trueIndices; + trueIndices.push_back(Point(0, 0)); + trueIndices.push_back(Point(patternSize.width - 1, 0)); + trueIndices.push_back(Point(patternSize.width - 1, 1)); + trueIndices.push_back(Point(patternSize.width - 1, patternSize.height - 2)); + trueIndices.push_back(Point(patternSize.width - 1, patternSize.height - 1)); + trueIndices.push_back(Point(0, patternSize.height - 1)); + + vector idealPoints; + for(size_t idx=0; idx &patternPoints, const std::vector &rectifiedPatternPoints, std::vector ¢ers) +{ + flann::LinearIndexParams flannIndexParams; + flann::Index flannIndex(Mat(rectifiedPatternPoints).reshape(1), flannIndexParams); + + centers.clear(); + for( int i = 0; i < patternSize.height; i++ ) + { + for( int j = 0; j < patternSize.width; j++ ) + { + Point2f idealPt((2*j + i % 2)*squareSize, i*squareSize); + vector query = Mat(idealPt); + int knn = 1; + vector indices(knn); + vector dists(knn); + flannIndex.knnSearch(query, indices, dists, knn, flann::SearchParams()); + centers.push_back(patternPoints.at(indices[0])); + + if(dists[0] > maxRectifiedDistance) + { + centers.clear(); + return; + } + } + } +} + Graph::Graph(size_t n) { for (size_t i = 0; i < n; i++) diff --git a/modules/calib3d/src/circlesgrid.hpp b/modules/calib3d/src/circlesgrid.hpp index 892271ed15..90b38cb051 100644 --- a/modules/calib3d/src/circlesgrid.hpp +++ b/modules/calib3d/src/circlesgrid.hpp @@ -45,9 +45,33 @@ #include #include +#include +#include #include "precomp.hpp" +class CirclesGridClusterFinder +{ +public: + CirclesGridClusterFinder() + { + squareSize = 1.0f; + maxRectifiedDistance = squareSize / 2.0; + } + void findGrid(const std::vector points, cv::Size patternSize, std::vector& centers); + + //cluster 2d points by geometric coordinates + void hierarchicalClustering(const std::vector points, const cv::Size &patternSize, std::vector &patternPoints); +private: + void findCorners(const std::vector &hull2f, std::vector &corners); + void findOutsideCorners(const std::vector &corners, std::vector &outsideCorners); + void getSortedCorners(const std::vector &hull2f, const std::vector &corners, const std::vector &outsideCorners, std::vector &sortedCorners); + void rectifyPatternPoints(const cv::Size &patternSize, const std::vector &patternPoints, const std::vector &sortedCorners, std::vector &rectifiedPatternPoints); + void parsePatternPoints(const cv::Size &patternSize, const std::vector &patternPoints, const std::vector &rectifiedPatternPoints, std::vector ¢ers); + + float squareSize, maxRectifiedDistance; +}; + class Graph { public: