Fix used corners being returned from mrcal and persist corner ids

build.gradle

@@ -44,7 +44,7 @@ ext {
     rknnVersion = "v2027.0.0"
     tfliteVersion = "v2027.0.2-alpha-1"
     wpilibYear = "2027_alpha5"
-    mrcalVersion = "dev-v2027.0.2-6-gb65c352";
+    mrcalVersion = "dev-v2027.0.2-10-gba8e332";
 
     pubVersion = versionString
     isDev = pubVersion.startsWith("dev")

photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java

@@ -48,6 +48,9 @@ public final class BoardObservation implements Cloneable {
     // Solver optimized board poses
     public Pose3d optimisedCameraToObject;
 
+    // Ids of each corner since ChArUco may produce partial observations
+    public int[] cornerIds;
+
     // If we should use this observation when re-calculating camera calibration
     public boolean[] cornersUsed;
 
@@ -60,13 +63,15 @@ public BoardObservation(
             List<Point> locationInImageSpace,
             List<Point> reprojectionErrors,
             Pose3d optimisedCameraToObject,
+            int[] cornerIds,
             boolean[] cornersUsed,
             String snapshotName,
             Path snapshotDataLocation) {
         this.locationInObjectSpace = locationInObjectSpace;
         this.locationInImageSpace = locationInImageSpace;
         this.reprojectionErrors = reprojectionErrors;
         this.optimisedCameraToObject = optimisedCameraToObject;
+        this.cornerIds = cornerIds;
         this.snapshotName = snapshotName;
         this.snapshotDataLocation = snapshotDataLocation;
 
@@ -88,6 +93,8 @@ public String toString() {
                 + reprojectionErrors
                 + ", optimisedCameraToObject="
                 + optimisedCameraToObject
+                + ", cornerIds="
+                + cornerIds
                 + ", cornersUsed="
                 + cornersUsed
                 + ", snapshotName="

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java

@@ -21,7 +21,6 @@
 import java.nio.file.Paths;
 import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.Iterator;
 import java.util.List;
 import org.apache.commons.io.FileUtils;
 import org.opencv.calib3d.Calib3d;
@@ -221,27 +220,26 @@ protected CameraCalibrationCoefficients calibrateMrcal(
             List<FindBoardCornersPipe.FindBoardCornersPipeResult> observationCorners,
             FrameStaticProperties imageProps,
             Path imageSavePath) {
-        Iterator<MrCalObservation> observationData =
+        List<MrCalObservation> observationData =
                 observationCorners.stream()
                         .map(
-                                it -> {
-                                    var corners = it.imagePoints.toArray();
+                                observation -> {
+                                    var corners = observation.imagePoints.toArray();
 
-                                    var levels = new float[(int) it.imagePoints.total()];
-                                    Arrays.fill(levels, it.level);
+                                    var levels = new float[(int) observation.imagePoints.total()];
+                                    Arrays.fill(levels, observation.level);
 
-                                    var ids = it.ids != null ? it.ids.toArray() : null;
+                                    var ids = observation.ids != null ? observation.ids.toArray() : null;
 
                                     return new MrCalObservation(corners, levels, ids);
                                 })
-                        .iterator();
+                        .toList();
 
         int imageWidth = (int) observationCorners.get(0).size.width;
         int imageHeight = (int) observationCorners.get(0).size.height;
 
         MrCalResult result =
                 MrCalJNI.calibrateCamera(
-                        observationCorners.size(),
                         observationData,
                         params.boardWidth,
                         params.boardHeight,
@@ -428,12 +426,16 @@ private List<BoardObservation> createObservations(
                 Imgcodecs.imwrite(image_path.toString(), inputImage);
             }
 
+            var cornerIdsMat = observationData.get(snapshotId).ids;
+            var cornerIds = cornerIdsMat != null ? cornerIdsMat.toArray() : null;
+
             observations.add(
                     new BoardObservation(
                             objectPoints.toList(),
                             iPoints,
                             reprojectionError,
                             camToBoard,
+                            cornerIds,
                             cornersUsed.get(snapshotId),
                             snapshotName,
                             image_path));

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java

@@ -18,6 +18,7 @@
 package org.photonvision.vision.pipe.impl;
 
 import java.util.ArrayList;
+import java.util.Collections;
 import java.util.List;
 import java.util.Optional;
 import org.opencv.calib3d.Calib3d;
@@ -350,6 +351,39 @@ private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
                 // Decimation was not used
                 level = 0.0f;
 
+                if (ids.rows() == params.boardWidth * params.boardHeight) {
+                    // All points are present, so ids are unnecessary
+                    // Ensure points are ordered by id, which OpenCV should guarantee
+                    int currentId = 0;
+                    // Scan for out of place elements first to determine if data conversion is necessary
+                    for (; currentId < ids.rows(); currentId++) {
+                        if (ids.at(int.class, currentId, 0).getV() != currentId) {
+                            break;
+                        }
+                    }
+                    if (currentId < ids.rows()) {
+                        // Continue sorting from end of scan if it did not complete
+                        var objectPointsList = objectPoints.toList();
+                        var imagePointsList = imagePoints.toList();
+                        var idsList = ids.toList();
+                        while (currentId < idsList.size()) {
+                            // While current id is not equal to current index, swap with the correct index
+                            if (idsList.get(currentId) != currentId) {
+                                Collections.swap(objectPointsList, idsList.get(currentId), currentId);
+                                Collections.swap(imagePointsList, idsList.get(currentId), currentId);
+                                Collections.swap(idsList, idsList.get(currentId), currentId);
+                            } else {
+                                currentId++;
+                            }
+                        }
+                        logger.debug("Sorted ids: " + idsList);
+                        objectPoints.fromList(objectPointsList);
+                        imagePoints.fromList(imagePointsList);
+                    }
+                    ids.release();
+                    ids = null;
+                }
+
                 break;
             case CHESSBOARD:
                 // Reduce the image size to be much more manageable