Return partial used corners for calibration

src/main/java/org/photonvision/mrcal/MrCalJNI.java

@@ -27,12 +27,12 @@
 import org.wpilib.math.geometry.Translation3d;
 import org.wpilib.math.linalg.VecBuilder;
 
 public class MrCalJNI {
     public static enum CameraLensModel {
         LENSMODEL_OPENCV5,
         LENSMODEL_OPENCV8,
         LENSMODEL_STEREOGRAPHIC,
         LENSMODEL_SPLINED_STEREOGRAPHIC;
     }
 
     /**
@@ -57,7 +57,9 @@
         }
     }
 
+    protected static record ObservationInfo(double[] data, int[][] ids) {}
+
     public static class MrCalResult {
         public boolean success;
         public double[] intrinsics;
         public double rms_error;
@@ -66,7 +68,7 @@
         public double warp_y;
         public int Noutliers;
         public List<Pose3d> optimizedPoses;
         public List<boolean[]> cornersUsed;
 
         public MrCalResult(boolean success) {
             this.success = success;
@@ -278,12 +280,13 @@
      * present. Levels will be converted to weights using weight = 0.5^level, as explained
      * [here](https://github.com/dkogan/mrcal/blob/7cd9ac4c854a4b244a35f554c9ebd0464d59e9ff/mrcal-calibrate-cameras#L152)
      */
-    private static double[] makeObservations(
+    private static ObservationInfo makeObservations(
             int observationCount,
             Iterator<MrCalObservation> observationData,
             int boardWidth,
             int boardHeight) {
         double[] observations = new double[boardWidth * boardHeight * 3 * observationCount];
+        int[][] all_ids = new int[observationCount][];
         Arrays.fill(observations, -1.0);
 
         for (int b = 0; b < observationCount; b++) {
@@ -297,6 +300,7 @@
             final var corners = observation.corners;
             final var levels = observation.levels;
             final var ids = observation.ids;
+            all_ids[b] = ids;
 
             if (ids == null) {
                 // No ids, assume a full rectangular board
@@ -347,7 +351,7 @@
             return null;
         }
 
-        return observations;
+        return new ObservationInfo(observations, all_ids);
     }
 
     /**
@@ -380,7 +384,28 @@
 
         var observations = makeObservations(observationCount, observationData, boardWidth, boardHeight);
 
-        return mrcal_calibrate_camera(
-                observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, focalLen);
+        var results =
+                mrcal_calibrate_camera(
+                        observations.data,
+                        boardWidth,
+                        boardHeight,
+                        boardSpacing,
+                        imageWidth,
+                        imageHeight,
+                        focalLen);
+
+        // Only return corners used for the corners that were provided in the input
+        for (int b = 0; b < observations.ids.length; b++) {
+            if (observations.ids[b] != null) {
+                boolean[] fullCorners = results.cornersUsed.get(b);
+                boolean[] partialCorners = new boolean[observations.ids[b].length];
+                for (int i = 0; i < observations.ids[b].length; i++) {
+                    partialCorners[i] = fullCorners[observations.ids[b][i]];
+                }
+                results.cornersUsed.set(b, partialCorners);
+            }
+        }
+
+        return results;
     }
 }