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,30 @@ 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();
+                        sortRelatedSubranges(idsList, List.of(objectPointsList, imagePointsList), 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
@@ -398,6 +423,23 @@ private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
         return new FindBoardCornersPipeResult(inFrame.size(), objectPoints, imagePoints, level, ids);
     }
 
+    public static void sortRelatedSubranges(
+            List<Integer> positions, List<List<?>> others, int startIndex) {
+        int currentIndex = startIndex;
+        while (currentIndex < positions.size()) {
+            /* While current position is not equal to current index, swap the current index's position with
+            the current position */
+            if (positions.get(currentIndex) != currentIndex) {
+                for (var other : others) {
+                    Collections.swap(other, positions.get(currentIndex), currentIndex);
+                }
+                Collections.swap(positions, positions.get(currentIndex), currentIndex);
+            } else {
+                currentIndex++;
+            }
+        }
+    }
+
     @Override
     public void release() {
         objectPointsTemplate.ifPresent(mat -> mat.release());

photon-core/src/test/java/org/photonvision/vision/pipeline/FindBoardCornersPipeTest.java

@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
+
+public class FindBoardCornersPipeTest {
+    @Test
+    void testSorting() {
+        List<Integer> unsorted = new ArrayList<>(List.of(0, 1, 2, 3, 6, 8, 5, 4, 9, 7));
+        List<Boolean> evenness =
+                new ArrayList<>(List.of(true, false, true, false, true, true, false, true, false, false));
+        List<Integer> sorted = new ArrayList<>(unsorted);
+        Collections.sort(sorted);
+
+        // This function expects that ids are a continuous range from 0 to n, already sorted prior to
+        // the start index
+        FindBoardCornersPipe.sortRelatedSubranges(unsorted, List.of(evenness), 4);
+
+        assertEquals(sorted, unsorted);
+        assertTrue(sorted.stream().allMatch(id -> evenness.get(id) == (id % 2 == 0)));
+    }
+}