From 52ea8a3218b35985dfc37d8811165f0bd6d2d0ba Mon Sep 17 00:00:00 2001
From: Mario Kleiner <mario.kleiner.de@gmail.com>
Date: Fri, 12 Apr 2024 23:42:45 +0100
Subject: [PATCH] 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.
---
 .../Source/Common/PsychCV/PsychCVAprilTags.c  | 21 +++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/PsychSourceGL/Source/Common/PsychCV/PsychCVAprilTags.c b/PsychSourceGL/Source/Common/PsychCV/PsychCVAprilTags.c
index a188f7c80..152674b6c 100644
--- a/PsychSourceGL/Source/Common/PsychCV/PsychCVAprilTags.c
+++ b/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;