diff --git a/.gitignore b/.gitignore
index f8a2123..a231d7f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -37,3 +37,5 @@ cmake_build/*
bin/
vcpkg_installed
+
+.venv
diff --git a/.vscode/launch.json b/.vscode/launch.json
index 620cc8d..4902ff7 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -8,7 +8,7 @@
"name": "(gdb) Attach",
"type": "cppdbg",
"request": "attach",
- "program": "/home/matt/github/mrcal-java/cmake_build/lib/libmrcal_jni.so",
+ "program": "${workspaceFolder}/cmake_build/lib/libmrcal_jni.so",
"processId": "${command:pickProcess}",
"MIMode": "gdb",
"setupCommands": [
@@ -23,23 +23,11 @@
"name": "Python debug calibrate cameras",
"type": "debugpy",
"request": "launch",
- "program": "/usr/bin/mrcal-calibrate-cameras",
+ "program": "mrcal-show-projection-uncertainty",
"console": "integratedTerminal",
- "cwd": "/home/matt/mrcal_debug_tmp/output_will/images-trimmed",
+ "cwd": "${workspaceFolder}/mrcal",
"args": [
- "--corners-cache",
- "corners.vnl",
- "--lensmodel",
- "LENSMODEL_OPENCV8",
- "--focal",
- "1200",
- "--object-spacing",
- "0.03",
- "--object-width-n",
- "18",
- "--object-height-n",
- "13",
- "*.png"
+ "./camera-0.cameramodel", "--cbmax","10","--unset","key","--gridn","60","40"
],
"justMyCode": false
},
@@ -65,7 +53,8 @@
"text": "-gdb-set disassembly-flavor intel",
"ignoreFailures": true
}
- ]
+ ],
+ "preLaunchTask": "build-test"
},
{
"name": "Python: Current File",
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
new file mode 100644
index 0000000..658b371
--- /dev/null
+++ b/.vscode/tasks.json
@@ -0,0 +1,21 @@
+{
+ "version": "2.0.0",
+ "tasks": [
+ {
+ "label": "build-test",
+ "command": "cmake --build cmake_build/",
+ "type": "shell",
+ "args": [],
+ "problemMatcher": [
+ "$tsc"
+ ],
+ "presentation": {
+ "reveal": "always"
+ },
+ "group": {
+ "kind": "build",
+ "isDefault": true
+ }
+ }
+ ]
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0072bc7..139cb71 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,7 +5,7 @@ project(mrcal_jni LANGUAGES C CXX VERSION 1.0.0)
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
# C++
-set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -14,16 +14,21 @@ if(WITH_ASAN)
add_compile_options(-fsanitize=address -g -Wall -fsanitize=undefined)
endif()
+# Include FetchContent module for downloading dependencies
+include(FetchContent)
+
+# OpenCV configuration
# Keep this in sync with build.gradle and with
# https://github.com/PhotonVision/photonvision/blob/main/build.gradle
set(OPENCV_YEAR "frc2025")
set(OPENCV_VERSION "4.10.0-3")
+set(WPIMATH_VERSION "2026.2.1")
# type can be "", "debug", "static", or "staticdebug"
set(OPENCV_TYPE "")
-# Download opencv, and save the path
-include(FetchContent)
+message(STATUS "Using FRC OpenCV for architecture: ${OPENCV_ARCH}")
+
FetchContent_Declare(
opencv_lib
URL
@@ -39,7 +44,6 @@ FetchContent_Declare(
)
FetchContent_MakeAvailable(opencv_header)
-# This probably doesn't work great with shared libraries, but I don't care about those right now
file(
GLOB_RECURSE OPENCV_LIB_PATH
"${opencv_lib_SOURCE_DIR}/**/*.lib"
@@ -47,7 +51,17 @@ file(
"${opencv_lib_SOURCE_DIR}/**/*.*.dylib"
)
set(OPENCV_INCLUDE_PATH ${opencv_header_SOURCE_DIR})
-message("Depending on opencv ${OPENCV_LIB_PATH}")
+set(OPENCV_LIBRARIES ${OPENCV_LIB_PATH})
+message(STATUS "Using FRC OpenCV libraries: ${OPENCV_LIB_PATH}")
+
+# Also download wpimath
+FetchContent_Declare(
+ wpimath_header
+ URL
+ https://frcmaven.wpi.edu/artifactory/release/edu/wpi/first/wpimath/wpimath-cpp/${WPIMATH_VERSION}/wpimath-cpp-${WPIMATH_VERSION}-headers.zip
+)
+FetchContent_MakeAvailable(wpimath_header)
+set(WPIMATH_INCLUDE_PATH ${wpimath_header_SOURCE_DIR})
# Openblas/suitesparse/friends
# From https://github.com/wpilibsuite/thirdparty-ceres/blob/main/CMakeLists.txt
@@ -136,6 +150,7 @@ set(SRC_HPP)
set(SRC_CPP
src/mrcal_jni.h
src/mrcal_wrapper.cpp
+ src/mrcal-uncertainty.cpp
src/mrcal_jni.cpp
libdogleg/dogleg.c
mrcal/mrcal.c
@@ -173,17 +188,23 @@ add_library(mrcal_jni SHARED ${INCLUDE_HPP} ${SRC_HPP} ${SRC_CPP})
target_include_directories(
mrcal_jni
SYSTEM
- PUBLIC ${JNI_INCLUDE_DIRS} ${OPENCV_INCLUDE_PATH} mrcal libdogleg
+ PUBLIC
+ ${JNI_INCLUDE_DIRS}
+ ${OPENCV_INCLUDE_PATH}
+ mrcal
+ libdogleg
+ ${WPIMATH_INCLUDE_PATH}
)
add_dependencies(mrcal_jni generate_minimath)
target_link_libraries(
mrcal_jni
- ${OPENCV_LIB_PATH}
- SuiteSparse::CHOLMOD_static
- SuiteSparse::SuiteSparseConfig_static
- ${OPENBLAS_TARGET}
- lapack
+ PUBLIC
+ ${OPENCV_LIBRARIES}
+ SuiteSparse::CHOLMOD_static
+ SuiteSparse::SuiteSparseConfig_static
+ ${OPENBLAS_TARGET}
+ lapack
)
# vnlog for the test script
@@ -192,15 +213,18 @@ add_library(vnlog STATIC ${VNLOG_SRC_CPP})
target_include_directories(vnlog SYSTEM PUBLIC ${PROJECT_SOURCE_DIR}/vnlog)
# Test script for checking our linker
+
add_executable(mrcal_jni_test src/mrcal_test.cpp)
+
target_link_libraries(mrcal_jni_test PUBLIC mrcal_jni)
+
target_include_directories(
mrcal_jni_test
SYSTEM
PRIVATE ${PROJECT_SOURCE_DIR}/vnlog
)
add_dependencies(mrcal_jni_test mrcal_jni vnlog)
-target_link_libraries(mrcal_jni_test PRIVATE ${OpenCV_LIBS} vnlog)
+target_link_libraries(mrcal_jni_test PRIVATE ${OPENCV_LIBRARIES} vnlog)
if(WITH_ASAN)
target_link_libraries(
diff --git a/README.md b/README.md
index d60c1d9..a85ffa9 100644
--- a/README.md
+++ b/README.md
@@ -19,3 +19,9 @@ cpan -i List::MoreUtils
cmake -B build -DOPENCV_ARCH=osxuniversal -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64"
cmake --build build
```
+
+# Uncertainty testing
+
+I configure with `cmake -B cmake_build`, build with `cmake --build cmake_build` and run with `time ./cmake_build/bin/mrcal_jni_test > /dev/null`
+
+I benchmark with `perf record -F 99 -g ./cmake_build/bin/mrcal_jni_test`
diff --git a/build.gradle b/build.gradle
index 151fe7f..aac22e2 100644
--- a/build.gradle
+++ b/build.gradle
@@ -32,12 +32,12 @@ dependencies {
implementation wpilibTools.deps.wpilibOpenCvJava("frc2025", "4.10.0-3")
// Junit
- testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
- testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
- testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
+ testImplementation 'org.junit.jupiter:junit-jupiter:5.7.1'
+ testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation wpilibTools.deps.wpilibJava("wpiunits")
+ implementation wpilibTools.deps.wpilibJava("wpiutil")
implementation "com.fasterxml.jackson.core:jackson-annotations:2.15.2"
}
@@ -57,6 +57,41 @@ test {
useJUnitPlatform()
}
+spotless {
+ java {
+ target fileTree('.') {
+ include '**/*.java'
+ exclude '**/build/**', '**/build-*/**', '**/src/generated/**', "**/cmake_build/**"
+ }
+ toggleOffOn()
+ googleJavaFormat()
+ indentWithTabs(2)
+ indentWithSpaces(4)
+ removeUnusedImports()
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ groovyGradle {
+ target fileTree('.') {
+ include '**/*.gradle'
+ exclude '**/build/**', '**/build-*/**', "**/cmake_build/**"
+ }
+ greclipse()
+ indentWithSpaces(4)
+ trimTrailingWhitespace()
+ endWithNewline()
+ }
+ format 'misc', {
+ target fileTree('.') {
+ include '**/*.md', '**/.gitignore'
+ exclude '**/build/**', '**/build-*/**', '**/node_modules/**', "**/cmake_build/**"
+ }
+ trimTrailingWhitespace()
+ indentWithSpaces(2)
+ endWithNewline()
+ }
+}
+
def nativeName = wpilibTools.platformMapper.platformPath
ext.outputsFolder = file("$buildDir/outputs")
@@ -74,4 +109,14 @@ tasks.register('copyNativeLibrary', Sync) {
publish.dependsOn it
}
+tasks.named('test', Test) {
+ useJUnitPlatform()
+
+ maxHeapSize = '1G'
+
+ testLogging {
+ events "passed"
+ }
+}
+
apply from: "publish.gradle"
diff --git a/heatmap-gen.py b/heatmap-gen.py
new file mode 100644
index 0000000..83f40dd
--- /dev/null
+++ b/heatmap-gen.py
@@ -0,0 +1,32 @@
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+# Load CSV-like data
+data = np.loadtxt("out", delimiter=",")
+
+x = data[:, 0]
+y = data[:, 1]
+z = data[:, 2]
+
+plt.figure()
+plt.title("Projection Uncertainty (in pixels), looking out to infinity")
+
+print(f"Mean={np.mean(z)}")
+
+# Create contour plot with 1px increments
+levels = np.arange(0, math.ceil(np.max(z)), 0.1) # 0, 1, 2, ..., 10
+contour = plt.tricontour(x, y, z, levels=levels, colors="black", linewidths=0.5)
+contourf = plt.tricontourf(x, y, z, levels=levels, cmap="viridis")
+plt.clabel(contour, inline=True, fontsize=8, fmt="%0.1f px")
+plt.colorbar(contourf, label="Uncertainty (px)")
+
+plt.xlabel("x")
+plt.ylabel("y")
+plt.gca().invert_yaxis()
+plt.axis("equal")
+plt.tight_layout()
+
+# plt.show()
+plt.savefig("heatmap.svg")
diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java
index 96f9c6b..16dcf40 100644
--- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java
+++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java
@@ -17,19 +17,24 @@
package org.photonvision.mrcal;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
-
import org.opencv.core.MatOfFloat;
import org.opencv.core.MatOfPoint2f;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
-
public class MrCalJNI {
+ public static enum CameraLensModel {
+ LENSMODEL_OPENCV5,
+ LENSMODEL_OPENCV8,
+ LENSMODEL_STEREOGRAPHIC,
+ LENSMODEL_SPLINED_STEREOGRAPHIC;
+ }
+
public static class MrCalResult {
public boolean success;
public double[] intrinsics;
@@ -46,9 +51,17 @@ public MrCalResult(boolean success) {
}
public MrCalResult(
- boolean success, int width, int height, double[] intrinsics, double[] optimized_rt_rtoref,
- double rms_error, double[] residuals, double warp_x,
- double warp_y, int Noutliers, boolean[] cornerUseMask) {
+ boolean success,
+ int width,
+ int height,
+ double[] intrinsics,
+ double[] optimized_rt_rtoref,
+ double rms_error,
+ double[] residuals,
+ double warp_x,
+ double warp_y,
+ int Noutliers,
+ boolean[] cornerUseMask) {
this.success = success;
this.intrinsics = intrinsics;
this.rms_error = rms_error;
@@ -59,14 +72,15 @@ public MrCalResult(
optimizedPoses = new ArrayList<>();
for (int i = 0; i < optimized_rt_rtoref.length; i += 6) {
- var rot = new Rotation3d(VecBuilder.fill(
- optimized_rt_rtoref[i + 0],
- optimized_rt_rtoref[i + 1],
- optimized_rt_rtoref[i + 2]));
- var t = new Translation3d(
- optimized_rt_rtoref[i + 3],
- optimized_rt_rtoref[i + 4],
- optimized_rt_rtoref[i + 5]);
+ var rot =
+ new Rotation3d(
+ VecBuilder.fill(
+ optimized_rt_rtoref[i + 0],
+ optimized_rt_rtoref[i + 1],
+ optimized_rt_rtoref[i + 2]));
+ var t =
+ new Translation3d(
+ optimized_rt_rtoref[i + 3], optimized_rt_rtoref[i + 4], optimized_rt_rtoref[i + 5]);
optimizedPoses.add(new Pose3d(t, rot));
}
@@ -78,33 +92,177 @@ public MrCalResult(
}
}
+ public double[] framePosesToRtToref() {
+ double[] ret = new double[optimizedPoses.size() * 6];
+
+ for (int i = 0; i < optimizedPoses.size(); i++) {
+ var pose = optimizedPoses.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;
+ }
+
@Override
public String toString() {
- return "MrCalResult [success=" + success + ", intrinsics=" + Arrays.toString(intrinsics) + ", rms_error="
- + rms_error + ", warp_x=" + warp_x + ", warp_y="
- + warp_y + ", Noutliers=" + Noutliers + "]";
+ return "MrCalResult [success="
+ + success
+ + ", intrinsics="
+ + Arrays.toString(intrinsics)
+ + ", rms_error="
+ + rms_error
+ + ", warp_x="
+ + warp_x
+ + ", warp_y="
+ + warp_y
+ + ", Noutliers="
+ + Noutliers
+ + "]";
}
}
+ /**
+ * Performs camera calibration using [mrcal](https://mrcal.secretsauce.net/formulation.html)
+ *
+ * @param observations_board a packed double array containing all observations across all frames.
+ * The array is structured as a flattened list where each observation consists of 3
+ * consecutive doubles: [x, y, level] for each corner of each board in each frame. Structure:
+ * For N frames, each observing a boardWidth×boardHeight checkerboard, the array has length N
+ * × boardWidth × boardHeight × 3. The level value (0-based) represents detection quality and
+ * will be converted to a weight using weight = 0.5^level. A negative level indicates the
+ * corner was not detected (will be marked as an outlier).
+ * @param boardWidth the number of internal corners in the horizontal direction of the calibration
+ * board
+ * @param boardHeight the number of internal corners in the vertical direction of the calibration
+ * board
+ * @param boardSpacing the physical distance (in arbitrary but consistent units) between adjacent
+ * corners on the calibration board. PhotonVision uses meters.
+ * @param imageWidth the width in pixels of the images used for calibration
+ * @param imageHeight the height in pixels of the images used for calibration
+ * @param focalLen an initial guess for the focal length in pixels, used to seed the optimization
+ * @return an {@link MrCalResult} object containing the calibration results, including optimized
+ * intrinsics, board poses, RMS error, per-observation residuals, detected outliers, and a
+ * boolean mask indicating which corners were actually used in the final optimization
+ */
public static native MrCalResult mrcal_calibrate_camera(
double[] observations_board,
- int boardWidth, int boardHeight, double boardSpacing,
- int imageWidth, int imageHeight, double focalLen);
+ int boardWidth,
+ int boardHeight,
+ double boardSpacing,
+ int imageWidth,
+ int imageHeight,
+ double focalLen);
- public static native boolean undistort_mrcal(long srcMat, long dstMat, long cameraMat, long distCoeffsMat,
- int lensModelOrdinal, int order, int Nx, int Ny, int fov_x_deg);
+ /**
+ * Applies mrcal lens distortion correction to undistort pixel coordinates.
+ *
+ * This method takes distorted pixel coordinates and applies the inverse of mrcal's lens
+ * distortion model to produce undistorted (corrected) coordinates. The input coordinates are
+ * unprojected through the specified lens model to 3D ray vectors, then reprojected through a
+ * pinhole model.
+ *
+ * @param dstMat a pointer to a cv::Mat containing distorted pixel coordinates (passed as a long
+ * representing the memory address). Must be a CV_64FC2 continuous matrix where each row
+ * contains one (x, y) coordinate pair. The matrix is modified in-place with the undistorted
+ * coordinates as output.
+ * @param cameraMat a pointer to the camera calibration matrix cv::Mat (3×3, opencv format).
+ * @param distCoeffsMat a pointer to the distortion coefficients cv::Mat. The interpretation of
+ * these coefficients depends on the specified lens model.
+ * @param lensModelOrdinal the ordinal value of the {@link CameraLensModel} enum indicating which
+ * lens distortion model to use for undistortion
+ * @param order the spline interpolation order to use when warping pixels, if the lensmodel is
+ * splined. Unused otherwise
+ * @param Nx the number of control points in the x-direction for the distortion spline model.
+ * Unused otherwise
+ * @param Ny the number of control points in the y-direction for the distortion spline model.
+ * Unused otherwise
+ * @param fov_x_deg the horizontal field of view in degrees used for certain lens model, if
+ * splined. Unused otherwise calculations
+ * @return true if the undistortion was successful, false otherwise
+ */
+ public static native boolean undistort_mrcal(
+ long dstMat,
+ long cameraMat,
+ long distCoeffsMat,
+ int lensModelOrdinal,
+ int order,
+ int Nx,
+ int Ny,
+ int fov_x_deg);
- public static MrCalResult calibrateCamera(
+ /**
+ * Computes projection uncertainty for camera calibration across a grid of image points.
+ *
+ *
This method evaluates the uncertainty in 3D point projection due to calibration error. It
+ * computes uncertainty at a regular grid of points across the image plane.
+ *
+ * @param observations_board a packed double array containing calibration observations with the
+ * same structure as used in {@link #mrcal_calibrate_camera}. For N frames observing a
+ * boardWidth×boardHeight checkerboard, the array has length N × boardWidth × boardHeight × 3,
+ * where each triplet is [x, y, level].
+ * @param intrinsics a double array containing the camera intrinsic parameters. The exact content
+ * and length depend on the lens distortion model being used, but typically includes focal
+ * length, principal point, and distortion coefficients.
+ * @param rt_ref_frames a packed double array containing the poses (rotations and translations) of
+ * the calibration board relative to the camera for each frame. The array is structured as N
+ * consecutive 6-element groups, where each group represents one frame's pose: [rx, ry, rz,
+ * tx, ty, tz]. The rotation components (rx, ry, rz) form a rotation vector (axis-angle
+ * representation), and (tx, ty, tz) represent the translation. Total array length is N × 6,
+ * where N is the number of observed frames.
+ * @param boardWidth the number of internal corners in the horizontal direction of the calibration
+ * board
+ * @param boardHeight the number of internal corners in the vertical direction of the calibration
+ * board
+ * @param boardSpacing the physical distance between adjacent corners on the calibration board
+ * (must match the value used in calibration). PhotonVision uses meters.
+ * @param imageWidth the width in pixels of the camera imager at this particular VideoMode
+ * @param imageHeight the height in pixels of the camera imager at this particular VideoMode
+ * @param sampleGridWidth the number of sample points in the horizontal direction for uncertainty
+ * computation
+ * @param sampleGridHeight the number of sample points in the vertical direction for uncertainty
+ * computation
+ * @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 a packed double array containing uncertainty values at each sampled grid point. The
+ * array is structured as a sequence of 3D points (mrcal_point3_t), representing [u, v,
+ * uncertainty] for each point we sampled. Total array length is sampleGridWidth ×
+ * sampleGridHeight × 3. Returns null if computation fails.
+ */
+ public static native double[] compute_uncertainty(
+ double[] observations_board,
+ double[] intrinsics,
+ double[] rt_ref_frames,
+ int boardWidth,
+ int boardHeight,
+ double boardSpacing,
+ int imageWidth,
+ int imageHeight,
+ int sampleGridWidth,
+ int sampleGridHeight,
+ double warpX,
+ double warpY);
+
+ /**
+ * Convert a list of board-pixel-corners and detection levels for each snapshot of a chessboard
+ * boardWidth x boardHeight to a packed double[] suitable to pass to
+ * MrCalJni::mrcal_calibrate_camera. 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(
List board_corners,
List board_corner_levels,
- int boardWidth, int boardHeight, double boardSpacing,
- int imageWidth, int imageHeight, double focalLen) {
+ int boardWidth,
+ int boardHeight) {
double[] observations = new double[boardWidth * boardHeight * 3 * board_corners.size()];
- if (!(board_corners.size() == board_corner_levels.size())) {
- return new MrCalResult(false);
- }
-
int i = 0;
for (int b = 0; b < board_corners.size(); b++) {
var board = board_corners.get(b);
@@ -112,7 +270,7 @@ public static MrCalResult calibrateCamera(
var corners = board.toArray();
if (!(corners.length == levels.length && corners.length == boardWidth * boardHeight)) {
- return new MrCalResult(false);
+ return null;
}
// Assume that we're correct in terms of row/column major-ness (lol)
@@ -128,11 +286,49 @@ public static MrCalResult calibrateCamera(
}
}
- if (i * 3 != observations.length){
+ if (i * 3 != observations.length) {
+ return null;
+ }
+
+ return observations;
+ }
+
+ /**
+ * High-level wrapper for camera calibration using mrcal.
+ *
+ * Converts detected chessboard corners and their detection levels 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 board_corners List of detected chessboard corners for each frame
+ * @param board_corner_levels List of detection levels for each corner in each frame. Point weight
+ * is calculated as weight = 0.5^level. Negative weight will ignore this observation
+ * @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 focalLen Initial guess for focal length (pixels)
+ * @return Calibration result with optimized intrinsics, poses, and error metrics
+ */
+ public static MrCalResult calibrateCamera(
+ List board_corners,
+ List board_corner_levels,
+ int boardWidth,
+ int boardHeight,
+ double boardSpacing,
+ int imageWidth,
+ int imageHeight,
+ double focalLen) {
+
+ if (!(board_corners.size() == board_corner_levels.size())) {
return new MrCalResult(false);
}
- return mrcal_calibrate_camera(observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight,
- focalLen);
+ var observations =
+ makeObservations(board_corners, board_corner_levels, boardWidth, boardHeight);
+
+ return mrcal_calibrate_camera(
+ observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, focalLen);
}
}
diff --git a/src/mrcal-uncertainty.cpp b/src/mrcal-uncertainty.cpp
new file mode 100644
index 0000000..735a487
--- /dev/null
+++ b/src/mrcal-uncertainty.cpp
@@ -0,0 +1,567 @@
+/*
+ * 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 .
+ */
+
+#include "mrcal-uncertainty.hpp"
+#include
+#include
+#include
+#include
+#include
+
+using namespace cv;
+
+using EigenPoint2 = Eigen::Matrix;
+using EigenPoint3 = Eigen::Matrix;
+
+// 10.5 seconds. definitely still CPU bound
+// using SolverType = Eigen::CholmodSupernodalLLT>;
+
+// 10.5 seconds
+using SolverType =
+ Eigen::SparseLU>;
+
+// 11 seconds
+// using SolverType = Eigen::SimplicialLLT>;
+
+struct CalibrationUncertaintyContext {
+ std::unique_ptr solver;
+ Eigen::SparseMatrix
+ J_observations; // J matrix for board observations only
+ double observed_pixel_uncertainty;
+ int Nstate;
+ int Nmeasurements_boards;
+};
+
+// Helper to create CHOLMOD factorization from sparse Jt
+cholmod_factor *create_factorization(cholmod_sparse *Jt,
+ cholmod_common *common) {
+ // Compute JtJ (which is Jt * Jt^T since Jt is already transposed)
+ cholmod_sparse *JtJ = cholmod_aat(Jt, nullptr, 0, 1, common);
+ if (!JtJ) {
+ throw std::runtime_error("cholmod_aat failed");
+ }
+
+ // Factorize JtJ
+ cholmod_factor *factorization = cholmod_analyze(JtJ, common);
+ if (!factorization) {
+ cholmod_free_sparse(&JtJ, common);
+ throw std::runtime_error("cholmod_analyze failed");
+ }
+
+ if (!cholmod_factorize(JtJ, factorization, common)) {
+ cholmod_free_factor(&factorization, common);
+ cholmod_free_sparse(&JtJ, common);
+ throw std::runtime_error("cholmod_factorize failed");
+ }
+
+ cholmod_free_sparse(&JtJ, common);
+ return factorization;
+}
+
+inline double worst_direction_stdev(const Eigen::Matrix2d &cov) {
+ // Direct formula for 2x2: sqrt((a+c)/2 + sqrt((a-c)^2/4 + b^2))
+ double a = cov(0, 0);
+ double b = cov(1, 0);
+ double c = cov(1, 1);
+
+ return std::sqrt((a + c) / 2.0 + std::sqrt((a - c) * (a - c) / 4.0 + b * b));
+}
+
+Eigen::MatrixXd solve_xt_JtJ_bt(
+ cholmod_factor *factorization,
+ const Eigen::Matrix &bt, // shape (2, Nstate)
+ cholmod_common *common) {
+ int Nstate = bt.cols();
+ int Nrhs = bt.rows();
+
+ if (factorization->n != static_cast(Nstate)) {
+ throw std::runtime_error("Dimension mismatch in solve");
+ }
+
+ // Transpose bt to column-major for CHOLMOD
+ Eigen::MatrixXd bt_col_major = bt.transpose(); // (Nstate, Nrhs)
+
+ cholmod_dense b = {.nrow = static_cast(Nstate),
+ .ncol = static_cast(Nrhs),
+ .nzmax = static_cast(Nrhs * Nstate),
+ .d = static_cast(Nstate),
+ .x = bt_col_major.data(),
+ .z = nullptr,
+ .xtype = CHOLMOD_REAL,
+ .dtype = CHOLMOD_DOUBLE};
+
+ Eigen::MatrixXd result(Nstate, Nrhs);
+ cholmod_dense out = {.nrow = static_cast(Nstate),
+ .ncol = static_cast(Nrhs),
+ .nzmax = static_cast(Nrhs * Nstate),
+ .d = static_cast(Nstate),
+ .x = result.data(),
+ .z = nullptr,
+ .xtype = CHOLMOD_REAL,
+ .dtype = CHOLMOD_DOUBLE};
+
+ cholmod_dense *M = &out;
+ cholmod_dense *Y = nullptr;
+ cholmod_dense *E = nullptr;
+
+ if (!cholmod_solve2(CHOLMOD_A, factorization, &b, nullptr, &M, nullptr, &Y,
+ &E, common)) {
+ throw std::runtime_error("cholmod_solve2 failed");
+ }
+
+ if (M != &out) {
+ throw std::runtime_error("cholmod_solve2 reallocated output");
+ }
+
+ return result;
+}
+
+std::vector sample_imager(Size numSamples, Size imagerSize) {
+ std::vector samples;
+ samples.reserve(numSamples.width * numSamples.height);
+
+ for (int j = 0; j < numSamples.height; j++) {
+ for (int i = 0; i < numSamples.width; i++) {
+ double x, y;
+
+ // linspace formula: start + (stop - start) * i / (n - 1)
+ if (numSamples.width == 1) {
+ x = 0;
+ } else {
+ x = (imagerSize.width - 1) * i / (numSamples.width - 1.0);
+ }
+
+ if (numSamples.height == 1) {
+ y = 0;
+ } else {
+ y = (imagerSize.height - 1) * j / (numSamples.height - 1.0);
+ }
+
+ samples.push_back({.x = x, .y = y});
+ }
+ }
+ return samples;
+}
+
+// The derivative of q (pixel space location/s) wrt b (state vector)
+// at some point this should be a matrix
+Eigen::Matrix
+_dq_db_projection_uncertainty(mrcal_point3_t pcam, mrcal_lensmodel_t lensmodel,
+ std::span rt_ref_frame, int Nstate,
+ int istate_frames0,
+ std::span intrinsics) {
+ // project with gradients
+ // model_analysis.py:1067
+ mrcal_point2_t q{};
+ Eigen::Matrix dq_dpcam;
+ std::vector dq_dintrinsics(2 * 1ull * intrinsics.size());
+ bool ret = mrcal_project(
+ // out
+ &q, reinterpret_cast(dq_dpcam.data()),
+ dq_dintrinsics.data(),
+ // in
+ &pcam, 1, &lensmodel, intrinsics.data());
+
+ if (!ret) {
+ throw std::runtime_error("mrcal_project_with_gradients failed");
+ }
+
+ // number of boards
+ const size_t Nboards{rt_ref_frame.size()};
+
+ // p_ref = pcam rotated by r (always zero1)
+ Eigen::Matrix p_ref{pcam.x, pcam.y, pcam.z};
+
+ // prepare dq_db. Mrcal does this as a 40x60x2xNstate tensor, but we
+ // are only projecting one point
+ Eigen::Matrix dq_db(2, Nstate);
+ dq_db.setZero();
+
+ // set dq_db[0:num intrinsics] = [dq_dintrinsics]
+ Eigen::Map>
+ dq_dintrinsics_eigen(dq_dintrinsics.data(), 2, intrinsics.size());
+ dq_db.leftCols(intrinsics.size()) = dq_dintrinsics_eigen;
+
+ // determine dpcam_dr and dpcamp_dpref
+ Eigen::Matrix dpcam_dpref; // dxout/dxin
+ Eigen::Matrix dpcam_dr; // dx_out/dr
+ {
+ // HACK -- hard-coded to origin
+ Eigen::Matrix rt_cam_ref;
+ rt_cam_ref.setZero();
+
+ // output arrays
+ mrcal_point3_t rotated_p_ref;
+
+ mrcal_rotate_point_r(
+ // out
+ rotated_p_ref.xyz, dpcam_dr.data(), dpcam_dpref.data(),
+ // in
+ rt_cam_ref.data(), p_ref.data());
+ }
+
+ // method is always mean-pcam
+ Eigen::Matrix dq_dpref = dq_dpcam * dpcam_dpref;
+
+ // calculate p_frames
+ Eigen::Matrix p_frames(Nboards,
+ 3);
+ p_frames.setZero();
+ {
+ // output arrays
+
+ for (size_t pose = 0; pose < Nboards; pose++) {
+ mrcal_rotate_point_r_inverted(
+ // out
+ p_frames.row(pose).data(), NULL, NULL,
+ // in
+ rt_ref_frame[pose].r.xyz, p_ref.data());
+ }
+ }
+
+ // and rotate to yield dpref_dframes
+ std::vector> dpref_dframes;
+ dpref_dframes.resize(Nboards);
+ for (size_t pose = 0; pose < Nboards; pose++) {
+ mrcal_point3_t dummy;
+ mrcal_rotate_point_r(
+ // out
+ dummy.xyz, dpref_dframes[pose].data(), NULL,
+ // in
+ rt_ref_frame[pose].r.xyz, p_frames.row(pose).data());
+ }
+
+ // Calculate dq_dframes
+ std::vector> dq_dframes;
+ dq_dframes.resize(Nboards);
+ for (size_t pose = 0; pose < Nboards; pose++) {
+ dq_dframes[pose] = dq_dpref * dpref_dframes[pose];
+ }
+
+ // Populate dq_db_slice_frames
+ // Shape after mean and xchg: (2, 3) for at_infinity
+ // Each frame gets 3 DOF (translation only)
+ for (size_t frame = 0; frame < Nboards; frame++) {
+ int frame_start = istate_frames0 + frame * 6;
+ // Populate first 3 columns of each frame's 6 DOF block with the mean
+ dq_db.block(0, frame_start, 2, 3) = dq_dframes[frame] / Nboards;
+ }
+
+ return dq_db;
+}
+
+double _observed_pixel_uncertainty_from_inputs(std::vector &x,
+ int num_measurements_board,
+ int measurement_index_board) {
+ // Compute variance from residuals
+ double sum = 0.0, sum_sq = 0.0;
+ for (size_t i = measurement_index_board;
+ i < measurement_index_board + num_measurements_board; i++) {
+ double val = x[i];
+ sum += val;
+ sum_sq += val * val;
+ }
+ double mean = sum / x.size();
+ double variance = (sum_sq / x.size()) - (mean * mean);
+
+ return std::sqrt(variance);
+}
+
+CalibrationUncertaintyContext create_calibration_uncertainty_context(
+ mrcal_problem_selections_t &problem_selections,
+ mrcal_lensmodel_t &lensmodel, const std::span intrinsics,
+ const std::span rt_ref_frame,
+ const mrcal_observation_board_t *observations_board,
+ const mrcal_point3_t *observations_board_pool, int Nobservations_board,
+ int calibration_object_width_n, int calibration_object_height_n,
+ double calibration_object_spacing, cv::Size imagerSize,
+ mrcal_calobject_warp_t warp) {
+ int Nstate =
+ mrcal_num_states(1, 0, rt_ref_frame.size(), 0, 0, Nobservations_board,
+ problem_selections, &lensmodel);
+
+ int Nmeasurements = mrcal_num_measurements(
+ Nobservations_board, 0, NULL, 0, calibration_object_width_n,
+ calibration_object_height_n, 1, 0, 6, 0, 0, problem_selections,
+ &lensmodel);
+ int Nmeasurements_boards = mrcal_num_measurements_boards(
+ Nobservations_board, calibration_object_width_n,
+ calibration_object_height_n);
+ int imeas0 = mrcal_measurement_index_boards(0, Nobservations_board, 0,
+ calibration_object_width_n,
+ calibration_object_height_n);
+
+ // Allocate buffers for Jt sparse matrix
+ int N_j_nonzero = Nstate * Nmeasurements; // Upper bound, actual will be less
+ std::vector Jt_p(Nmeasurements + 1);
+ std::vector Jt_i(N_j_nonzero);
+ std::vector Jt_x(N_j_nonzero);
+
+ cholmod_sparse Jt = {.nrow = static_cast(Nstate),
+ .ncol = static_cast(Nmeasurements),
+ .nzmax = static_cast(N_j_nonzero),
+ .p = Jt_p.data(),
+ .i = Jt_i.data(),
+ .nz = nullptr,
+ .x = Jt_x.data(),
+ .z = nullptr,
+ .stype = 0,
+ .itype = CHOLMOD_INT,
+ .xtype = CHOLMOD_REAL,
+ .dtype = CHOLMOD_DOUBLE,
+ .sorted = 1,
+ .packed = 1};
+
+ std::vector b_packed(Nstate);
+ std::vector x(Nmeasurements);
+
+ double point_min_range = -1.0, point_max_range = -1.0;
+ mrcal_problem_constants_t problem_constants = {
+ .point_min_range = point_min_range, .point_max_range = point_max_range};
+
+ int imagersize[2]{imagerSize.width, imagerSize.height};
+
+ bool ret = mrcal_optimizer_callback(
+ b_packed.data(), b_packed.size() * sizeof(double), // out
+ x.data(), x.size() * sizeof(double), // out
+ &Jt, // Get the Jacobian
+ // IN parameters
+ intrinsics.data(),
+ nullptr, // no extrinsics
+ rt_ref_frame.data(),
+ nullptr, // no points
+ &warp, 1, 0, static_cast(rt_ref_frame.size()), 0, 0,
+ observations_board, nullptr, Nobservations_board, 0, nullptr, 0,
+ observations_board_pool, nullptr, &lensmodel, imagersize,
+ problem_selections, &problem_constants, calibration_object_spacing,
+ calibration_object_width_n, calibration_object_height_n, false);
+
+ if (!ret) {
+ throw std::runtime_error("mrcal_optimizer_callback failed");
+ }
+
+ double observed_pixel_uncertainty =
+ _observed_pixel_uncertainty_from_inputs(x, Nmeasurements_boards, imeas0);
+
+ // Convert CHOLMOD sparse Jt to Eigen sparse matrix
+ using SpMat = Eigen::SparseMatrix;
+ SpMat Jt_eigen(Nstate, Nmeasurements);
+
+ {
+ std::vector> triplets;
+ triplets.reserve(N_j_nonzero);
+
+ for (int col = 0; col < Nmeasurements; col++) {
+ for (int p = Jt_p[col]; p < Jt_p[col + 1]; p++) {
+ int row = Jt_i[p];
+ double val = Jt_x[p];
+ triplets.emplace_back(row, col, val);
+ }
+ }
+
+ Jt_eigen.setFromTriplets(triplets.begin(), triplets.end());
+ }
+
+ // J = Jt^T has shape (Nmeasurements, Nstate)
+ SpMat J = Jt_eigen.transpose();
+
+ // Compute JtJ = Jt * J (shape Nstate x Nstate)
+ SpMat JtJ = Jt_eigen * J;
+
+ // check positive definite happens for free during compute/info
+
+ // Pre-compute the factorization
+ auto solver = std::make_unique();
+ solver->compute(JtJ);
+
+ if (solver->info() != Eigen::Success) {
+ throw std::runtime_error("Eigen factorization failed");
+ }
+
+ // Store the observation rows of J for fast uncertainty propagation
+ SpMat J_observations = J.topRows(Nmeasurements_boards);
+
+ return CalibrationUncertaintyContext{
+ .solver = std::move(solver),
+ .J_observations = std::move(J_observations),
+ .observed_pixel_uncertainty = observed_pixel_uncertainty,
+ .Nstate = Nstate,
+ .Nmeasurements_boards = Nmeasurements_boards};
+}
+
+double _propagate_calibration_uncertainty_fast(
+ const CalibrationUncertaintyContext &context,
+ Eigen::Matrix dF_dbunpacked,
+ mrcal_problem_selections_t &problem_selections,
+ mrcal_lensmodel_t &lensmodel, const std::span intrinsics,
+ const std::span rt_ref_frame, int Nobservations_board) {
+ // Pack the gradient
+ Eigen::Matrix dF_dbpacked =
+ dF_dbunpacked;
+
+ // called for each Nstate elements of dF_dbunpacked
+ for (int i = 0; i < dF_dbunpacked.rows(); i++) {
+ size_t offset = i * dF_dbunpacked.cols();
+ mrcal_unpack_solver_state_vector(
+ dF_dbpacked.data() + offset, 1, 0, rt_ref_frame.size(), 0, 0,
+ Nobservations_board, problem_selections, &lensmodel);
+ }
+
+ // Solve JtJ * A = dF_dbpacked^T using pre-computed factorization
+ Eigen::MatrixXd rhs = dF_dbpacked.transpose(); // (Nstate, 2)
+ Eigen::MatrixXd A = context.solver->solve(rhs); // (Nstate, 2)
+
+ if (context.solver->info() != Eigen::Success) {
+ throw std::runtime_error("Eigen solve failed");
+ }
+
+ // Compute J_obs * A using the stored observation Jacobian
+ Eigen::MatrixXd JA = context.J_observations * A; // (Nmeas_obs, 2)
+
+ // Compute Var(F) = JA^T * JA
+ Eigen::Matrix2d Var_dF = JA.transpose() * JA;
+
+ return worst_direction_stdev(Var_dF) * context.observed_pixel_uncertainty;
+}
+
+double projection_uncertainty_fast(const CalibrationUncertaintyContext &context,
+ mrcal_point3_t pcam,
+ mrcal_lensmodel_t lensmodel,
+ std::span rt_ref_frames,
+ std::span intrinsics) {
+ // Prepare inputs
+ mrcal_problem_selections_t problem_selections{0};
+ problem_selections.do_optimize_intrinsics_core = true;
+ problem_selections.do_optimize_intrinsics_distortions = true;
+ problem_selections.do_optimize_extrinsics = true;
+ problem_selections.do_optimize_frames = true;
+ problem_selections.do_optimize_calobject_warp = true;
+ problem_selections.do_apply_regularization = true;
+ problem_selections.do_apply_outlier_rejection = true;
+ problem_selections.do_apply_regularization_unity_cam01 = false;
+
+ int istate_frames0 = mrcal_state_index_frames(0, 1, 0, 6, 0, 0, 6,
+ problem_selections, &lensmodel);
+
+ int Nobservations_board = rt_ref_frames.size();
+
+ auto dq_db{_dq_db_projection_uncertainty(pcam, lensmodel, rt_ref_frames,
+ context.Nstate, istate_frames0,
+ intrinsics)};
+
+ return _propagate_calibration_uncertainty_fast(
+ context, dq_db, problem_selections, lensmodel, intrinsics, rt_ref_frames,
+ Nobservations_board);
+}
+
+std::vector compute_uncertainty(
+ std::span observations_board, std::span intrinsics,
+ std::span rt_ref_frames, mrcal_calobject_warp_t warp,
+ cv::Size imagerSize, cv::Size calobjectSize, double calobjectSpacing,
+ cv::Size sampleResolution) {
+
+ mrcal_lensmodel_t lensmodel;
+ lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
+
+ // Create calibration uncertainty context once
+ mrcal_problem_selections_t problem_selections{0};
+ problem_selections.do_optimize_intrinsics_core = true;
+ problem_selections.do_optimize_intrinsics_distortions = true;
+ problem_selections.do_optimize_extrinsics = true;
+ problem_selections.do_optimize_frames = true;
+ problem_selections.do_optimize_calobject_warp = true;
+ problem_selections.do_apply_regularization = true;
+ problem_selections.do_apply_outlier_rejection = true;
+ problem_selections.do_apply_regularization_unity_cam01 = false;
+
+ std::vector indices_frame_camintrinsics_camextrinsics;
+ for (int i = 0; i < rt_ref_frames.size(); i++) {
+ indices_frame_camintrinsics_camextrinsics.push_back(
+ {.x = static_cast(i), .y = 0, .z = -1});
+ }
+
+ std::vector observations_board_data(
+ rt_ref_frames.size());
+ auto c_observations_board = observations_board_data.data();
+
+ for (int i_observation = 0; i_observation < rt_ref_frames.size();
+ i_observation++) {
+ int32_t iframe =
+ indices_frame_camintrinsics_camextrinsics.at(i_observation).x;
+ int32_t icam_intrinsics =
+ indices_frame_camintrinsics_camextrinsics.at(i_observation).y;
+ int32_t icam_extrinsics =
+ indices_frame_camintrinsics_camextrinsics.at(i_observation).z;
+
+ c_observations_board[i_observation].icam.intrinsics = icam_intrinsics;
+ c_observations_board[i_observation].icam.extrinsics = icam_extrinsics;
+ c_observations_board[i_observation].iframe = iframe;
+ }
+
+ auto context = create_calibration_uncertainty_context(
+ problem_selections, lensmodel, intrinsics, rt_ref_frames,
+ c_observations_board, observations_board.data(), rt_ref_frames.size(),
+ calobjectSize.width, calobjectSize.height, calobjectSpacing, imagerSize,
+ warp);
+
+ // generate grid of samples in (u, v) pixels
+ auto q = sample_imager(sampleResolution, imagerSize);
+
+ // unproject
+ std::vector pcam;
+ pcam.resize(q.size());
+ mrcal_unproject(pcam.data(), q.data(), q.size(), &lensmodel,
+ intrinsics.data());
+
+ // normalize, setting rows with any non-finite elements to zero
+ for (auto &p : pcam) {
+ double nrm = std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
+ if (std::isfinite(nrm) && nrm > 0) {
+ p.x /= nrm;
+ p.y /= nrm;
+ p.z /= nrm;
+ } else {
+ p.x = 0;
+ p.y = 0;
+ p.z = 0;
+ }
+ }
+
+ std::vector ret;
+ ret.reserve(pcam.size());
+
+ // iterate over pcam and q simultaneously - now much faster!
+ for (size_t i = 0; i < pcam.size(); i++) {
+ auto &pi = pcam[i];
+ auto &qi = q[i];
+
+ auto start = std::chrono::high_resolution_clock::now();
+
+ double uncertainty = projection_uncertainty_fast(context, pi, lensmodel,
+ rt_ref_frames, intrinsics);
+
+ auto end = std::chrono::high_resolution_clock::now();
+ auto duration =
+ std::chrono::duration_cast(end - start);
+
+ ret.push_back(mrcal_point3_t{qi.x, qi.y, uncertainty});
+ }
+
+ return ret;
+}
diff --git a/src/mrcal-uncertainty.hpp b/src/mrcal-uncertainty.hpp
new file mode 100644
index 0000000..0abb6ac
--- /dev/null
+++ b/src/mrcal-uncertainty.hpp
@@ -0,0 +1,33 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+std::vector compute_uncertainty(
+ std::span observations_board, std::span intrinsics,
+ std::span rt_ref_frames, mrcal_calobject_warp_t warp,
+ cv::Size imagerSize, cv::Size calobjectSize, double calobjectSpacing,
+ cv::Size sampleResolution);
diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp
index 4ecf150..d41089d 100644
--- a/src/mrcal_jni.cpp
+++ b/src/mrcal_jni.cpp
@@ -27,6 +27,7 @@
#include
#include
+#include "mrcal-uncertainty.hpp"
#include "mrcal_wrapper.h"
// JClass helper from wpilib
@@ -272,18 +273,108 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
/*
* Class: org_photonvision_mrcal_MrCalJNI_undistort
* Method: 1mrcal
- * Signature: (JJJJIIIII)Z
+ * Signature: (JJJIIIII)Z
*/
JNIEXPORT jboolean JNICALL
Java_org_photonvision_mrcal_MrCalJNI_undistort_1mrcal
- (JNIEnv *, jclass, jlong srcMat, jlong dstMat, jlong camMat, jlong distCoeffs,
+ (JNIEnv *, jclass, jlong dstMat, jlong camMat, jlong distCoeffs,
jint lensModelOrdinal, jint order, jint Nx, jint Ny, jint fov_x_deg)
{
return undistort_mrcal(
- reinterpret_cast(srcMat), reinterpret_cast(dstMat),
- reinterpret_cast(camMat),
+ reinterpret_cast(dstMat), reinterpret_cast(camMat),
reinterpret_cast(distCoeffs),
static_cast(lensModelOrdinal),
static_cast(order), static_cast(Nx),
static_cast(Ny), static_cast(fov_x_deg));
}
+
+// Helper class for managing JNI array access with automatic cleanup. Thanks,
+// Claude
+template class JNIArrayView {
+public:
+ JNIArrayView(JNIEnv *env, jdoubleArray jArray)
+ : env_(env), jArray_(jArray), data_(nullptr), size_(0) {
+ if (jArray) {
+ size_ = env->GetArrayLength(jArray);
+ data_ = env->GetDoubleArrayElements(jArray, nullptr);
+ }
+ }
+
+ ~JNIArrayView() {
+ if (data_) {
+ env_->ReleaseDoubleArrayElements(jArray_, data_, JNI_ABORT);
+ }
+ }
+
+ // Delete copy operations to prevent double-free
+ JNIArrayView(const JNIArrayView &) = delete;
+ JNIArrayView &operator=(const JNIArrayView &) = delete;
+
+ bool isValid() const { return data_ != nullptr; }
+
+ template std::span asSpan(jsize elementSize = sizeof(T)) {
+ return std::span(reinterpret_cast(data_),
+ size_ / (sizeof(U) / sizeof(double)));
+ }
+
+ std::span asDoubleSpan() { return std::span(data_, size_); }
+
+private:
+ JNIEnv *env_;
+ jdoubleArray jArray_;
+ jdouble *data_;
+ jsize size_;
+};
+
+/*
+ * Class: org_photonvision_mrcal_MrCalJNI_compute
+ * Method: 1uncertainty
+ * Signature: ([D[D[DIIDIIIIDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_org_photonvision_mrcal_MrCalJNI_compute_1uncertainty
+ (JNIEnv *env, jclass, jdoubleArray jObservations, jdoubleArray jIntrinsics,
+ jdoubleArray jRtRefFrames, jint boardWidth, jint boardHeight,
+ jdouble boardSpacing, jint imageWidth, jint imageHeight,
+ jint sampleGridWidth, jint sampleGridHeight, jdouble warpX, jdouble warpY)
+{
+ // Create RAII wrappers - automatic cleanup on scope exit
+ JNIArrayView observations(env, jObservations);
+ JNIArrayView intrinsics(env, jIntrinsics);
+ JNIArrayView rtFrames(env, jRtRefFrames);
+
+ // Validate all arrays
+ if (!observations.isValid() || !intrinsics.isValid() || !rtFrames.isValid()) {
+ std::cout << "bad array in?" << std::endl;
+ return nullptr;
+ }
+
+ // Setup parameters
+ mrcal_calobject_warp_t warp{.x2 = warpX, .y2 = warpY};
+ cv::Size imagerSize(imageWidth, imageHeight);
+ cv::Size calobjectSize(boardWidth, boardHeight);
+ cv::Size sampleRes(sampleGridWidth, sampleGridHeight);
+
+ std::vector result;
+ try {
+ result = compute_uncertainty(
+ observations.asSpan(), intrinsics.asDoubleSpan(),
+ rtFrames.asSpan(), warp, imagerSize, calobjectSize,
+ boardSpacing, sampleRes);
+ } catch (const std::exception &e) {
+ std::cout << "exception thrown -- " << e.what() << std::endl;
+ return nullptr;
+ }
+
+ jsize resultSize = result.size() * 3;
+ jdoubleArray jResult = env->NewDoubleArray(resultSize);
+ if (jResult == nullptr) {
+ std::cout << "waah" << std::endl;
+ return nullptr;
+ }
+
+ // point3's are just packed doubles so we can do trickery
+ env->SetDoubleArrayRegion(jResult, 0, resultSize,
+ reinterpret_cast(result.data()));
+ return jResult;
+}
diff --git a/src/mrcal_jni.h b/src/mrcal_jni.h
index ee4df26..ea736e8 100644
--- a/src/mrcal_jni.h
+++ b/src/mrcal_jni.h
@@ -44,6 +44,16 @@ Java_org_photonvision_mrcal_MrCalJNI_undistort_1mrcal(JNIEnv *, jclass, jlong,
jlong, jlong, jlong, jint,
jint, jint, jint, jint);
+/*
+ * Class: org_photonvision_mrcal_MrCalJNI
+ * Method: compute_uncertainty
+ * Signature: ([D[D[DIIDIIII)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_org_photonvision_mrcal_MrCalJNI_compute_1uncertainty(
+ JNIEnv *, jclass, jdoubleArray, jdoubleArray, jdoubleArray, jint, jint,
+ jdouble, jint, jint, jint, jint, jdouble, jdouble);
+
#ifdef __cplusplus
} // extern "C"
#endif
diff --git a/src/mrcal_test.cpp b/src/mrcal_test.cpp
index db8df2f..fc6ac72 100644
--- a/src/mrcal_test.cpp
+++ b/src/mrcal_test.cpp
@@ -15,19 +15,20 @@
* along with this program. If not, see .
*/
-#include
-#include
-
#include
#include
#include
+#include
#include
#include