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

@@ -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 >