CircleGridClusterFinder: map circle pattern width and height correctly

During the cluster-based detection of circle grids, the detected circle
pattern has to be mapped to 3D-points. When doing this the width (i.e.
more circles) and height (i.e. less circles) of the pattern need to
be identified in image coordinates.

Until now this was done by assuming that the shorter side in image
coordinates (length in pixels) corresponds to the height in 3D.
This assumption does not hold if we look at the pattern from
a perspective where the projection of the width is shorter
than the projection of the height. This in turn lead to misdetections in
although the circle pattern was clearly visible.

Instead count how many circles have been detected along two edges of the
projected quadrangle and use the one with more circles as width and the
one with less as height.

modules/calib3d/src/circlesgrid.cpp

@@ -178,7 +178,7 @@ void CirclesGridClusterFinder::findGrid(const std::vector<cv::Point2f> &points,
     if(outsideCorners.size() != outsideCornersCount)
       return;
   }
-  getSortedCorners(hull2f, corners, outsideCorners, sortedCorners);
+  getSortedCorners(hull2f, patternPoints, corners, outsideCorners, sortedCorners);
   if(sortedCorners.size() != cornersCount)
     return;
 
@@ -295,7 +295,18 @@ void CirclesGridClusterFinder::findOutsideCorners(const std::vector<cv::Point2f>
 #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)
+namespace {
+double pointLineDistance(const cv::Point2f &p, const cv::Vec4f &line)
+{
+  Vec3f pa( line[0], line[1], 1 );
+  Vec3f pb( line[2], line[3], 1 );
+  Vec3f l = pa.cross(pb);
+  return std::abs((p.x * l[0] + p.y * l[1] + l[2])) * 1.0 /
+         std::sqrt(double(l[0] * l[0] + l[1] * l[1]));
+}
+}
+
+void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners)
 {
   Point2f firstCorner;
   if(isAsymmetricGrid)
@@ -341,10 +352,26 @@ void CirclesGridClusterFinder::getSortedCorners(const std::vector<cv::Point2f> &
 
   if(!isAsymmetricGrid)
   {
-    double dist1 = norm(sortedCorners[0] - sortedCorners[1]);
-    double dist2 = norm(sortedCorners[1] - sortedCorners[2]);
+    double dist01 = norm(sortedCorners[0] - sortedCorners[1]);
+    double dist12 = norm(sortedCorners[1] - sortedCorners[2]);
+    // Use half the average distance between circles on the shorter side as threshold for determining whether a point lies on an edge.
+    double thresh = min(dist01, dist12) / min(patternSize.width, patternSize.height) / 2;
+
+    size_t circleCount01 = 0;
+    size_t circleCount12 = 0;
+    Vec4f line01( sortedCorners[0].x, sortedCorners[0].y, sortedCorners[1].x, sortedCorners[1].y );
+    Vec4f line12( sortedCorners[1].x, sortedCorners[1].y, sortedCorners[2].x, sortedCorners[2].y );
+    // Count the circles along both edges.
+    for (size_t i = 0; i < patternPoints.size(); i++)
+    {
+      if (pointLineDistance(patternPoints[i], line01) < thresh)
+        circleCount01++;
+      if (pointLineDistance(patternPoints[i], line12) < thresh)
+        circleCount12++;
+    }
 
-    if((dist1 > dist2 && patternSize.height > patternSize.width) || (dist1 < dist2 && patternSize.height < patternSize.width))
+    // Ensure that the edge from sortedCorners[0] to sortedCorners[1] is the one with more circles (i.e. it is interpreted as the pattern's width).
+    if ((circleCount01 > circleCount12 && patternSize.height > patternSize.width) || (circleCount01 < circleCount12 && patternSize.height < patternSize.width))
     {
       for(size_t i=0; i<sortedCorners.size()-1; i++)
       {

modules/calib3d/src/circlesgrid.hpp

@@ -69,7 +69,7 @@ public:
 private:
   void findCorners(const std::vector<cv::Point2f> &hull2f, std::vector<cv::Point2f> &corners);
   void findOutsideCorners(const std::vector<cv::Point2f> &corners, std::vector<cv::Point2f> &outsideCorners);
-  void getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners);
+  void getSortedCorners(const std::vector<cv::Point2f> &hull2f, const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &corners, const std::vector<cv::Point2f> &outsideCorners, std::vector<cv::Point2f> &sortedCorners);
   void rectifyPatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &sortedCorners, std::vector<cv::Point2f> &rectifiedPatternPoints);
   void parsePatternPoints(const std::vector<cv::Point2f> &patternPoints, const std::vector<cv::Point2f> &rectifiedPatternPoints, std::vector<cv::Point2f> &centers);