WPIcal: Field Calibration Tool

.gitignore

@@ -14,7 +14,7 @@ networktables.json
 
 ntcore/connectionlistenertest.json
 ntcore/timesynctest.json
-
+wpical/src/main/native/resources/camera calibration.json
 nanopb_pb2.py
 
 # Created by the jenkins test script

CMakeLists.txt

@@ -76,6 +76,7 @@ cmake_dependent_option(
 )
 option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
 option(WITH_NTCORE "Build ntcore" ON)
+option(WITH_WPICAL "Build wpical" OFF)
 option(WITH_WPIMATH "Build wpimath" ON)
 cmake_dependent_option(
     WITH_WPIUNITS
@@ -142,6 +143,7 @@ if(WITH_DOCS)
     include(AddDoxygenDocs)
     add_doxygen_docs()
 endif()
+find_package(Ceres CONFIG)
 find_package(LIBSSH CONFIG 0.7.1)
 
 set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ON)
@@ -314,6 +316,9 @@ if(WITH_GUI)
     add_subdirectory(glass)
     add_subdirectory(outlineviewer)
     add_subdirectory(sysid)
+    if(Ceres_FOUND AND WITH_WPICAL)
+        add_subdirectory(wpical)
+    endif()
     if(LIBSSH_FOUND)
         add_subdirectory(roborioteamnumbersetter)
         add_subdirectory(datalogtool)

README.md

@@ -59,6 +59,8 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 
 On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be able to use the macOS x86 roboRIO toolchain on ARM.
 
+On linux, run `sudo apt install gfortran`. This is necessary to be able to build WPIcal on linux platforms.
+
 ## Setup
 
 Clone the WPILib repository and follow the instructions above for installing any required tooling. The build process uses versioning information from git. Downloading the source is not sufficient to run the build.

settings.gradle

@@ -62,6 +62,7 @@ include 'epilogue-processor'
 include 'epilogue-runtime'
 include 'thirdparty:googletest'
 include 'thirdparty:imgui_suite'
+include 'wpical'
 
 buildCache {
     def cred = {

shared/ceres.gradle

@@ -0,0 +1,13 @@
+nativeUtils {
+    nativeDependencyContainer {
+        ceres(getNativeDependencyTypeClass('WPIStaticMavenDependency')) {
+            groupId = "edu.wpi.first.thirdparty.frc2024.ceres"
+            artifactId = "ceres-cpp"
+            headerClassifier = "headers"
+            sourceClassifier = "sources"
+            ext = "zip"
+            version = '2.2-3'
+            targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms)
+        }
+    }
+}

shared/java/javastyle.gradle

@@ -48,7 +48,7 @@ spotless {
         target fileTree('.') {
             include '**/*.json'
             exclude '**/build/**', '**/build-*/**', '**/bin/**'
-            exclude '**/simgui-ds.json', '**/simgui-window.json', '**/simgui.json', '**/networktables.json', '**/*test.json'
+            exclude '**/simgui-ds.json', '**/simgui-window.json', '**/simgui.json', '**/networktables.json', '**/*test.json', '**/camera calibration.json'
         }
         gson()
           .indentWithSpaces(2)

upstream_utils/libdogleg.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import os
+import shutil
+
+from upstream_utils import Lib, walk_cwd_and_copy_if
+
+
+def copy_upstream_src(wpilib_root):
+    wpical = os.path.join(wpilib_root, "wpical")
+
+    # Delete old install
+    for d in [
+        "src/main/native/thirdparty/libdogleg/src",
+        "src/main/native/thirdparty/libdogleg/include",
+    ]:
+        shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
+
+    files = walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith("dogleg.h"),
+        os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"),
+    )
+    for f in files:
+        with open(f) as file:
+            content = file.read()
+        content = content.replace(
+            "#include <cholmod.h>", "#include <suitesparse/cholmod.h>"
+        )
+        with open(f, "w") as file:
+            file.write(content)
+
+    files = walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith("dogleg.cpp"),
+        os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"),
+    )
+    for f in files:
+        with open(f) as file:
+            content = file.read()
+        content = content.replace("#warning", "// #warning")
+        content = content.replace("__attribute__((unused))", "")
+        content = content.replace(
+            "#include <cholmod_function.h>", "#include <suitesparse/cholmod_function.h>"
+        )
+        with open(f, "w") as file:
+            file.write(content)
+
+
+def main():
+    name = "libdogleg"
+    url = "https://github.com/dkogan/libdogleg"
+    tag = "c971ea43088d286a3683c1039b9a85f761f7df15"
+
+    libdogleg = Lib(name, url, tag, copy_upstream_src)
+    libdogleg.main()
+
+
+if __name__ == "__main__":
+    main()

upstream_utils/libdogleg_patches/0001-Convert-to-C.patch

@@ -0,0 +1,38 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Gold856 <[email protected]>
+Date: Fri, 29 Nov 2024 19:52:22 -0500
+Subject: [PATCH 1/3] Convert to C++
+
+---
+ dogleg.c => dogleg.cpp | 0
+ dogleg.h               | 8 +++++++-
+ 2 files changed, 7 insertions(+), 1 deletion(-)
+ rename dogleg.c => dogleg.cpp (100%)
+
+diff --git a/dogleg.c b/dogleg.cpp
+similarity index 100%
+rename from dogleg.c
+rename to dogleg.cpp
+diff --git a/dogleg.h b/dogleg.h
+index 74337263a89d3448f55e125db6702ebfa7588b97..4a23b669a30642d290549bb94a1c787f079d0647 100644
+--- a/dogleg.h
++++ b/dogleg.h
+@@ -126,7 +126,9 @@ typedef struct
+
+ } dogleg_solverContext_t;
+
+-
++#ifdef __cplusplus
++extern "C" {
++#endif
+ // Fills in the given structure with the default parameter set
+ void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters);
+
+@@ -293,3 +295,7 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double*            Jqu
+                                                     int                      NoutlierFeatures,
+                                                     dogleg_operatingPoint_t* point,
+                                                     dogleg_solverContext_t*  ctx);
++
++#ifdef __cplusplus
++} // extern "C"
++#endif

upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch

@@ -0,0 +1,79 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Gold856 <[email protected]>
+Date: Fri, 29 Nov 2024 19:54:54 -0500
+Subject: [PATCH 2/3] Replace VLAs with vectors
+
+---
+ dogleg.cpp | 17 +++++++++--------
+ 1 file changed, 9 insertions(+), 8 deletions(-)
+
+diff --git a/dogleg.cpp b/dogleg.cpp
+index 7e4259c303e21a9f4c63ba16f1bf5df131935057..9ed95f64b232f41a51fe23d72885ecadd86dc065 100644
+--- a/dogleg.cpp
++++ b/dogleg.cpp
+@@ -6,6 +6,7 @@
+ // Apparently I need this in MSVC to get constants
+ #define _USE_MATH_DEFINES
+
++#include <vector>
+ #include <stdio.h>
+ #include <stdarg.h>
+ #include <math.h>
+@@ -1907,7 +1908,7 @@ static bool getOutliernessFactors_dense( // output
+     // where A = Jo inv(JtJ) Jot
+     //
+     // A is symmetric. I store the upper triangle
+-    double A[featureSize*(featureSize+1)/2];
++    std::vector<double> A(featureSize*(featureSize+1)/2);
+     int iA=0;
+     for(int i=0; i<featureSize; i++)
+       for(int j=i; j<featureSize; j++, iA++)
+@@ -1921,7 +1922,7 @@ static bool getOutliernessFactors_dense( // output
+       }
+     accum_outlierness_factor(&factors[i_feature],
+                              &point->x[i_measurement],
+-                             A, featureSize, *scale);
++                             A.data(), featureSize, *scale);
+   }
+
+   result = true;
+@@ -2008,7 +2009,7 @@ static bool getOutliernessFactors_sparse( // output
+     // where A = Jo inv(JtJ) Jot
+     //
+     // A is symmetric. I store the upper triangle
+-    double A[featureSize*(featureSize+1)/2];
++    std::vector<double> A(featureSize*(featureSize+1)/2);
+     int iA=0;
+     for(int i=0; i<featureSize; i++)
+       for(int j=i; j<featureSize; j++, iA++)
+@@ -2027,7 +2028,7 @@ static bool getOutliernessFactors_sparse( // output
+       }
+     accum_outlierness_factor(&factors[i_feature],
+                              &point->x[i_measurement],
+-                             A, featureSize, *scale);
++                             A.data(), featureSize, *scale);
+   }
+
+   result = true;
+@@ -2212,8 +2213,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double*            Jqu
+
+   // This is Jt because cholmod thinks in terms of col-first instead of
+   // row-first
+-  int Jt_p[featureSize+1];
+-  int Jt_i[NstateActive*featureSize];
++  std::vector<int> Jt_p(featureSize+1);
++  std::vector<int> Jt_i(NstateActive*featureSize);
+   for(int i=0; i<=featureSize; i++)
+   {
+     Jt_p[i] = i*NstateActive;
+@@ -2224,8 +2225,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double*            Jqu
+   cholmod_sparse Jt_query_sparse = {.nrow   = ctx->Nstate,
+                                     .ncol   = featureSize,
+                                     .nzmax  = NstateActive*featureSize,
+-                                    .p      = (void*)Jt_p,
+-                                    .i      = (void*)Jt_i,
++                                    .p      = (void*)Jt_p.data(),
++                                    .i      = (void*)Jt_i.data(),
+                                     .x      = (double*)JqueryFeature,
+                                     .sorted = 1,
+                                     .packed = 1,