From 068b6c700e8990d499715f63d685e712ad0dd8b2 Mon Sep 17 00:00:00 2001
From: Jan-Kristian Herring <jan-kristian.herring@varjo.com>
Date: Wed, 24 Mar 2021 15:23:30 +0200
Subject: [PATCH] Fix aruco _filterTooCloseCandidates

_filterTooCloseCandidates was occasionally not choosing the biggest
contour of a candidate group.

Firstly, if the biggest contour of a candidate group happened to be
the first candidate of that group, it would never be chosen due to the
comparison loop skipping it.

Secondly, contour sizes were being compared by their circumference
(via counting the number of points in a contour) rather than area.
In some cases it is possible for a smaller candidate to have more
points than a bigger one. This was fixed by comparing the contour
area as defined by the contour's corners.
---
 modules/aruco/src/aruco.cpp | 42 +++++++++++++++++++------------------
 1 file changed, 22 insertions(+), 20 deletions(-)

diff --git a/modules/aruco/src/aruco.cpp b/modules/aruco/src/aruco.cpp
index 4d8f2b85f01..ca515bb3515 100644
--- a/modules/aruco/src/aruco.cpp
+++ b/modules/aruco/src/aruco.cpp
@@ -306,34 +306,36 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida
     vector< vector< Point > > smallerContours;
 
     // save possible candidates
-    for( unsigned int i = 0; i < groupedCandidates.size(); i++ ) {
-        int smallerIdx = groupedCandidates[i][0];
-        int biggerIdx = -1;
+    for(unsigned int i = 0; i < groupedCandidates.size(); i++) {
+        unsigned int smallerIdx = groupedCandidates[i][0];
+        unsigned int biggerIdx = smallerIdx;
+        double smallerArea = contourArea(candidatesIn[smallerIdx]);
+        double biggerArea = smallerArea;
 
         // evaluate group elements
-        for( unsigned int j = 1; j < groupedCandidates[i].size(); j++ ) {
-            size_t currPerim = contoursIn[ groupedCandidates[i][j] ].size();
+        for(unsigned int j = 1; j < groupedCandidates[i].size(); j++) {
+            unsigned int currIdx = groupedCandidates[i][j];
+            double currArea = contourArea(candidatesIn[currIdx]);
 
             // check if current contour is bigger
-            if ( biggerIdx < 0 )
-                biggerIdx = groupedCandidates[i][j];
-            else if(currPerim >= contoursIn[ biggerIdx ].size())
-                biggerIdx = groupedCandidates[i][j];
+            if(currArea >= biggerArea) {
+                biggerIdx = currIdx;
+                biggerArea = currArea;
+            }
 
             // check if current contour is smaller
-            if(currPerim < contoursIn[ smallerIdx ].size() && detectInvertedMarker)
-                smallerIdx = groupedCandidates[i][j];
+            if(currArea < smallerArea && detectInvertedMarker) {
+                smallerIdx = currIdx;
+                smallerArea = currArea;
+            }
         }
-        // add contours und candidates
-        if(biggerIdx > -1){
 
-            biggerCandidates.push_back(candidatesIn[biggerIdx]);
-            biggerContours.push_back(contoursIn[biggerIdx]);
-
-            if( detectInvertedMarker ){
-                smallerCandidates.push_back(alignContourOrder(candidatesIn[biggerIdx][0], candidatesIn[smallerIdx]));
-                smallerContours.push_back(contoursIn[smallerIdx]);
-            }
+        // add contours and candidates
+        biggerCandidates.push_back(candidatesIn[biggerIdx]);
+        biggerContours.push_back(contoursIn[biggerIdx]);
+        if(detectInvertedMarker) {
+            smallerCandidates.push_back(alignContourOrder(candidatesIn[biggerIdx][0], candidatesIn[smallerIdx]));
+            smallerContours.push_back(contoursIn[smallerIdx]);
         }
     }
     // to preserve the structure :: candidateSet< defaultCandidates, whiteCandidates >