Projection uncertainty (#20)

Author: mcm001

.gitignore

@@ -37,3 +37,5 @@ cmake_build/*
 bin/
 
 vcpkg_installed
+
+.venv

.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",

.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
+            }
+        }
+    ]
+}

CMakeLists.txt

@@ -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,15 +44,24 @@ 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"
     "${opencv_lib_SOURCE_DIR}/**/*.so*"
     "${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(

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`

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()
+        leadingSpacesToTabs(2)
+        leadingTabsToSpaces(4)
+        removeUnusedImports()
+        trimTrailingWhitespace()
+        endWithNewline()
+    }
+    groovyGradle {
+        target fileTree('.') {
+            include '**/*.gradle'
+            exclude '**/build/**', '**/build-*/**', "**/cmake_build/**"
+        }
+        greclipse()
+        leadingTabsToSpaces(4)
+        trimTrailingWhitespace()
+        endWithNewline()
+    }
+    format 'misc', {
+        target fileTree('.') {
+            include '**/*.md', '**/.gitignore'
+            exclude '**/build/**', '**/build-*/**', '**/node_modules/**', "**/cmake_build/**"
+        }
+        trimTrailingWhitespace()
+        leadingTabsToSpaces(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"

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")