PsychCV('AprilDetectMarkers'): Make sure to choose best candidate for…

… each marker.

If the same Apriltag marker id is detected multiple times in an image by
the apriltag library, e.g., due to some false positive detections, we only
return one detection as marker of that id. As opposed to the past, when
we just returned the first detected candidate of a certain id (first come,
first served) instead return the candidate detection with the "highest
quality".

Best candidate detection is defined as the one with the lowest hamming bit
error, and in case of multiple candidates with same hamming bit error the
one with the highest decision margin (aka "score").

Practical testing showed that this greatly improves stability of marker
detection in a real use case.

PsychSourceGL/Source/Common/PsychCV/PsychCVAprilTags.c

@@ -621,8 +621,25 @@ PsychError PSYCHCVAprilDetectMarkers(void)
         detected = NULL;
         for (j = 0; j < marker_num; j++) {
             zarray_get(marker_info, j, &detcand);
-            if ((candHandle == detcand->id) && (detected == NULL))
-                detected = detcand;
+            if (candHandle == detcand->id) {
+                // detcand is a candidate for marker with id candHandle:
+                if (detected == NULL) {
+                    // First candidate, assign as best choice so far:
+                    detected = detcand;
+                }
+                else {
+                    // Already got a choice. Lets see if the new candidate is a better
+                    // contender for the spot, ie. if the new 'detcand' is a higher quality
+                    // detection than the previous 'detected' candidate. If so, we make 'detcand'
+                    // our new favorite. Criteria for better: Lower Hamming bit error, or if hamming
+                    // error is the same, a higher decision margin:
+                    if ((detected->hamming > detcand->hamming) ||
+                        ((detected->hamming == detcand->hamming) && (detected->decision_margin < detcand->decision_margin))) {
+                        // A new champ! Assign:
+                        detected = detcand;
+                    }
+                }
+            }
         }
 
         matchQuality = 0;