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 index 7cc1361015..d3141d5436 100644 --- a/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java +++ b/photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java @@ -19,6 +19,7 @@ import java.util.ArrayList; import java.util.Arrays; +import java.util.Comparator; import org.opencv.core.Mat; import org.opencv.objdetect.ArucoDetector; import org.opencv.objdetect.DetectorParameters; @@ -33,9 +34,8 @@ public class PhotonArucoDetector { private final ArucoDetector detector = new ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5)); - private Mat ids = new Mat(); - private ArrayList cornerMats = new ArrayList(); - private Mat cornerMat; + private final Mat ids = new Mat(); + private final ArrayList cornerMats = new ArrayList<>(); public ArucoDetector getDetector() { return detector; @@ -64,10 +64,10 @@ public ArucoDetectionResult[] detect(Mat grayscaleImg) { // detect tags detector.detectMarkers(grayscaleImg, cornerMats, ids); - ArucoDetectionResult[] toReturn = new ArucoDetectionResult[cornerMats.size()]; + ArucoDetectionResult[] results = new ArucoDetectionResult[cornerMats.size()]; for (int i = 0; i < cornerMats.size(); i++) { // each detection has a Mat of corners - cornerMat = cornerMats.get(i); + Mat cornerMat = cornerMats.get(i); // Aruco detection returns corners (BR, BL, TL, TR). // For parity with AprilTags and photonlib, we want (BL, BR, TR, TL). @@ -85,22 +85,17 @@ public ArucoDetectionResult[] detect(Mat grayscaleImg) { }; cornerMat.release(); - toReturn[i] = new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0]); + results[i] = new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0]); } 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; - }); + results, + Comparator.comparingInt(ArucoDetectionResult::getId) + ); - return toReturn; + return results; } }