Use mrcal for camera-calibration (PhotonVision#1036)

Uses jars built from https://github.com/photonvision/mrcal-java/
See: https://mrcal.secretsauce.net/ and https://docs.photonvision.org/en/latest/docs/calibration/calibration.html#investigating-calibration-data-with-mrcal

---------

Co-authored-by: Sriman Achanta <[email protected]>

.github/workflows/build.yml

@@ -78,6 +78,8 @@ jobs:
         with:
           java-version: 17
           distribution: temurin
+      - name: Install mrcal deps
+        run: sudo apt-get update && sudo apt-get install -y libcholmod3 liblapack3 libsuitesparseconfig5
       - name: Gradle Build
         run: |
           chmod +x gradlew

README.md

@@ -93,6 +93,14 @@ Our meeting notes can be found in the wiki section of this repository.
 * [2020 Meeting Notes](https://github.com/PhotonVision/photonvision/wiki/2020-Meeting-Notes)
 * [2021 Meeting Notes](https://github.com/PhotonVision/photonvision/wiki/2021-Meeting-Notes)
 
+## Additional packages
+
+For now, using mrcal requires installing these additional packages on Linux systems:
+
+```
+sudo apt install libcholmod3 liblapack3 libsuitesparseconfig5
+```
+
 ## Documentation
 
 - Our main documentation page: [docs.photonvision.org](https://docs.photonvision.org)

build.gradle

@@ -29,18 +29,21 @@ ext {
     javalinVersion = "5.6.2"
     photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678"
     frcYear = "2024"
+    mrcalVersion = "dev-v2024.0.0-7-gc976aaa";
 
     pubVersion = versionString
     isDev = pubVersion.startsWith("dev")
 
     // A list, for legacy reasons, with only the current platform contained
-    String nativeName = wpilibTools.platformMapper.currentPlatform.platformName;
-    if (nativeName == "linuxx64") nativeName = "linuxx86-64";
-    if (nativeName == "winx64") nativeName = "windowsx86-64";
-    if (nativeName == "macx64") nativeName = "osxx86-64";
-    if (nativeName == "macarm64") nativeName = "osxarm64";
+    wpilibNativeName = wpilibTools.platformMapper.currentPlatform.platformName;
+    def nativeName = wpilibNativeName
+    if (wpilibNativeName == "linuxx64") nativeName = "linuxx86-64";
+    if (wpilibNativeName == "winx64") nativeName = "windowsx86-64";
+    if (wpilibNativeName == "macx64") nativeName = "osxx86-64";
+    if (wpilibNativeName == "macarm64") nativeName = "osxarm64";
     jniPlatform = nativeName
-    println("Building for platform: " + jniPlatform)
+
+    println("Building for platform " + jniPlatform + " wpilib: " + wpilibNativeName)
     println("Using Wpilib: " + wpilibVersion)
     println("Using OpenCV: " + openCVversion)
 }

devTools/calibrationUtils.py

@@ -1,13 +1,17 @@
+import argparse
 import base64
 from dataclasses import dataclass
 import json
 import os
+from typing import Union
 import cv2
 import numpy as np
+import mrcal
+from wpimath.geometry import Quaternion as _Quat
 
 
 @dataclass
-class Resolution:
+class Size:
     width: int
     height: int
 
@@ -86,10 +90,98 @@ class Observation:
 
 @dataclass
 class CameraCalibration:
-    resolution: Resolution
+    resolution: Size
     cameraIntrinsics: JsonMatOfDoubles
     distCoeffs: JsonMatOfDoubles
     observations: list[Observation]
+    calobjectWarp: list[float]
+    calobjectSize: Size
+    calobjectSpacing: float
+
+
+def __convert_cal_to_mrcal_cameramodel(
+    cal: CameraCalibration,
+) -> mrcal.cameramodel | None:
+    if len(cal.distCoeffs.data) == 5:
+        model = "LENSMODEL_OPENCV5"
+    elif len(cal.distCoeffs.data) == 8:
+        model = "LENSMODEL_OPENCV8"
+    else:
+        print("Unknown camera model? giving up")
+        return None
+
+    def opencv_to_mrcal_intrinsics(ocv):
+        return [ocv[0], ocv[4], ocv[2], ocv[5]]
+
+    def pose_to_rt(pose: Pose3d):
+        r = _Quat(
+            w=pose.rotation.quaternion.W,
+            x=pose.rotation.quaternion.X,
+            y=pose.rotation.quaternion.Y,
+            z=pose.rotation.quaternion.Z,
+        ).toRotationVector()
+        t = [
+            pose.translation.x,
+            pose.translation.y,
+            pose.translation.z,
+        ]
+        return np.concatenate((r, t))
+
+    imagersize = (cal.resolution.width, cal.resolution.height)
+
+    # Always weight=1 for Photon data
+    WEIGHT = 1
+    observations_board = np.array(
+        [
+            # note that we expect row-major observations here. I think this holds
+            np.array(
+                list(map(lambda it: [it.x, it.y, WEIGHT], o.locationInImageSpace))
+            ).reshape((cal.calobjectSize.width, cal.calobjectSize.height, 3))
+            for o in cal.observations
+        ]
+    )
+
+    optimization_inputs = {
+        "intrinsics": np.array(
+            [
+                opencv_to_mrcal_intrinsics(cal.cameraIntrinsics.data)
+                + cal.distCoeffs.data
+            ],
+            dtype=np.float64,
+        ),
+        "extrinsics_rt_fromref": np.zeros((0, 6), dtype=np.float64),
+        "frames_rt_toref": np.array(
+            [pose_to_rt(o.optimisedCameraToObject) for o in cal.observations]
+        ),
+        "points": None,
+        "observations_board": observations_board,
+        "indices_frame_camintrinsics_camextrinsics": np.array(
+            [[i, 0, -1] for i in range(len(cal.observations))], dtype=np.int32
+        ),
+        "observations_point": None,
+        "indices_point_camintrinsics_camextrinsics": None,
+        "lensmodel": model,
+        "imagersizes": np.array([imagersize], dtype=np.int32),
+        "calobject_warp": np.array(cal.calobjectWarp)
+        if len(cal.calobjectWarp) > 0
+        else None,
+        # We always do all the things
+        "do_optimize_intrinsics_core": True,
+        "do_optimize_intrinsics_distortions": True,
+        "do_optimize_extrinsics": True,
+        "do_optimize_frames": True,
+        "do_optimize_calobject_warp": len(cal.calobjectWarp) > 0,
+        "do_apply_outlier_rejection": True,
+        "do_apply_regularization": True,
+        "verbose": False,
+        "calibration_object_spacing": cal.calobjectSpacing,
+        "imagepaths": np.array([it.snapshotName for it in cal.observations]),
+    }
+
+    return mrcal.cameramodel(
+        optimization_inputs=optimization_inputs,
+        icam_intrinsics=0,
+    )
 
 
 def convert_photon_to_mrcal(photon_cal_json_path: str, output_folder: str):
@@ -132,3 +224,32 @@ def from_dict(cls, dict):
                     vnl_file.write(f"{obs.snapshotName} {corner.x} {corner.y} 0\n")
 
             vnl_file.flush()
+
+        mrcal_model = __convert_cal_to_mrcal_cameramodel(camera_cal_data)
+
+        with open(f"{output_folder}/camera-0.cameramodel", "w+") as mrcal_file:
+            mrcal_model.write(
+                mrcal_file,
+                note="Generated from PhotonVision calibration file: "
+                + photon_cal_json_path
+                + "\nCalobject_warp (m): "
+                + str(camera_cal_data.calobjectWarp),
+            )
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Convert Photon calibration JSON for use with mrcal"
+    )
+    parser.add_argument("input", type=str, help="Path to Photon calibration JSON file")
+    parser.add_argument(
+        "output_folder", type=str, help="Output folder for mrcal VNL file + images"
+    )
+
+    args = parser.parse_args()
+
+    convert_photon_to_mrcal(args.input, args.output_folder)
+
+
+if __name__ == "__main__":
+    main()

photon-client/src/components/cameras/CameraCalibrationCard.vue

@@ -13,6 +13,7 @@ import PvNumberInput from "@/components/common/pv-number-input.vue";
 import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
 import { getResolutionString, resolutionsAreEqual } from "@/lib/PhotonUtils";
 import CameraCalibrationInfoCard from "@/components/cameras/CameraCalibrationInfoCard.vue";
+import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
 
 const settingsValid = ref(true);
 
@@ -74,6 +75,15 @@ const squareSizeIn = ref(1);
 const patternWidth = ref(8);
 const patternHeight = ref(8);
 const boardType = ref<CalibrationBoardTypes>(CalibrationBoardTypes.Chessboard);
+const useMrCalRef = ref(true);
+const useMrCal = computed<boolean>({
+  get() {
+    return useMrCalRef.value && useSettingsStore().general.mrCalWorking;
+  },
+  set(value) {
+    useMrCalRef.value = value && useSettingsStore().general.mrCalWorking;
+  }
+});
 
 const downloadCalibBoard = () => {
   const doc = new JsPDF({ unit: "in", format: "letter" });
@@ -188,7 +198,8 @@ const startCalibration = () => {
     squareSizeIn: squareSizeIn.value,
     patternHeight: patternHeight.value,
     patternWidth: patternWidth.value,
-    boardType: boardType.value
+    boardType: boardType.value,
+    useMrCal: useMrCal.value
   });
   // The Start PnP method already handles updating the backend so only a store update is required
   useCameraSettingsStore().currentCameraSettings.currentPipelineIndex = WebsocketPipelineType.Calib3d;
@@ -314,6 +325,23 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
               :rules="[(v) => v >= 4 || 'Height must be at least 4']"
               :label-cols="5"
             />
+            <pv-switch
+              v-model="useMrCal"
+              label="Try using MrCal over OpenCV"
+              :disabled="!useSettingsStore().general.mrCalWorking || isCalibrating"
+              tooltip="If enabled, Photon will (try to) use MrCal instead of OpenCV for camera calibration."
+              :label-cols="5"
+            />
+            <v-banner
+              v-show="!useSettingsStore().general.mrCalWorking"
+              rounded
+              color="red"
+              text-color="white"
+              class="mt-3"
+              icon="mdi-alert-circle-outline"
+            >
+              MrCal JNI could not be loaded! Consult journalctl logs for additional details.
+            </v-banner>
           </v-form>
           <v-row justify="center">
             <v-chip

photon-client/src/components/cameras/CameraCalibrationInfoCard.vue

@@ -241,6 +241,22 @@ const getObservationDetails = (): ObservationDetails[] | undefined => {
               <td>Diagonal FOV</td>
               <td>{{ videoFormat.diagonalFOV !== undefined ? videoFormat.diagonalFOV.toFixed(2) + "°" : "-" }}</td>
             </tr>
+            <!-- Board warp, only shown for mrcal-calibrated cameras -->
+            <tr
+              v-if="
+                useCameraSettingsStore().getCalibrationCoeffs(props.videoFormat.resolution)?.calobjectWarp?.length === 2
+              "
+            >
+              <td>Board warp, X/Y</td>
+              <td>
+                {{
+                  useCameraSettingsStore()
+                    .getCalibrationCoeffs(props.videoFormat.resolution)
+                    ?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm")
+                    .join(" / ")
+                }}
+              </td>
+            </tr>
           </tbody>
         </template>
       </v-simple-table>

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -317,6 +317,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
         patternWidth: number;
         patternHeight: number;
         boardType: CalibrationBoardTypes;
+        useMrCal: boolean;
       },
       cameraIndex: number = useStateStore().currentCameraIndex
     ) {

photon-client/src/stores/settings/GeneralSettingsStore.ts

@@ -26,7 +26,8 @@ export const useSettingsStore = defineStore("settings", {
       version: undefined,
       gpuAcceleration: undefined,
       hardwareModel: undefined,
-      hardwarePlatform: undefined
+      hardwarePlatform: undefined,
+      mrCalWorking: true
     },
     network: {
       ntServerAddress: "",
@@ -97,7 +98,8 @@ export const useSettingsStore = defineStore("settings", {
         version: data.general.version || undefined,
         hardwareModel: data.general.hardwareModel || undefined,
         hardwarePlatform: data.general.hardwarePlatform || undefined,
-        gpuAcceleration: data.general.gpuAcceleration || undefined
+        gpuAcceleration: data.general.gpuAcceleration || undefined,
+        mrCalWorking: data.general.mrCalWorking
       };
       this.lighting = data.lighting;
       this.network = data.networkSettings;

photon-client/src/types/SettingTypes.ts

@@ -6,6 +6,7 @@ export interface GeneralSettings {
   gpuAcceleration?: string;
   hardwareModel?: string;
   hardwarePlatform?: string;
+  mrCalWorking: boolean;
 }
 
 export interface MetricData {
@@ -131,6 +132,7 @@ export interface CameraCalibrationResult {
   cameraIntrinsics: JsonMatOfDouble;
   distCoeffs: JsonMatOfDouble;
   observations: BoardObservation[];
+  calobjectWarp?: number[];
 }
 
 export interface ConfigurableCameraSettings {

photon-core/build.gradle

@@ -16,6 +16,19 @@ dependencies {
 
     implementation "org.photonvision:photon-libcamera-gl-driver-jni:$photonGlDriverLibVersion:linuxarm64"
     implementation "org.photonvision:photon-libcamera-gl-driver-java:$photonGlDriverLibVersion"
+
+    implementation "org.photonvision:photon-mrcal-java:$mrcalVersion"
+
+    // Only include mrcal natives on platforms that we build for
+    if (!(jniPlatform in [
+                "osxx86-64",
+                "osxarm64",
+                "linuxarm32"
+            ])) {
+        implementation "org.photonvision:photon-mrcal-jni:$mrcalVersion:$wpilibNativeName"
+    }
+
+    testImplementation group: 'org.junit-pioneer' , name: 'junit-pioneer', version: '2.2.0'
 }
 
 task writeCurrentVersion {

photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java

@@ -27,6 +27,7 @@
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.networking.NetworkUtils;
 import org.photonvision.common.util.SerializationUtils;
+import org.photonvision.mrcal.MrCalJNILoader;
 import org.photonvision.raspi.LibCameraJNILoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.processes.VisionModule;
@@ -140,6 +141,7 @@ public Map<String, Object> toHashMap() {
                 LibCameraJNILoader.isSupported()
                         ? "Zerocopy Libcamera Working"
                         : ""); // TODO add support for other types of GPU accel
+        generalSubmap.put("mrCalWorking", MrCalJNILoader.isWorking());
         generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
         generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
         settingsSubmap.put("general", generalSubmap);