From bdafd316bc18593572c569a413feab042d5ddf77 Mon Sep 17 00:00:00 2001 From: thatcomputerguy0101 Date: Wed, 29 Jul 2026 12:43:32 -0400 Subject: [PATCH 1/4] Return partial used corners for calibration --- .../java/org/photonvision/mrcal/MrCalJNI.java | 33 ++++++++++++++++--- 1 file changed, 29 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index f5c8dd1..2681448 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -57,6 +57,8 @@ public MrCalObservation(Point[] corners, float[] levels) { } } + protected static record ObservationInfo(double[] data, int[][] ids) {} + public static class MrCalResult { public boolean success; public double[] intrinsics; @@ -278,12 +280,13 @@ public static native double[] compute_uncertainty( * 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 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 @@ private static double[] makeObservations( 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 @@ private static double[] makeObservations( return null; } - return observations; + return new ObservationInfo(observations, all_ids); } /** @@ -380,7 +384,28 @@ public static MrCalResult calibrateCamera( 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; } } From 5674c20230f521afa59f362147e63e6fec66eea8 Mon Sep 17 00:00:00 2001 From: thatcomputerguy0101 Date: Thu, 30 Jul 2026 13:17:21 -0400 Subject: [PATCH 2/4] Switch to list of observation data --- .../java/org/photonvision/mrcal/MrCalJNI.java | 60 +++++++------------ 1 file changed, 22 insertions(+), 38 deletions(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index 2681448..2591466 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -19,7 +19,6 @@ import java.util.ArrayList; import java.util.Arrays; -import java.util.Iterator; import java.util.List; import org.opencv.core.Point; import org.wpilib.math.geometry.Pose3d; @@ -281,21 +280,13 @@ public static native double[] compute_uncertainty( * [here](https://github.com/dkogan/mrcal/blob/7cd9ac4c854a4b244a35f554c9ebd0464d59e9ff/mrcal-calibrate-cameras#L152) */ private static ObservationInfo makeObservations( - int observationCount, - Iterator 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++) { - if (!observationData.hasNext()) { - // Too few observations - return null; - } + List observations, int boardWidth, int boardHeight) { + double[] packedObservations = new double[boardWidth * boardHeight * 3 * observations.size()]; + int[][] all_ids = new int[observations.size()][]; + Arrays.fill(packedObservations, -1.0); - final var observation = observationData.next(); + for (int b = 0; b < observations.size(); b++) { + final var observation = observations.get(b); final var corners = observation.corners; final var levels = observation.levels; @@ -315,9 +306,9 @@ private static ObservationInfo makeObservations( int i = boardWidth * boardHeight * b + c; - observations[i * 3 + 0] = corner.x; - observations[i * 3 + 1] = corner.y; - observations[i * 3 + 2] = level; + packedObservations[i * 3 + 0] = corner.x; + packedObservations[i * 3 + 1] = corner.y; + packedObservations[i * 3 + 2] = level; } } else { // Ids present, some corners may be missing @@ -339,19 +330,14 @@ private static ObservationInfo makeObservations( int i = boardWidth * boardHeight * b + id; - observations[i * 3 + 0] = corner.x; - observations[i * 3 + 1] = corner.y; - observations[i * 3 + 2] = level; + packedObservations[i * 3 + 0] = corner.x; + packedObservations[i * 3 + 1] = corner.y; + packedObservations[i * 3 + 2] = level; } } } - if (observationData.hasNext()) { - // Too many observations - return null; - } - - return new ObservationInfo(observations, all_ids); + return new ObservationInfo(packedObservations, all_ids); } /** @@ -361,8 +347,7 @@ private static ObservationInfo makeObservations( * then calls {@link #mrcal_calibrate_camera} to perform calibration. Each corner's detection * level is converted to a weight (0.5^level), and negative levels indicate undetected corners. * - * @param observationCount Number of observations in `observationData` - * @param observationData An iterator of observations, each containing a list of corner locations, + * @param observations An list of observations, each containing a list of corner locations, * decimation levels, and optional corner ids * @param boardWidth Number of internal corners horizontally * @param boardHeight Number of internal corners vertically @@ -373,8 +358,7 @@ private static ObservationInfo makeObservations( * @return Calibration result with optimized intrinsics, poses, and error metrics */ public static MrCalResult calibrateCamera( - int observationCount, - Iterator observationData, + List observations, int boardWidth, int boardHeight, double boardSpacing, @@ -382,11 +366,11 @@ public static MrCalResult calibrateCamera( int imageHeight, double focalLen) { - var observations = makeObservations(observationCount, observationData, boardWidth, boardHeight); + var packedObservations = makeObservations(observations, boardWidth, boardHeight); var results = mrcal_calibrate_camera( - observations.data, + packedObservations.data, boardWidth, boardHeight, boardSpacing, @@ -395,12 +379,12 @@ public static MrCalResult calibrateCamera( 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) { + for (int b = 0; b < packedObservations.ids.length; b++) { + if (packedObservations.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]]; + boolean[] partialCorners = new boolean[packedObservations.ids[b].length]; + for (int i = 0; i < packedObservations.ids[b].length; i++) { + partialCorners[i] = fullCorners[packedObservations.ids[b][i]]; } results.cornersUsed.set(b, partialCorners); } From 047ddb69912ecf50e36741ececa9789be467a2ac Mon Sep 17 00:00:00 2001 From: thatcomputerguy0101 Date: Thu, 30 Jul 2026 13:20:56 -0400 Subject: [PATCH 3/4] Drop separate observation info type --- .../java/org/photonvision/mrcal/MrCalJNI.java | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index 2591466..ed1884a 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -56,8 +56,6 @@ public MrCalObservation(Point[] corners, float[] levels) { } } - protected static record ObservationInfo(double[] data, int[][] ids) {} - public static class MrCalResult { public boolean success; public double[] intrinsics; @@ -279,10 +277,9 @@ public static native double[] compute_uncertainty( * 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 ObservationInfo makeObservations( + private static double[] makeObservations( List observations, int boardWidth, int boardHeight) { double[] packedObservations = new double[boardWidth * boardHeight * 3 * observations.size()]; - int[][] all_ids = new int[observations.size()][]; Arrays.fill(packedObservations, -1.0); for (int b = 0; b < observations.size(); b++) { @@ -291,7 +288,6 @@ private static ObservationInfo makeObservations( 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 @@ -337,7 +333,7 @@ private static ObservationInfo makeObservations( } } - return new ObservationInfo(packedObservations, all_ids); + return packedObservations; } /** @@ -370,7 +366,7 @@ public static MrCalResult calibrateCamera( var results = mrcal_calibrate_camera( - packedObservations.data, + packedObservations, boardWidth, boardHeight, boardSpacing, @@ -379,12 +375,13 @@ public static MrCalResult calibrateCamera( focalLen); // Only return corners used for the corners that were provided in the input - for (int b = 0; b < packedObservations.ids.length; b++) { - if (packedObservations.ids[b] != null) { + for (int b = 0; b < observations.size(); b++) { + var observation = observations.get(b); + if (observation.ids != null) { boolean[] fullCorners = results.cornersUsed.get(b); - boolean[] partialCorners = new boolean[packedObservations.ids[b].length]; - for (int i = 0; i < packedObservations.ids[b].length; i++) { - partialCorners[i] = fullCorners[packedObservations.ids[b][i]]; + boolean[] partialCorners = new boolean[observation.ids.length]; + for (int i = 0; i < observation.ids.length; i++) { + partialCorners[i] = fullCorners[observation.ids[i]]; } results.cornersUsed.set(b, partialCorners); } From 2e2c6598b2c395de982624c5664768b15f303eaf Mon Sep 17 00:00:00 2001 From: thatcomputerguy0101 Date: Fri, 31 Jul 2026 00:25:17 -0400 Subject: [PATCH 4/4] Add java wrapper for compute_uncertainty --- .../java/org/photonvision/mrcal/MrCalJNI.java | 96 +++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index ed1884a..4a15363 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -21,6 +21,7 @@ import java.util.Arrays; import java.util.List; import org.opencv.core.Point; +import org.opencv.core.Point3; import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Translation3d; @@ -336,6 +337,30 @@ private static double[] makeObservations( return packedObservations; } + /** + * Convert from WPILib poses to a raw RT array + * + * @return array of size [numObservations * 6] where each group of 6 is (rvec[0], rvec[1], + * rvec[2], tvec[0], tvec[1], tvec[2]) for the + */ + private static double[] observationPosesToRtRef(List observationPoses) { + double[] ret = new double[observationPoses.size() * 6]; + + for (int i = 0; i < observationPoses.size(); i++) { + var pose = observationPoses.get(i); + var r = pose.getRotation().toVector(); + var t = pose.getTranslation().toVector(); + ret[i * 6 + 0] = r.get(0); + ret[i * 6 + 1] = r.get(1); + ret[i * 6 + 2] = r.get(2); + ret[i * 6 + 3] = t.get(0); + ret[i * 6 + 4] = t.get(1); + ret[i * 6 + 5] = t.get(2); + } + + return ret; + } + /** * High-level wrapper for camera calibration using mrcal. * @@ -389,4 +414,75 @@ public static MrCalResult calibrateCamera( return results; } + + /** + * High-level wrapper for computing uncertainty using mrcal. + * + *

Converts detected chessboard corners and their detection levels into a packed double array. + * Converts observation poses into a packed double array. Then calls {@link + * #mrcal_calibrate_camera} to perform calibration. Each corner's detection level is converted to + * a weight (0.5^level), and negative levels indicate undetected corners. + * + * @param observations An iterator of observations, each containing a list of corner locations, + * decimation levels, and optional corner ids + * @param intrinsics Lens intrinsics from the calibration + * @param observationPoses Estimated pose of the calibration board in each snapshot + * @param boardWidth Number of internal corners horizontally + * @param boardHeight Number of internal corners vertically + * @param boardSpacing Physical spacing between corners (meters) + * @param imageWidth Image width in pixels + * @param imageHeight Image height in pixels + * @param sampleGridWidth Width of the generated sample grid + * @param sampleGridHeight Height of the generated sample grid + * @param warpX The x-component of lens distortion warp, or zero if no warp was estimated. + * @param warpY The y-component of lens distortion warp + * @return + */ + public static Point3[] computeUncertainty( + List observations, + double[] intrinsics, + List observationPoses, + int boardWidth, + int boardHeight, + double boardSpacing, + int imageWidth, + int imageHeight, + int sampleGridWidth, + int sampleGridHeight, + double warpX, + double warpY) { + if (observations.size() != observationPoses.size()) { + // Mismatched sizes + return null; + } + + var packedObservations = makeObservations(observations, boardWidth, boardHeight); + + var packedResult = + compute_uncertainty( + packedObservations, + intrinsics, + observationPosesToRtRef(observationPoses), + boardWidth, + boardHeight, + boardSpacing, + imageWidth, + imageHeight, + sampleGridWidth, + sampleGridHeight, + warpX, + warpY); + + var result = new Point3[sampleGridWidth * sampleGridHeight]; + + if (packedResult == null) { + return null; + } + + for (int i = 0; i < result.length; i++) { + result[i] = new Point3(packedResult[i * 3], packedResult[i * 3 + 1], packedResult[i * 3 + 2]); + } + + return result; + } }