sort tags by ID

PhotonVision · Oct 20, 2023 · dc14106 · dc14106
1 parent a163ac2
commit dc14106
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java
@@ -18,6 +18,7 @@
 package org.photonvision.vision.aruco;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import org.opencv.core.Mat;
 import org.opencv.objdetect.ArucoDetector;
 import org.opencv.objdetect.DetectorParameters;
@@ -89,6 +90,17 @@ public ArucoDetectionResult[] detect(Mat grayscaleImg) {
 
         ids.release();
 
+        // sort tags by ID
+        Arrays.sort(
+                toReturn,
+                (ArucoDetectionResult a, ArucoDetectionResult b) -> {
+                    if (a.getId() > b.getId()) {
+                        return 1;
+                    } else if (a.getId() < b.getId()) {
+                        return -1;
+                    } else return 0;
+                });
+
         return toReturn;
     }
 }