/*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 "precomp.hpp" #include "circlesgrid.hpp" #include <limits> //#define DEBUG_CIRCLES #ifdef DEBUG_CIRCLES # include "opencv2/opencv_modules.hpp" # ifdef HAVE_OPENCV_HIGHGUI # include "opencv2/highgui.hpp" # else # undef DEBUG_CIRCLES # endif #endif using namespace cv; #ifdef DEBUG_CIRCLES void drawPoints(const std::vector<Point2f> &points, Mat &outImage, int radius = 2, Scalar color = Scalar::all(255), int thickness = -1) { for(size_t i=0; i<points.size(); i++) { circle(outImage, points[i], radius, color, thickness); } } #endif void CirclesGridClusterFinder::hierarchicalClustering(const std::vector<Point2f> &points, const Size &patternSz, std::vector<Point2f> &patternPoints) { #ifdef HAVE_TEGRA_OPTIMIZATION if(tegra::useTegra() && tegra::hierarchicalClustering(points, patternSz, patternPoints)) return; #endif int j, n = (int)points.size(); size_t pn = static_cast<size_t>(patternSz.area()); patternPoints.clear(); if (pn >= points.size()) { if (pn == points.size()) patternPoints = points; return; } Mat dists(n, n, CV_32FC1, Scalar(0)); Mat distsMask(dists.size(), CV_8UC1, Scalar(0)); for(int i = 0; i < n; i++) { for(j = i+1; j < n; j++) { dists.at<float>(i, j) = (float)norm(points[i] - points[j]); distsMask.at<uchar>(i, j) = 255; //TODO: use symmetry distsMask.at<uchar>(j, i) = 255;//distsMask.at<uchar>(i, j); dists.at<float>(j, i) = dists.at<float>(i, j); } } std::vector<std::list<size_t> > clusters(points.size()); for(size_t i=0; i<points.size(); i++) { clusters[i].push_back(i); } int patternClusterIdx = 0; while(clusters[patternClusterIdx].size() < pn) { 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 tmpRow = dists.row(minIdx); Mat tmpCol = dists.col(minIdx); cv::min(dists.row(minLoc.x), dists.row(minLoc.y), tmpRow); tmpRow.copyTo(tmpCol); clusters[minIdx].splice(clusters[minIdx].end(), clusters[maxIdx]); patternClusterIdx = minIdx; } //the largest cluster can have more than pn points -- we need to filter out such situations if(clusters[patternClusterIdx].size() != static_cast<size_t>(patternSz.area())) { return; } patternPoints.reserve(clusters[patternClusterIdx].size()); for(std::list<size_t>::iterator it = clusters[patternClusterIdx].begin(); it != clusters[patternClusterIdx].end(); it++) { patternPoints.push_back(points[*it]); } } void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> &points, cv::Size _patternSize, std::vector<Point2f>& centers) { patternSize = _patternSize; centers.clear(); if(points.empty()) { return; } std::vector<Point2f> patternPoints; hierarchicalClustering(points, patternSize, patternPoints); if(patternPoints.empty()) { return; } #ifdef DEBUG_CIRCLES Mat patternPointsImage(1024, 1248, CV_8UC1, Scalar(0)); drawPoints(patternPoints, patternPointsImage); imshow("pattern points", patternPointsImage); #endif std::vector<Point2f> hull2f; convexHull(Mat(patternPoints), hull2f, false); const size_t cornersCount = isAsymmetricGrid ? 6 : 4; if(hull2f.size() < cornersCount) return; std::vector<Point2f> corners; findCorners(hull2f, corners); if(corners.size() != cornersCount) return; std::vector<Point2f> outsideCorners, sortedCorners; if(isAsymmetricGrid) { findOutsideCorners(corners, outsideCorners); const size_t outsideCornersCount = 2; if(outsideCorners.size() != outsideCornersCount) return; } getSortedCorners(hull2f, corners, outsideCorners, sortedCorners); if(sortedCorners.size() != cornersCount) return; std::vector<Point2f> rectifiedPatternPoints; rectifyPatternPoints(patternPoints, sortedCorners, rectifiedPatternPoints); if(patternPoints.size() != rectifiedPatternPoints.size()) return; parsePatternPoints(patternPoints, rectifiedPatternPoints, centers); } void CirclesGridClusterFinder::findCorners(const std::vector<cv::Point2f> &hull2f, std::vector<cv::Point2f> &corners) { //find angles (cosines) of vertices in convex hull std::vector<float> angles; for(size_t i=0; i<hull2f.size(); i++) { Point2f vec1 = hull2f[(i+1) % hull2f.size()] - hull2f[i % hull2f.size()]; Point2f vec2 = hull2f[(i-1 + static_cast<int>(hull2f.size())) % hull2f.size()] - hull2f[i % hull2f.size()]; float angle = (float)(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, SORT_EVERY_COLUMN + SORT_DESCENDING); CV_Assert(sortedIndices.type() == CV_32SC1); CV_Assert(sortedIndices.cols == 1); const int cornersCount = isAsymmetricGrid ? 6 : 4; Mat cornersIndices; cv::sort(sortedIndices.rowRange(0, cornersCount), cornersIndices, SORT_EVERY_COLUMN + SORT_ASCENDING); corners.clear(); for(int i=0; i<cornersCount; i++) { corners.push_back(hull2f[cornersIndices.at<int>(i, 0)]); } } void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f> &corners, std::vector<cv::Point2f> &outsideCorners) { CV_Assert(!corners.empty()); outsideCorners.clear(); //find two pairs of the most nearest corners int i, j, n = (int)corners.size(); #ifdef DEBUG_CIRCLES Mat cornersImage(1024, 1248, CV_8UC1, Scalar(0)); drawPoints(corners, cornersImage); imshow("corners", cornersImage); #endif std::vector<Point2f> tangentVectors(corners.size()); for(size_t k=0; k<corners.size(); k++) { Point2f diff = corners[(k + 1) % corners.size()] - corners[k]; tangentVectors[k] = diff * (1.0f / norm(diff)); } //compute angles between all sides Mat cosAngles(n, n, CV_32FC1, 0.0f); for(i = 0; i < n; i++) { for(j = i + 1; j < n; j++) { float val = fabs(tangentVectors[i].dot(tangentVectors[j])); cosAngles.at<float>(i, j) = val; cosAngles.at<float>(j, i) = val; } } //find two parallel sides to which outside corners belong Point maxLoc; minMaxLoc(cosAngles, 0, 0, 0, &maxLoc); const int diffBetweenFalseLines = 3; if(abs(maxLoc.x - maxLoc.y) == diffBetweenFalseLines) { cosAngles.row(maxLoc.x).setTo(0.0f); cosAngles.col(maxLoc.x).setTo(0.0f); cosAngles.row(maxLoc.y).setTo(0.0f); cosAngles.col(maxLoc.y).setTo(0.0f); minMaxLoc(cosAngles, 0, 0, 0, &maxLoc); } #ifdef DEBUG_CIRCLES Mat linesImage(1024, 1248, CV_8UC1, Scalar(0)); line(linesImage, corners[maxLoc.y], corners[(maxLoc.y + 1) % n], Scalar(255)); line(linesImage, corners[maxLoc.x], corners[(maxLoc.x + 1) % n], Scalar(255)); imshow("lines", linesImage); #endif int maxIdx = std::max(maxLoc.x, maxLoc.y); int minIdx = std::min(maxLoc.x, maxLoc.y); const int bigDiff = 4; if(maxIdx - minIdx == bigDiff) { minIdx += n; std::swap(maxIdx, minIdx); } if(maxIdx - minIdx != n - bigDiff) { return; } int outsidersSegmentIdx = (minIdx + maxIdx) / 2; outsideCorners.push_back(corners[outsidersSegmentIdx % n]); outsideCorners.push_back(corners[(outsidersSegmentIdx + 1) % n]); #ifdef DEBUG_CIRCLES drawPoints(outsideCorners, cornersImage, 2, Scalar(128)); imshow("corners", outsideCornersImage); #endif } void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners) { Point2f firstCorner; if(isAsymmetricGrid) { Point2f center = std::accumulate(corners.begin(), corners.end(), Point2f(0.0f, 0.0f)); center *= 1.0 / corners.size(); std::vector<Point2f> centerToCorners; for(size_t i=0; i<outsideCorners.size(); i++) { centerToCorners.push_back(outsideCorners[i] - center); } //TODO: use CirclesGridFinder::getDirection float crossProduct = centerToCorners[0].x * centerToCorners[1].y - centerToCorners[0].y * centerToCorners[1].x; //y axis is inverted in computer vision so we check > 0 bool isClockwise = crossProduct > 0; firstCorner = isClockwise ? outsideCorners[1] : outsideCorners[0]; } else { firstCorner = corners[0]; } std::vector<Point2f>::const_iterator firstCornerIterator = std::find(hull2f.begin(), hull2f.end(), firstCorner); sortedCorners.clear(); for(std::vector<Point2f>::const_iterator it = firstCornerIterator; it != hull2f.end(); it++) { std::vector<Point2f>::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it); if(itCorners != corners.end()) { sortedCorners.push_back(*it); } } for(std::vector<Point2f>::const_iterator it = hull2f.begin(); it != firstCornerIterator; it++) { std::vector<Point2f>::const_iterator itCorners = std::find(corners.begin(), corners.end(), *it); if(itCorners != corners.end()) { sortedCorners.push_back(*it); } } if(!isAsymmetricGrid) { double dist1 = norm(sortedCorners[0] - sortedCorners[1]); double dist2 = norm(sortedCorners[1] - sortedCorners[2]); if((dist1 > dist2 && patternSize.height > patternSize.width) || (dist1 < dist2 && patternSize.height < patternSize.width)) { for(size_t i=0; i<sortedCorners.size()-1; i++) { sortedCorners[i] = sortedCorners[i+1]; } sortedCorners[sortedCorners.size() - 1] = firstCorner; } } } void CirclesGridClusterFinder::rectifyPatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &sortedCorners, std::vector<cv::Point2f> &rectifiedPatternPoints) { //indices of corner points in pattern std::vector<Point> trueIndices; trueIndices.push_back(Point(0, 0)); trueIndices.push_back(Point(patternSize.width - 1, 0)); if(isAsymmetricGrid) { 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)); std::vector<Point2f> idealPoints; for(size_t idx=0; idx<trueIndices.size(); idx++) { int i = trueIndices[idx].y; int j = trueIndices[idx].x; if(isAsymmetricGrid) { idealPoints.push_back(Point2f((2*j + i % 2)*squareSize, i*squareSize)); } else { idealPoints.push_back(Point2f(j*squareSize, i*squareSize)); } } Mat homography = findHomography(Mat(sortedCorners), Mat(idealPoints), 0); Mat rectifiedPointsMat; transform(patternPoints, rectifiedPointsMat, homography); rectifiedPatternPoints.clear(); convertPointsFromHomogeneous(rectifiedPointsMat, rectifiedPatternPoints); } void CirclesGridClusterFinder::parsePatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &rectifiedPatternPoints, std::vector<cv::Point2f> ¢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; if(isAsymmetricGrid) idealPt = Point2f((2*j + i % 2)*squareSize, i*squareSize); else idealPt = Point2f(j*squareSize, i*squareSize); Mat query(1, 2, CV_32F, &idealPt); const int knn = 1; int indicesbuf[knn] = {0}; float distsbuf[knn] = {0.f}; Mat indices(1, knn, CV_32S, &indicesbuf); Mat dists(1, knn, CV_32F, &distsbuf); flannIndex.knnSearch(query, indices, dists, knn, flann::SearchParams()); centers.push_back(patternPoints.at(indicesbuf[0])); if(distsbuf[0] > maxRectifiedDistance) { #ifdef DEBUG_CIRCLES cout << "Pattern not detected: too large rectified distance" << endl; #endif centers.clear(); return; } } } } Graph::Graph(size_t n) { for (size_t i = 0; i < n; i++) { addVertex(i); } } bool Graph::doesVertexExist(size_t id) const { return (vertices.find(id) != vertices.end()); } void Graph::addVertex(size_t id) { CV_Assert( !doesVertexExist( id ) ); vertices.insert(std::pair<size_t, Vertex> (id, Vertex())); } void Graph::addEdge(size_t id1, size_t id2) { CV_Assert( doesVertexExist( id1 ) ); CV_Assert( doesVertexExist( id2 ) ); vertices[id1].neighbors.insert(id2); vertices[id2].neighbors.insert(id1); } void Graph::removeEdge(size_t id1, size_t id2) { CV_Assert( doesVertexExist( id1 ) ); CV_Assert( doesVertexExist( id2 ) ); vertices[id1].neighbors.erase(id2); vertices[id2].neighbors.erase(id1); } bool Graph::areVerticesAdjacent(size_t id1, size_t id2) const { CV_Assert( doesVertexExist( id1 ) ); CV_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(size_t id) const { CV_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 int n = (int)getVerticesCount(); distanceMatrix.create(n, n, CV_32SC1); distanceMatrix.setTo(infinity); for (Vertices::const_iterator it1 = vertices.begin(); it1 != vertices.end(); it1++) { distanceMatrix.at<int> ((int)it1->first, (int)it1->first) = 0; for (Neighbors::const_iterator it2 = it1->second.neighbors.begin(); it2 != it1->second.neighbors.end(); it2++) { CV_Assert( it1->first != *it2 ); distanceMatrix.at<int> ((int)it1->first, (int)*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 i1 = (int)it1->first, i2 = (int)it2->first, i3 = (int)it3->first; int val1 = distanceMatrix.at<int> (i2, i3); int val2; if (distanceMatrix.at<int> (i2, i1) == infinity || distanceMatrix.at<int> (i1, i3) == infinity) val2 = val1; else { val2 = distanceMatrix.at<int> (i2, i1) + distanceMatrix.at<int> (i1, i3); } distanceMatrix.at<int> (i2, i3) = (val1 == infinity) ? val2 : std::min(val1, val2); } } } } const Graph::Neighbors& Graph::getNeighbors(size_t id) const { CV_Assert( doesVertexExist(id) ); Vertices::const_iterator it = vertices.find(id); return it->second.neighbors; } CirclesGridFinder::Segment::Segment(cv::Point2f _s, cv::Point2f _e) : s(_s), e(_e) { } void computeShortestPath(Mat &predecessorMatrix, int v1, int v2, std::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.1f; keypointScale = 1; minGraphConfidence = 9; vertexGain = 2; vertexPenalty = -5; edgeGain = 1; edgePenalty = -5; existingVertexGain = 0; minRNGEdgeSwitchDist = 5.f; gridType = SYMMETRIC_GRID; } CirclesGridFinder::CirclesGridFinder(Size _patternSize, const std::vector<Point2f> &testKeypoints, const CirclesGridFinderParameters &_parameters) : patternSize(static_cast<size_t> (_patternSize.width), static_cast<size_t> (_patternSize.height)) { CV_Assert(_patternSize.height >= 0 && _patternSize.width >= 0); keypoints = testKeypoints; parameters = _parameters; largeHoles = 0; smallHoles = 0; } bool CirclesGridFinder::findHoles() { switch (parameters.gridType) { case CirclesGridFinderParameters::SYMMETRIC_GRID: { std::vector<Point2f> vectors, filteredVectors, basis; Graph rng(0); computeRNG(rng, vectors); filterOutliersByDensity(vectors, filteredVectors); std::vector<Graph> basisGraphs; findBasis(filteredVectors, basis, basisGraphs); findMCS(basis, basisGraphs); break; } case CirclesGridFinderParameters::ASYMMETRIC_GRID: { std::vector<Point2f> vectors, tmpVectors, filteredVectors, basis; Graph rng(0); computeRNG(rng, tmpVectors); rng2gridGraph(rng, vectors); filterOutliersByDensity(vectors, filteredVectors); std::vector<Graph> basisGraphs; findBasis(filteredVectors, basis, basisGraphs); findMCS(basis, basisGraphs); eraseUsedGraph(basisGraphs); holes2 = holes; holes.clear(); findMCS(basis, basisGraphs); break; } default: CV_Error(Error::StsBadArg, "Unkown pattern type"); } return (isDetectionCorrect()); //CV_Error( 0, "Detection is not correct" ); } void CirclesGridFinder::rng2gridGraph(Graph &rng, std::vector<cv::Point2f> &vectors) const { for (size_t i = 0; i < rng.getVerticesCount(); i++) { Graph::Neighbors neighbors1 = rng.getNeighbors(i); for (Graph::Neighbors::iterator it1 = neighbors1.begin(); it1 != neighbors1.end(); it1++) { Graph::Neighbors neighbors2 = rng.getNeighbors(*it1); for (Graph::Neighbors::iterator it2 = neighbors2.begin(); it2 != neighbors2.end(); it2++) { if (i < *it2) { Point2f vec1 = keypoints[i] - keypoints[*it1]; Point2f vec2 = keypoints[*it1] - keypoints[*it2]; if (norm(vec1 - vec2) < parameters.minRNGEdgeSwitchDist || norm(vec1 + vec2) < parameters.minRNGEdgeSwitchDist) continue; vectors.push_back(keypoints[i] - keypoints[*it2]); vectors.push_back(keypoints[*it2] - keypoints[i]); } } } } } void CirclesGridFinder::eraseUsedGraph(std::vector<Graph> &basisGraphs) const { for (size_t i = 0; i < holes.size(); i++) { for (size_t j = 0; j < holes[i].size(); j++) { for (size_t k = 0; k < basisGraphs.size(); k++) { if (i != holes.size() - 1 && basisGraphs[k].areVerticesAdjacent(holes[i][j], holes[i + 1][j])) { basisGraphs[k].removeEdge(holes[i][j], holes[i + 1][j]); } if (j != holes[i].size() - 1 && basisGraphs[k].areVerticesAdjacent(holes[i][j], holes[i][j + 1])) { basisGraphs[k].removeEdge(holes[i][j], holes[i][j + 1]); } } } } } bool CirclesGridFinder::isDetectionCorrect() { switch (parameters.gridType) { case CirclesGridFinderParameters::SYMMETRIC_GRID: { if (holes.size() != patternSize.height) return false; std::set<size_t> 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(); } case CirclesGridFinderParameters::ASYMMETRIC_GRID: { if (holes.size() < holes2.size() || holes[0].size() < holes2[0].size()) { largeHoles = &holes2; smallHoles = &holes; } else { largeHoles = &holes; smallHoles = &holes2; } size_t largeWidth = patternSize.width; size_t largeHeight = (size_t)ceil(patternSize.height / 2.); size_t smallWidth = patternSize.width; size_t smallHeight = (size_t)floor(patternSize.height / 2.); size_t sw = smallWidth, sh = smallHeight, lw = largeWidth, lh = largeHeight; if (largeHoles->size() != largeHeight) { std::swap(lh, lw); } if (smallHoles->size() != smallHeight) { std::swap(sh, sw); } if (largeHoles->size() != lh || smallHoles->size() != sh) { return false; } std::set<size_t> vertices; for (size_t i = 0; i < largeHoles->size(); i++) { if (largeHoles->at(i).size() != lw) { return false; } for (size_t j = 0; j < largeHoles->at(i).size(); j++) { vertices.insert(largeHoles->at(i)[j]); } if (i < smallHoles->size()) { if (smallHoles->at(i).size() != sw) { return false; } for (size_t j = 0; j < smallHoles->at(i).size(); j++) { vertices.insert(smallHoles->at(i)[j]); } } } return (vertices.size() == largeHeight * largeWidth + smallHeight * smallWidth); } default: CV_Error(0, "Unknown pattern type"); } return false; } void CirclesGridFinder::findMCS(const std::vector<Point2f> &basis, std::vector<Graph> &basisGraphs) { holes.clear(); Path longestPath; size_t bestGraphIdx = findLongestPath(basisGraphs, longestPath); std::vector<size_t> 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); size_t w = holes[0].size(); size_t 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 (size_t 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 (size_t 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]); size_t w = holes[0].size(); size_t h = holes.size(); parameters.minGraphConfidence = holes.size() * parameters.existingVertexGain; for (size_t i = w; i < patternSize.width; i++) { addHolesByGraph(basisGraphs, false, basis[0]); } parameters.minGraphConfidence = holes[0].size() * parameters.existingVertexGain; for (size_t i = h; i < patternSize.height; i++) { addHolesByGraph(basisGraphs, true, basis[1]); } } } Mat CirclesGridFinder::rectifyGrid(Size detectedGridSize, const std::vector<Point2f>& centers, const std::vector<Point2f> &keypoints, std::vector<Point2f> &warpedKeypoints) { CV_Assert( !centers.empty() ); const float edgeLength = 30; const Point2f offset(150, 150); std::vector<Point2f> dstPoints; bool isClockwiseBefore = getDirection(centers[0], centers[detectedGridSize.width - 1], centers[centers.size() - 1]) < 0; int iStart = isClockwiseBefore ? 0 : detectedGridSize.height - 1; int iEnd = isClockwiseBefore ? detectedGridSize.height : -1; int iStep = isClockwiseBefore ? 1 : -1; for (int i = iStart; i != iEnd; i += iStep) { for (int j = 0; j < detectedGridSize.width; j++) { dstPoints.push_back(offset + Point2f(edgeLength * j, edgeLength * i)); } } Mat H = findHomography(Mat(centers), Mat(dstPoints), RANSAC); //Mat H = findHomography( Mat( corners ), Mat( dstPoints ) ); if (H.empty()) H = Mat::zeros(3, 3, CV_64FC1); std::vector<Point2f> srcKeypoints; for (size_t i = 0; i < keypoints.size(); i++) { srcKeypoints.push_back(keypoints[i]); } Mat dstKeypointsMat; transform(Mat(srcKeypoints), dstKeypointsMat, H); std::vector<Point2f> dstKeypoints; convertPointsFromHomogeneous(dstKeypointsMat, dstKeypoints); warpedKeypoints.clear(); for (size_t i = 0; i < dstKeypoints.size(); i++) { Point2f pt = dstKeypoints[i]; warpedKeypoints.push_back(pt); } return H; } size_t CirclesGridFinder::findNearestKeypoint(Point2f pt) const { size_t bestIdx = 0; double minDist = std::numeric_limits<double>::max(); for (size_t i = 0; i < keypoints.size(); i++) { double dist = norm(pt - keypoints[i]); if (dist < minDist) { minDist = dist; bestIdx = i; } } return bestIdx; } void CirclesGridFinder::addPoint(Point2f pt, std::vector<size_t> &points) { size_t ptIdx = findNearestKeypoint(pt); if (norm(keypoints[ptIdx] - pt) > parameters.minDistanceToAddKeypoint) { Point2f kpt = Point2f(pt); keypoints.push_back(kpt); points.push_back(keypoints.size() - 1); } else { points.push_back(ptIdx); } } void CirclesGridFinder::findCandidateLine(std::vector<size_t> &line, size_t seedLineIdx, bool addRow, Point2f basisVec, std::vector<size_t> &seeds) { line.clear(); seeds.clear(); if (addRow) { for (size_t i = 0; i < holes[seedLineIdx].size(); i++) { Point2f pt = keypoints[holes[seedLineIdx][i]] + 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]] + basisVec; addPoint(pt, line); seeds.push_back(holes[i][seedLineIdx]); } } CV_Assert( line.size() == seeds.size() ); } void CirclesGridFinder::findCandidateHoles(std::vector<size_t> &above, std::vector<size_t> &below, bool addRow, Point2f basisVec, std::vector<size_t> &aboveSeeds, std::vector<size_t> &belowSeeds) { above.clear(); below.clear(); aboveSeeds.clear(); belowSeeds.clear(); findCandidateLine(above, 0, addRow, -basisVec, aboveSeeds); size_t lastIdx = addRow ? holes.size() - 1 : holes[0].size() - 1; findCandidateLine(below, lastIdx, addRow, basisVec, belowSeeds); CV_Assert( below.size() == above.size() ); CV_Assert( belowSeeds.size() == aboveSeeds.size() ); CV_Assert( below.size() == belowSeeds.size() ); } bool CirclesGridFinder::areCentersNew(const std::vector<size_t> &newCenters, const std::vector<std::vector<size_t> > &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 std::vector<size_t> &above, const std::vector<size_t> &below, std::vector<std::vector<size_t> > &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]); } } } } float CirclesGridFinder::computeGraphConfidence(const std::vector<Graph> &basisGraphs, bool addRow, const std::vector<size_t> &points, const std::vector<size_t> &seeds) { CV_Assert( points.size() == seeds.size() ); float confidence = 0; const size_t vCount = basisGraphs[0].getVerticesCount(); CV_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 std::vector<Graph> &basisGraphs, bool addRow, Point2f basisVec) { std::vector<size_t> 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 std::vector<Point2f> &samples, std::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 std::vector<Point2f> &samples, std::vector<Point2f> &basis, std::vector<Graph> &basisGraphs) { basis.clear(); Mat bestLabels; TermCriteria termCriteria; Mat centers; const int clustersCount = 4; kmeans(Mat(samples).reshape(1, 0), clustersCount, bestLabels, termCriteria, parameters.kmeansAttempts, KMEANS_RANDOM_CENTERS, centers); CV_Assert( centers.type() == CV_32FC1 ); std::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" ); std::vector<std::vector<Point2f> > clusters(2), hulls(2); for (int k = 0; k < (int)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] - keypoints[j]; for (size_t k = 0; k < hulls.size(); k++) { if (pointPolygonTest(Mat(hulls[k]), vec, false) >= 0) { basisGraphs[k].addEdge(i, j); } } } } if (basisGraphs.size() != 2) CV_Error(0, "Number of basis graphs is not 2"); } void CirclesGridFinder::computeRNG(Graph &rng, std::vector<cv::Point2f> &vectors, Mat *drawImage) const { rng = Graph(keypoints.size()); 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] - keypoints[j]; double dist = norm(vec); bool isNeighbors = true; for (size_t k = 0; k < keypoints.size(); k++) { if (k == i || k == j) continue; double dist1 = norm(keypoints[i] - keypoints[k]); double dist2 = norm(keypoints[j] - keypoints[k]); if (dist1 < dist && dist2 < dist) { isNeighbors = false; break; } } if (isNeighbors) { rng.addEdge(i, j); vectors.push_back(keypoints[i] - keypoints[j]); if (drawImage != 0) { line(*drawImage, keypoints[i], keypoints[j], Scalar(255, 0, 0), 2); circle(*drawImage, keypoints[i], 3, Scalar(0, 0, 255), -1); circle(*drawImage, keypoints[j], 3, Scalar(0, 0, 255), -1); } } } } } void computePredecessorMatrix(const Mat &dm, int verticesCount, Mat &predecessorMatrix) { CV_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; } } } } } static void computeShortestPath(Mat &predecessorMatrix, size_t v1, size_t v2, std::vector<size_t> &path) { if (predecessorMatrix.at<int> ((int)v1, (int)v2) < 0) { path.push_back(v1); return; } computeShortestPath(predecessorMatrix, v1, predecessorMatrix.at<int> ((int)v1, (int)v2), path); path.push_back(v2); } size_t CirclesGridFinder::findLongestPath(std::vector<Graph> &basisGraphs, Path &bestPath) { std::vector<Path> longestPaths(1); std::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, (int)g.getVerticesCount(), predecessorMatrix); double maxVal; Point maxLoc; 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, cvRound(maxVal)); CV_Assert(maxLoc.x >= 0 && maxLoc.y >= 0) ; size_t id1 = static_cast<size_t> (maxLoc.x); size_t id2 = static_cast<size_t> (maxLoc.y); computeShortestPath(predecessorMatrix, id1, id2, path.vertices); longestPaths.push_back(path); int conf = 0; for (int v2 = 0; v2 < (int)path.vertices.size(); v2++) { conf += (int)basisGraphs[1 - (int)graphIdx].getDegree(v2); } confidences.push_back(conf); } } //if( bestGraphIdx != 0 ) //CV_Error( 0, "" ); int maxConf = -1; int bestPathIdx = -1; for (int i = 0; i < (int)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].x < keypoints[bestPath.firstVertex].x) || (bestGraphIdx == 1 && keypoints[bestPath.lastVertex].y < keypoints[bestPath.firstVertex].y); if (needReverse) { std::swap(bestPath.lastVertex, bestPath.firstVertex); std::reverse(bestPath.vertices.begin(), bestPath.vertices.end()); } return bestGraphIdx; } void CirclesGridFinder::drawBasis(const std::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, (double)(i * 255), 0), 2); } } void CirclesGridFinder::drawBasisGraphs(const std::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], keypoints[v2], edgeColor, edgeThickness); } } } } } if (drawVertices) { for (size_t v = 0; v < basisGraphs[0].getVerticesCount(); v++) { circle(drawImage, keypoints[v], 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, COLOR_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]], keypoints[holes[i][j + 1]], Scalar(255, 0, 0), 2); if (i != holes.size() - 1) line(drawImage, keypoints[holes[i][j]], keypoints[holes[i + 1][j]], Scalar(255, 0, 0), 2); //circle(drawImage, keypoints[holes[i][j]], holeRadius, holeColor, holeThickness); circle(drawImage, keypoints[holes[i][j]], holeRadius, holeColor, holeThickness); } } } Size CirclesGridFinder::getDetectedGridSize() const { if (holes.size() == 0) return Size(0, 0); return Size((int)holes[0].size(), (int)holes.size()); } void CirclesGridFinder::getHoles(std::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]]); } } } static bool areIndicesCorrect(Point pos, std::vector<std::vector<size_t> > *points) { if (pos.y < 0 || pos.x < 0) return false; return (static_cast<size_t> (pos.y) < points->size() && static_cast<size_t> (pos.x) < points->at(pos.y).size()); } void CirclesGridFinder::getAsymmetricHoles(std::vector<cv::Point2f> &outHoles) const { outHoles.clear(); std::vector<Point> largeCornerIndices, smallCornerIndices; std::vector<Point> firstSteps, secondSteps; size_t cornerIdx = getFirstCorner(largeCornerIndices, smallCornerIndices, firstSteps, secondSteps); CV_Assert(largeHoles != 0 && smallHoles != 0) ; Point srcLargePos = largeCornerIndices[cornerIdx]; Point srcSmallPos = smallCornerIndices[cornerIdx]; while (areIndicesCorrect(srcLargePos, largeHoles) || areIndicesCorrect(srcSmallPos, smallHoles)) { Point largePos = srcLargePos; while (areIndicesCorrect(largePos, largeHoles)) { outHoles.push_back(keypoints[largeHoles->at(largePos.y)[largePos.x]]); largePos += firstSteps[cornerIdx]; } srcLargePos += secondSteps[cornerIdx]; Point smallPos = srcSmallPos; while (areIndicesCorrect(smallPos, smallHoles)) { outHoles.push_back(keypoints[smallHoles->at(smallPos.y)[smallPos.x]]); smallPos += firstSteps[cornerIdx]; } srcSmallPos += secondSteps[cornerIdx]; } } double CirclesGridFinder::getDirection(Point2f p1, Point2f p2, Point2f p3) { Point2f a = p3 - p1; Point2f b = p2 - p1; return a.x * b.y - a.y * b.x; } bool CirclesGridFinder::areSegmentsIntersecting(Segment seg1, Segment seg2) { bool doesStraddle1 = (getDirection(seg2.s, seg2.e, seg1.s) * getDirection(seg2.s, seg2.e, seg1.e)) < 0; bool doesStraddle2 = (getDirection(seg1.s, seg1.e, seg2.s) * getDirection(seg1.s, seg1.e, seg2.e)) < 0; return doesStraddle1 && doesStraddle2; /* Point2f t1 = e1-s1; Point2f n1(t1.y, -t1.x); double c1 = -n1.ddot(s1); Point2f t2 = e2-s2; Point2f n2(t2.y, -t2.x); double c2 = -n2.ddot(s2); bool seg1 = ((n1.ddot(s2) + c1) * (n1.ddot(e2) + c1)) <= 0; bool seg1 = ((n2.ddot(s1) + c2) * (n2.ddot(e1) + c2)) <= 0; return seg1 && seg2; */ } void CirclesGridFinder::getCornerSegments(const std::vector<std::vector<size_t> > &points, std::vector<std::vector<Segment> > &segments, std::vector<Point> &cornerIndices, std::vector<Point> &firstSteps, std::vector<Point> &secondSteps) const { segments.clear(); cornerIndices.clear(); firstSteps.clear(); secondSteps.clear(); int h = (int)points.size(); int w = (int)points[0].size(); CV_Assert(h >= 2 && w >= 2) ; //all 8 segments with one end in a corner std::vector<Segment> corner; corner.push_back(Segment(keypoints[points[1][0]], keypoints[points[0][0]])); corner.push_back(Segment(keypoints[points[0][0]], keypoints[points[0][1]])); segments.push_back(corner); cornerIndices.push_back(Point(0, 0)); firstSteps.push_back(Point(1, 0)); secondSteps.push_back(Point(0, 1)); corner.clear(); corner.push_back(Segment(keypoints[points[0][w - 2]], keypoints[points[0][w - 1]])); corner.push_back(Segment(keypoints[points[0][w - 1]], keypoints[points[1][w - 1]])); segments.push_back(corner); cornerIndices.push_back(Point(w - 1, 0)); firstSteps.push_back(Point(0, 1)); secondSteps.push_back(Point(-1, 0)); corner.clear(); corner.push_back(Segment(keypoints[points[h - 2][w - 1]], keypoints[points[h - 1][w - 1]])); corner.push_back(Segment(keypoints[points[h - 1][w - 1]], keypoints[points[h - 1][w - 2]])); segments.push_back(corner); cornerIndices.push_back(Point(w - 1, h - 1)); firstSteps.push_back(Point(-1, 0)); secondSteps.push_back(Point(0, -1)); corner.clear(); corner.push_back(Segment(keypoints[points[h - 1][1]], keypoints[points[h - 1][0]])); corner.push_back(Segment(keypoints[points[h - 1][0]], keypoints[points[h - 2][0]])); cornerIndices.push_back(Point(0, h - 1)); firstSteps.push_back(Point(0, -1)); secondSteps.push_back(Point(1, 0)); segments.push_back(corner); corner.clear(); //y axis is inverted in computer vision so we check < 0 bool isClockwise = getDirection(keypoints[points[0][0]], keypoints[points[0][w - 1]], keypoints[points[h - 1][w - 1]]) < 0; if (!isClockwise) { #ifdef DEBUG_CIRCLES cout << "Corners are counterclockwise" << endl; #endif std::reverse(segments.begin(), segments.end()); std::reverse(cornerIndices.begin(), cornerIndices.end()); std::reverse(firstSteps.begin(), firstSteps.end()); std::reverse(secondSteps.begin(), secondSteps.end()); std::swap(firstSteps, secondSteps); } } bool CirclesGridFinder::doesIntersectionExist(const std::vector<Segment> &corner, const std::vector<std::vector<Segment> > &segments) { for (size_t i = 0; i < corner.size(); i++) { for (size_t j = 0; j < segments.size(); j++) { for (size_t k = 0; k < segments[j].size(); k++) { if (areSegmentsIntersecting(corner[i], segments[j][k])) return true; } } } return false; } size_t CirclesGridFinder::getFirstCorner(std::vector<Point> &largeCornerIndices, std::vector<Point> &smallCornerIndices, std::vector< Point> &firstSteps, std::vector<Point> &secondSteps) const { std::vector<std::vector<Segment> > largeSegments; std::vector<std::vector<Segment> > smallSegments; getCornerSegments(*largeHoles, largeSegments, largeCornerIndices, firstSteps, secondSteps); getCornerSegments(*smallHoles, smallSegments, smallCornerIndices, firstSteps, secondSteps); const size_t cornersCount = 4; CV_Assert(largeSegments.size() == cornersCount) ; bool isInsider[cornersCount]; for (size_t i = 0; i < cornersCount; i++) { isInsider[i] = doesIntersectionExist(largeSegments[i], smallSegments); } int cornerIdx = 0; bool waitOutsider = true; for(;;) { if (waitOutsider) { if (!isInsider[(cornerIdx + 1) % cornersCount]) waitOutsider = false; } else { if (isInsider[(cornerIdx + 1) % cornersCount]) break; } cornerIdx = (cornerIdx + 1) % cornersCount; } return cornerIdx; }