diff --git a/build.gradle b/build.gradle
index bdb042259..8a3a2243b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -43,7 +43,6 @@ ext {
frcYear = "2026beta"
mrcalVersion = "dev-v2025.0.0-3-g2dc275f";
-
pubVersion = versionString
isDev = pubVersion.startsWith("dev")
diff --git a/photon-client/src/components/app/photon-3d-visualizer.vue b/photon-client/src/components/app/photon-3d-visualizer.vue
index 52e98e7a7..cc954c7f4 100644
--- a/photon-client/src/components/app/photon-3d-visualizer.vue
+++ b/photon-client/src/components/app/photon-3d-visualizer.vue
@@ -8,8 +8,10 @@ import { onBeforeUnmount, onMounted, watchEffect } from "vue";
const {
ArrowHelper,
BoxGeometry,
+ CameraHelper,
Color,
ConeGeometry,
+ Group,
Mesh,
MeshNormalMaterial,
PerspectiveCamera,
@@ -20,6 +22,18 @@ const {
} = await import("three");
const { TrackballControls } = await import("three/examples/jsm/controls/TrackballControls");
+import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
+import { createPerspectiveCamera } from "@/lib/ThreeUtils";
+import { useTheme } from "vuetify";
+
+const theme = useTheme();
+
+const calibrationCoeffs = useCameraSettingsStore().getCalibrationCoeffs(
+ useCameraSettingsStore().currentCameraSettings.validVideoFormats[
+ useCameraSettingsStore().currentPipelineSettings.cameraVideoModeIndex
+ ].resolution
+);
+
const props = defineProps<{
targets: PhotonTarget[];
}>();
@@ -32,15 +46,18 @@ let controls: TrackballControls | undefined;
let previousTargets: Object3D[] = [];
const drawTargets = (targets: PhotonTarget[]) => {
// Check here, since if we check in watchEffect this never gets called
- if (scene === undefined || camera === undefined || renderer === undefined || controls === undefined) {
+ if (!scene || !camera || !renderer || !controls) {
return;
}
+ if (theme.global.name.value === "LightTheme") scene.background = new Color(0xa9a9a9);
+ else scene.background = new Color(0x000000);
+
scene.remove(...previousTargets);
previousTargets = [];
targets.forEach((target) => {
- if (target.pose === undefined) return;
+ if (!target.pose) return;
const geometry = new BoxGeometry(0.3 / 5, 0.2, 0.2);
const material = new MeshNormalMaterial();
@@ -70,6 +87,18 @@ const drawTargets = (targets: PhotonTarget[]) => {
previousTargets.push(arrow);
});
+ if (calibrationCoeffs) {
+ // And show camera frustum
+ const calibCamera = createPerspectiveCamera(calibrationCoeffs.resolution, calibrationCoeffs.cameraIntrinsics, 10);
+ const helper = new CameraHelper(calibCamera);
+ const helperGroup = new Group();
+ helperGroup.add(helper);
+ // Flip to +Z forward
+ helperGroup.rotateX(-Math.PI / 2.0);
+ helperGroup.rotateY(-Math.PI / 2.0);
+ previousTargets.push(helperGroup);
+ }
+
if (previousTargets.length > 0) {
scene.add(...previousTargets);
}
@@ -78,7 +107,7 @@ const onWindowResize = () => {
const container = document.getElementById("container");
const canvas = document.getElementById("view");
- if (container === null || canvas === null || camera === undefined || renderer === undefined) {
+ if (!container || !canvas || !camera || !renderer) {
return;
}
@@ -89,7 +118,7 @@ const onWindowResize = () => {
renderer.setSize(canvas.clientWidth, canvas.clientHeight);
};
const resetCamFirstPerson = () => {
- if (scene === undefined || camera === undefined || controls === undefined) {
+ if (!scene || !camera || !controls) {
return;
}
@@ -103,7 +132,7 @@ const resetCamFirstPerson = () => {
}
};
const resetCamThirdPerson = () => {
- if (scene === undefined || camera === undefined || controls === undefined) {
+ if (!scene || !camera || !controls) {
return;
}
@@ -122,10 +151,11 @@ onMounted(async () => {
camera = new PerspectiveCamera(75, 800 / 800, 0.1, 1000);
const canvas = document.getElementById("view");
- if (canvas === null) return;
+ if (!canvas) return;
renderer = new WebGLRenderer({ canvas: canvas });
- scene.background = new Color(0xa9a9a9);
+ if (theme.global.name.value === "LightTheme") scene.background = new Color(0xa9a9a9);
+ else scene.background = new Color(0x000000);
onWindowResize();
window.addEventListener("resize", onWindowResize);
@@ -169,7 +199,7 @@ onMounted(async () => {
controls.update();
const animate = () => {
- if (scene === undefined || camera === undefined || renderer === undefined || controls === undefined) {
+ if (!scene || !camera || !renderer || !controls) {
return;
}
@@ -192,18 +222,31 @@ watchEffect(() => {
-
-
-
+
+
+ Target Visualization
-
-
-
- First Person
+
+
+ First Person
+
-
- Third Person
+
+
+ Third Person
+
-
+
+
diff --git a/photon-client/src/components/app/photon-calibration-visualizer.vue b/photon-client/src/components/app/photon-calibration-visualizer.vue
new file mode 100644
index 000000000..e63798c6d
--- /dev/null
+++ b/photon-client/src/components/app/photon-calibration-visualizer.vue
@@ -0,0 +1,371 @@
+
+
+
+
+
+
+
+ {{ props.title }}
+
+
+
+
+ First Person
+
+
+
+
+ Third Person
+
+
+
+
+
+
+
+
diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue
index c0ca6f8fb..55e3e16e2 100644
--- a/photon-client/src/components/cameras/CameraCalibrationCard.vue
+++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue
@@ -38,7 +38,8 @@ const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
if (!skip) {
const calib = useCameraSettingsStore().getCalibrationCoeffs(format.resolution);
if (calib !== undefined) {
- // For each error, square it, sum the squares, and divide by total points N
+ // Mean overall reprojection error
+ // Calculated as average of each observation's mean error
if (calib.meanErrors.length)
format.mean = calib.meanErrors.reduce((a, b) => a + b, 0) / calib.meanErrors.length;
else format.mean = NaN;
@@ -249,27 +250,31 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
Horizontal FOV
Vertical FOV
Diagonal FOV
- Info
-
- {{ getResolutionString(value.resolution) }}
-
- {{ value.mean !== undefined ? (isNaN(value.mean) ? "Unknown" : value.mean.toFixed(2) + "px") : "-" }}
-
- {{ value.horizontalFOV !== undefined ? value.horizontalFOV.toFixed(2) + "°" : "-" }}
- {{ value.verticalFOV !== undefined ? value.verticalFOV.toFixed(2) + "°" : "-" }}
- {{ value.diagonalFOV !== undefined ? value.diagonalFOV.toFixed(2) + "°" : "-" }}
-
-
-
- mdi-information
+
+
+
+ {{ getResolutionString(value.resolution) }}
+
+ {{
+ value.mean !== undefined ? (isNaN(value.mean) ? "Unknown" : value.mean.toFixed(2) + "px") : "-"
+ }}
-
- View calibration information
-
-
+ {{ value.horizontalFOV !== undefined ? value.horizontalFOV.toFixed(2) + "°" : "-" }}
+ {{ value.verticalFOV !== undefined ? value.verticalFOV.toFixed(2) + "°" : "-" }}
+ {{ value.diagonalFOV !== undefined ? value.diagonalFOV.toFixed(2) + "°" : "-" }}
+
+
+ View calibration information
+
diff --git a/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue
index cb5b02b27..f93b1ca19 100644
--- a/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue
+++ b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue
@@ -1,4 +1,5 @@
+
-
-
- Calibration Details
-
-
-
- mdi-import
- Import
-
-
-
- mdi-export
- Export
-
-
- (confirmRemoveDialog = { show: true, vf: props.videoFormat })"
- >
- mdi-delete
- Delete
-
-
-
- {{ useCameraSettingsStore().currentCameraName }}@{{ getResolutionString(videoFormat.resolution) }}
-
-
-
-
-
-
-
-
- Name
- Value
-
-
-
-
- Fx
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.cameraIntrinsics.data[0].toFixed(2) || 0.0
- }}
- mm
-
-
-
- Fy
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.cameraIntrinsics.data[4].toFixed(2) || 0.0
- }}
- mm
-
-
-
- Cx
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.cameraIntrinsics.data[2].toFixed(2) || 0.0
- }}
- px
-
-
-
- Cy
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.cameraIntrinsics.data[5].toFixed(2) || 0.0
- }}
- px
-
-
-
- Distortion
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.distCoeffs.data.map((it) => parseFloat(it.toFixed(3))) || []
- }}
-
-
-
- Mean Err
-
- {{
- videoFormat.mean !== undefined
- ? isNaN(videoFormat.mean)
- ? "NaN"
- : videoFormat.mean.toFixed(2) + "px"
- : "-"
- }}
-
-
-
- Horizontal FOV
-
- {{ videoFormat.horizontalFOV !== undefined ? videoFormat.horizontalFOV.toFixed(2) + "°" : "-" }}
-
-
-
- Vertical FOV
- {{ videoFormat.verticalFOV !== undefined ? videoFormat.verticalFOV.toFixed(2) + "°" : "-" }}
-
-
- Diagonal FOV
- {{ videoFormat.diagonalFOV !== undefined ? videoFormat.diagonalFOV.toFixed(2) + "°" : "-" }}
-
-
-
- Board warp, X/Y
-
- {{
- useCameraSettingsStore()
- .getCalibrationCoeffs(props.videoFormat.resolution)
- ?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm")
- .join(" / ")
- }}
-
-
-
-
-
-
- Individual Observations
-
-
-
+
+
+
+ Calibration Details
+
+
-
+ color="buttonPassive"
+ style="width: 100%"
+ :variant="theme.global.name.value === 'LightTheme' ? 'elevated' : 'outlined'"
+ @click="openUploadPhotonCalibJsonPrompt"
+ >
+ mdi-import
+ Import
+
+
+
+
+
+ mdi-export
+ Export
+
+
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+ Details
+ Observations
+
+
+
+
+
+
+
+ Camera
+
+ {{ useCameraSettingsStore().currentCameraName }}
+
+
+
+ Resolution
+
+ {{ getResolutionString(videoFormat.resolution) }}
+
+
+
+ Fx
+
+ {{
+ useCameraSettingsStore()
+ .getCalibrationCoeffs(props.videoFormat.resolution)
+ ?.cameraIntrinsics.data[0].toFixed(2) || 0.0
+ }}
+ mm
+
+
+
+ Fy
+
+ {{
+ useCameraSettingsStore()
+ .getCalibrationCoeffs(props.videoFormat.resolution)
+ ?.cameraIntrinsics.data[4].toFixed(2) || 0.0
+ }}
+ mm
+
+
+
+ Cx
+
+ {{
+ useCameraSettingsStore()
+ .getCalibrationCoeffs(props.videoFormat.resolution)
+ ?.cameraIntrinsics.data[2].toFixed(2) || 0.0
+ }}
+ px
+
+
+
+ Cy
+
+ {{
+ useCameraSettingsStore()
+ .getCalibrationCoeffs(props.videoFormat.resolution)
+ ?.cameraIntrinsics.data[5].toFixed(2) || 0.0
+ }}
+ px
+
+
+
+ Distortion
+
+ {{
+ useCameraSettingsStore()
+ .getCalibrationCoeffs(props.videoFormat.resolution)
+ ?.distCoeffs.data.map((it) => parseFloat(it.toFixed(3))) || []
+ }}
+
+
+
+ Mean Err
+
+ {{
+ videoFormat.mean !== undefined
+ ? isNaN(videoFormat.mean)
+ ? "NaN"
+ : videoFormat.mean.toFixed(2) + "px"
+ : "-"
+ }}
+
+
+
+ Horizontal FOV
+
+ {{ videoFormat.horizontalFOV !== undefined ? videoFormat.horizontalFOV.toFixed(2) + "°" : "-" }}
+
+
+
+ Vertical FOV
+
+ {{ videoFormat.verticalFOV !== undefined ? videoFormat.verticalFOV.toFixed(2) + "°" : "-" }}
+
+
+
+ Diagonal FOV
+
+ {{ videoFormat.diagonalFOV !== undefined ? videoFormat.diagonalFOV.toFixed(2) + "°" : "-" }}
+
+
+
+
+ Board warp, X/Y
+
+ {{
+ currentCalibrationCoeffs?.calobjectWarp?.map((it) => (it * 1000).toFixed(2) + " mm").join(" / ")
+ }}
+
+
+
+
+
+
+
+
+
+
+ mdi-eye
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Loading...
+
+
+
+
+
+
@@ -322,11 +349,7 @@ const calibrationImageURL = (index: number) =>
diff --git a/photon-client/src/components/dashboard/tabs/Map3DTab.vue b/photon-client/src/components/dashboard/tabs/Map3DTab.vue
index 699a36a59..765ae355a 100644
--- a/photon-client/src/components/dashboard/tabs/Map3DTab.vue
+++ b/photon-client/src/components/dashboard/tabs/Map3DTab.vue
@@ -9,20 +9,11 @@ const trackedTargets = computed(() => useStateStore().currentPip
-
-
- Target Visualization
-
-
-
-
-
-
-
+
+
+
- Loading...
-
-
-
+
Loading...
+
diff --git a/photon-client/src/lib/ThreeUtils.ts b/photon-client/src/lib/ThreeUtils.ts
new file mode 100644
index 000000000..b958c1b4f
--- /dev/null
+++ b/photon-client/src/lib/ThreeUtils.ts
@@ -0,0 +1,23 @@
+import type { JsonMatOfDouble, Resolution } from "@/types/SettingTypes";
+const { PerspectiveCamera } = await import("three");
+
+/**
+ * Convert a camera intrinsics matrix and image resolution to a Three.js PerspectiveCamera. This assumes no skew and square pixels (same focal length in x and y), which is a sane assumption for most FRC cameras
+ *
+ * @param resolution video mode width/height
+ * @param intrinsicsCore camera intrinsics from the backend, row-major
+ * @returns a Three.js PerspectiveCamera matching the provided intrinsics
+ */
+export const createPerspectiveCamera = (
+ resolution: Resolution,
+ intrinsicsCore: JsonMatOfDouble,
+ frustumMax: number = 1
+) => {
+ const imageWidth = resolution.width;
+ const imageHeight = resolution.height;
+ const focalLengthY = intrinsicsCore.data[4];
+ const fovY = 2 * Math.atan(imageHeight / (2 * focalLengthY)) * (180 / Math.PI);
+ const aspect = imageWidth / imageHeight;
+
+ return new PerspectiveCamera(fovY, aspect, 0.1, frustumMax);
+};
diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts
index b4444400c..29337c365 100644
--- a/photon-client/src/types/SettingTypes.ts
+++ b/photon-client/src/types/SettingTypes.ts
@@ -191,7 +191,7 @@ export interface BoardObservation {
locationInImageSpace: CvPoint[];
reprojectionErrors: CvPoint[];
optimisedCameraToObject: Pose3d;
- includeObservationInCalibration: boolean;
+ cornersUsed: boolean[];
snapshotName: string;
snapshotData: JsonImageMat;
}
@@ -202,9 +202,15 @@ export interface CameraCalibrationResult {
distCoeffs: JsonMatOfDouble;
observations: BoardObservation[];
calobjectWarp?: number[];
- // We might have to omit observations for bandwidth, so backend will send us this
+ calobjectSize: { width: number; height: number };
+ calobjectSpacing: number;
+ lensModel: string;
+
+ // We have to omit observations for bandwidth, so backend will send us this from UICameraCalibrationCoefficients
numSnapshots: number;
meanErrors: number[];
+ numMissing: number[];
+ numOutliers: number[];
}
export enum ValidQuirks {
@@ -315,7 +321,7 @@ export const PlaceholderCameraSettings: UiCameraConfiguration = reactive({
rows: 1,
cols: 1,
type: 1,
- data: [1, 2, 3, 4, 5, 6, 7, 8, 9]
+ data: [600, 0, 1920 / 2, 0, 600, 1080 / 2, 0, 0, 1]
},
distCoeffs: {
rows: 1,
@@ -325,28 +331,72 @@ export const PlaceholderCameraSettings: UiCameraConfiguration = reactive({
},
observations: [
{
- locationInImageSpace: [
- { x: 100, y: 100 },
- { x: 210, y: 100 },
- { x: 320, y: 101 }
+ locationInObjectSpace: [
+ {
+ x: 0,
+ y: 0,
+ z: 0
+ },
+ {
+ x: 0.02539999969303608,
+ y: 0,
+ z: 0
+ },
+ {
+ x: 0.05079999938607216,
+ y: 0,
+ z: 0
+ }
+ ],
+ locationInImageSpace: [
+ {
+ x: 57.062007904052734,
+ y: 108.12601470947266
+ },
+ {
+ x: 108.72974395751953,
+ y: 108.0336685180664
+ },
+ {
+ x: 158.78118896484375,
+ y: 107.8104019165039
+ }
],
- locationInObjectSpace: [{ x: 0, y: 0, z: 0 }],
optimisedCameraToObject: {
- translation: { x: 1, y: 2, z: 3 },
- rotation: { quaternion: { W: 1, X: 0, Y: 0, Z: 0 } }
+ translation: {
+ x: -0.28942385915178886,
+ y: -0.12895727420625547,
+ z: 0.5933086404370699
+ },
+ rotation: {
+ quaternion: {
+ W: 0.9890028788589879,
+ X: -0.0507354429843431,
+ Y: -0.13458187019694584,
+ Z: -0.034452004994036174
+ }
+ }
},
reprojectionErrors: [
{ x: 1, y: 1 },
{ x: 2, y: 1 },
{ x: 3, y: 1 }
],
- includeObservationInCalibration: false,
+ cornersUsed: [true, true, false],
snapshotName: "img0.png",
snapshotData: { rows: 480, cols: 640, type: CvType.CV_8U, data: "" }
}
],
+ calobjectSize: {
+ width: 10,
+ height: 10
+ },
+ calobjectSpacing: 0.0254,
+ lensModel: "opencv8",
numSnapshots: 1,
- meanErrors: [123.45]
+ meanErrors: [123.45],
+ numMissing: [0],
+ numOutliers: [1]
}
],
pipelineNicknames: ["Placeholder Pipeline"],
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
index 4119f6517..23bcb7b4e 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
@@ -18,14 +18,23 @@
package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.geometry.Pose3d;
+import java.awt.Color;
import java.nio.file.Path;
+import java.util.Arrays;
import java.util.List;
import org.jetbrains.annotations.Nullable;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Point3;
+import org.opencv.core.Scalar;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.util.ColorHelper;
// Ignore the previous calibration data that was stored in the json file.
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -47,8 +56,8 @@ public final class BoardObservation implements Cloneable {
public Pose3d optimisedCameraToObject;
// If we should use this observation when re-calculating camera calibration
- @JsonProperty("includeObservationInCalibration")
- public boolean includeObservationInCalibration;
+ @JsonProperty("cornersUsed")
+ public boolean[] cornersUsed;
@JsonProperty("snapshotName")
public String snapshotName;
@@ -63,16 +72,22 @@ public final class BoardObservation implements Cloneable {
@JsonProperty("locationInImageSpace") List locationInImageSpace,
@JsonProperty("reprojectionErrors") List reprojectionErrors,
@JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
- @JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
+ @JsonProperty("cornersUsed") boolean[] cornersUsed,
@JsonProperty("snapshotName") String snapshotName,
@JsonProperty("snapshotDataLocation") Path snapshotDataLocation) {
this.locationInObjectSpace = locationInObjectSpace;
this.locationInImageSpace = locationInImageSpace;
this.reprojectionErrors = reprojectionErrors;
this.optimisedCameraToObject = optimisedCameraToObject;
- this.includeObservationInCalibration = includeObservationInCalibration;
this.snapshotName = snapshotName;
this.snapshotDataLocation = snapshotDataLocation;
+
+ // legacy migration -- we assume all points are inliers
+ if (cornersUsed == null) {
+ cornersUsed = new boolean[locationInObjectSpace.size()];
+ Arrays.fill(cornersUsed, true);
+ }
+ this.cornersUsed = cornersUsed;
}
@Override
@@ -85,8 +100,8 @@ public final class BoardObservation implements Cloneable {
+ reprojectionErrors
+ ", optimisedCameraToObject="
+ optimisedCameraToObject
- + ", includeObservationInCalibration="
- + includeObservationInCalibration
+ + ", cornersUsed="
+ + cornersUsed
+ ", snapshotName="
+ snapshotName
+ ", snapshotDataLocation="
@@ -103,4 +118,73 @@ public final class BoardObservation implements Cloneable {
return null;
}
}
+
+ @JsonIgnore
+ /**
+ * Load the captured board image from disk. Allocates a new Mat, which the caller is responsible
+ * for releasing.
+ *
+ * @return The loaded image, or null if it could not be loaded.
+ */
+ public Mat loadImage() {
+ Mat img = Imgcodecs.imread(this.snapshotDataLocation.toString());
+ if (img == null || img.empty() || img.rows() == 0 || img.cols() == 0) {
+ return null;
+ }
+
+ return img;
+ }
+
+ /**
+ * Annotate the image with the detected corners, green for used, red for unused
+ *
+ * @return Annotated image, or null if the image could not be loaded. Caller is responsible for
+ * releasing the Mat.
+ */
+ @JsonIgnore
+ public Mat annotateImage() {
+ var image = loadImage();
+
+ if (image == null) {
+ return null;
+ }
+
+ int thickness = Core.FILLED;
+ var diag = Math.hypot(image.width(), image.height());
+ int r = (int) Math.max(diag * 4.0 / 500.0, 3);
+ for (int i = 0; i < this.locationInImageSpace.size(); i++) {
+ var c = locationInImageSpace.get(i);
+
+ // -1, -1 means unused corner
+ if (c.x < 0 || c.y < 0) {
+ continue;
+ }
+
+ Scalar color;
+ if (cornersUsed[i]) {
+ color = ColorHelper.colorToScalar(Color.green);
+ } else {
+ color = ColorHelper.colorToScalar(Color.red);
+ }
+ Imgproc.circle(image, c, r, color, thickness);
+ }
+
+ return image;
+ }
+
+ /**
+ * Mean reprojection error for this observation, skipping corners marked as unused. The overall
+ * mean is calculated as the mean of each individual corner's reprojection error, or the distance
+ * in pixels between the observed and expected location.
+ *
+ * @return Mean reprojection error in pixels.
+ */
+ @JsonIgnore
+ double meanReprojectionError() {
+ return reprojectionErrors.stream()
+ .filter(pt -> cornersUsed[reprojectionErrors.indexOf(pt)])
+ .mapToDouble(pt -> Math.hypot(pt.x, pt.y))
+ .average()
+ .orElse(0);
+ }
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
index 4bb7dbf9f..ad0f9db98 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
@@ -217,7 +217,7 @@ public class CameraCalibrationCoefficients implements Releasable {
}
@JsonIgnore
- public List getPerViewErrors() {
+ public List getObservations() {
return observations;
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java
index 123b1063d..823f9fec0 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java
@@ -18,13 +18,19 @@
package org.photonvision.vision.calibration;
import java.util.List;
+import java.util.stream.IntStream;
import org.opencv.core.Size;
public class UICameraCalibrationCoefficients extends CameraCalibrationCoefficients {
public int numSnapshots;
- /** Immutable list of mean errors. */
public List meanErrors;
+ public List numMissing;
+ public List numOutliers;
+
+ private static int countOutliers(BoardObservation obs) {
+ return (int) obs.locationInImageSpace.stream().filter(it -> it.x < 0 || it.y < 0).count();
+ }
public UICameraCalibrationCoefficients(
Size resolution,
@@ -47,14 +53,19 @@ public class UICameraCalibrationCoefficients extends CameraCalibrationCoefficien
lensmodel);
this.numSnapshots = observations.size();
- this.meanErrors =
+ this.meanErrors = observations.stream().map(BoardObservation::meanReprojectionError).toList();
+
+ this.numOutliers =
observations.stream()
.map(
- it2 ->
- it2.reprojectionErrors.stream()
- .mapToDouble(it -> Math.hypot(it.x, it.y))
- .average()
- .orElse(0))
+ obs ->
+ IntStream.range(0, obs.cornersUsed.length)
+ .filter(i -> !obs.cornersUsed[i])
+ .map(i -> 1)
+ .sum()
+ - countOutliers(obs))
.toList();
+ this.numMissing =
+ observations.stream().map(UICameraCalibrationCoefficients::countOutliers).toList();
}
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
index 961251676..9aaa03fa6 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
@@ -22,7 +22,6 @@ import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
-import java.util.stream.Collectors;
import org.apache.commons.io.FileUtils;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
@@ -145,8 +144,8 @@ public class Calibrate3dPipe
}
// And delete rows depending on the level -- otherwise, level has no impact for opencv
- List objPoints = new ArrayList<>();
- List imgPoints = new ArrayList<>();
+ List objPoints = new ArrayList<>();
+ List imgPoints = new ArrayList<>();
for (int i = 0; i < objPointsIn.size(); i++) {
MatOfPoint3f objPtsOut = new MatOfPoint3f();
MatOfPoint2f imgPtsOut = new MatOfPoint2f();
@@ -174,8 +173,8 @@ public class Calibrate3dPipe
// imageSize from, other parameters are output Mats
Calib3d.calibrateCameraExtended(
- objPoints,
- imgPoints,
+ objPoints.stream().map(it -> (Mat) it).toList(),
+ imgPoints.stream().map(it -> (Mat) it).toList(),
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
@@ -194,9 +193,29 @@ public class Calibrate3dPipe
JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix);
JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
+ // Opencv is lame, so we can only assume all points are inliers
+ var inliners =
+ objPoints.stream()
+ .map(
+ it -> {
+ var array = new boolean[it.rows() * it.cols()];
+ Arrays.fill(array, true);
+ return array;
+ })
+ .toList();
+
var observations =
createObservations(
- in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null, imageSavePath);
+ in,
+ cameraMatrix,
+ distortionCoefficients,
+ rvecs,
+ tvecs,
+ inliners,
+ new double[] {0, 0},
+ objPoints,
+ imgPoints,
+ imageSavePath);
cameraMatrix.release();
distortionCoefficients.release();
@@ -266,11 +285,10 @@ public class Calibrate3dPipe
JsonMatOfDouble distortionCoefficientsMat =
new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12));
- // Calculate optimized board poses manually. We get this for free from mrcal
- // too, but that's not JNIed (yet)
+ // We get these from the JNI (retsult.optimizedPoses), but these are subtly different from the
+ // ones our code used to produce. To preserve consistency, continue to redo this math
List rvecs = new ArrayList<>();
List tvecs = new ArrayList<>();
-
for (var o : in) {
var rvec = new Mat();
var tvec = new Mat();
@@ -309,6 +327,8 @@ public class Calibrate3dPipe
tvecs.add(tvec);
}
+ List objPoints = in.stream().map(it -> it.objectPoints).toList();
+ List imgPts = in.stream().map(it -> it.imagePoints).toList();
List observations =
createObservations(
in,
@@ -316,7 +336,10 @@ public class Calibrate3dPipe
distortionCoefficientsMat.getAsMatOfDouble(),
rvecs,
tvecs,
+ result.cornersUsed,
new double[] {result.warp_x, result.warp_y},
+ objPoints,
+ imgPts,
imageSavePath);
rvecs.forEach(Mat::release);
@@ -339,13 +362,12 @@ public class Calibrate3dPipe
MatOfDouble distortionCoefficients_,
List rvecs,
List tvecs,
+ List cornersUsed,
double[] calobject_warp,
+ List objPoints,
+ List imgPts,
Path imageSavePath) {
- List objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
- List imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
-
// Clear the calibration image folder of any old images before we save the new ones.
-
try {
FileUtils.cleanDirectory(imageSavePath.toFile());
} catch (Exception e) {
@@ -355,11 +377,24 @@ public class Calibrate3dPipe
// For each observation, calc reprojection error
Mat jac_temp = new Mat();
List observations = new ArrayList<>();
- for (int i = 0; i < objPoints.size(); i++) {
+ for (int snapshotId = 0; snapshotId < objPoints.size(); snapshotId++) {
+ // Copy object points to a new mat to allow warp modification without affecting underlying
+ // data
MatOfPoint3f i_objPtsNative = new MatOfPoint3f();
- objPoints.get(i).copyTo(i_objPtsNative);
- var i_objPts = i_objPtsNative.toList();
- var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList();
+ objPoints.get(snapshotId).copyTo(i_objPtsNative);
+
+ List i_imgPts = imgPts.get(snapshotId).toList();
+
+ if (i_objPtsNative.rows() != i_imgPts.size()) {
+ throw new RuntimeException(
+ "Objpts size ("
+ + i_objPtsNative.rows()
+ + ") != imgpts size ("
+ + i_imgPts.size()
+ + ") for snapshot "
+ + snapshotId
+ + "!");
+ }
// Apply warp, if set
if (calobject_warp != null && calobject_warp.length == 2) {
@@ -384,12 +419,13 @@ public class Calibrate3dPipe
i_objPtsNative.fromArray(list);
}
+ // Project distorted object points to image space
var img_pts_reprojected = new MatOfPoint2f();
try {
Calib3d.projectPoints(
i_objPtsNative,
- rvecs.get(i),
- tvecs.get(i),
+ rvecs.get(snapshotId),
+ tvecs.get(snapshotId),
cameraMatrix_,
distortionCoefficients_,
img_pts_reprojected,
@@ -399,22 +435,38 @@ public class Calibrate3dPipe
e.printStackTrace();
continue;
}
- var img_pts_reprojected_list = img_pts_reprojected.toList();
+ // Calculate reprojection error for each point
var reprojectionError = new ArrayList();
+ var img_pts_reprojected_list = img_pts_reprojected.toList();
for (int j = 0; j < img_pts_reprojected_list.size(); j++) {
+ // Outliers are not part of the calibration, so don't calculate error for them
+ if (!cornersUsed.get(snapshotId)[j]) {
+ continue;
+ }
+
// error = (measured - expected)
var measured = img_pts_reprojected_list.get(j);
var expected = i_imgPts.get(j);
+
+ // Sanity check -- negative corners make no sense here
+ if (!(measured.x >= 0 && measured.y >= 0 && expected.x >= 0 && expected.y >= 0)) {
+ throw new RuntimeException(
+ "Negative corner in reprojection error calc! Measured: "
+ + measured
+ + ", expected: "
+ + expected);
+ }
+
var error = new Point(measured.x - expected.x, measured.y - expected.y);
reprojectionError.add(error);
}
- var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
+ var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(snapshotId), tvecs.get(snapshotId));
- var inputImage = in.get(i).inputImage;
+ var inputImage = in.get(snapshotId).inputImage;
Path image_path = null;
- String snapshotName = "img" + i + ".png";
+ String snapshotName = "img" + snapshotId + ".png";
if (inputImage != null) {
image_path = Paths.get(imageSavePath.toString(), snapshotName);
Imgcodecs.imwrite(image_path.toString(), inputImage);
@@ -422,7 +474,13 @@ public class Calibrate3dPipe
observations.add(
new BoardObservation(
- i_objPts, i_imgPts, reprojectionError, camToBoard, true, snapshotName, image_path));
+ i_objPtsNative.toList(),
+ i_imgPts,
+ reprojectionError,
+ camToBoard,
+ cornersUsed.get(snapshotId),
+ snapshotName,
+ image_path));
}
jac_temp.release();
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
index 3909fc804..d6b30f2e9 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
@@ -287,7 +287,23 @@ public class FindBoardCornersPipe
imgPoints.copyTo(outBoardCorners);
objPoints.copyTo(objPts);
- // Since ChaArUco can still detect without the whole board we need to send "fake" (all
+ // Mrcal wants our top-left corner at 0, 0. But charuco hands us the first corner at the first
+ // board intersection, which is inset a couple mm. Adjust such that the top-left corner is at
+ // 0,0
+ {
+ // don't trust any particular ordering
+ List pointList = objPts.toList();
+ double minX = pointList.stream().mapToDouble(p -> p.x).min().orElse(0.0);
+ double minY = pointList.stream().mapToDouble(p -> p.y).min().orElse(0.0);
+
+ // Shift all object points so that the origin is at (0,0)
+ List shiftedPoints =
+ pointList.stream().map(p -> new Point3(p.x - minX, p.y - minY, p.z)).toList();
+
+ objPts.fromList(shiftedPoints);
+ }
+
+ // Since ChArUco can still detect without the whole board we need to send "fake" (all
// values less than zero) points and then tell it to ignore that corner by setting the
// corresponding level to -1. Calibrate3dPipe deals with piping this into the correct format
// for each backend
@@ -298,12 +314,17 @@ public class FindBoardCornersPipe
new Point3[(this.params.boardHeight() - 1) * (this.params.boardWidth() - 1)];
levels = new float[(this.params.boardHeight() - 1) * (this.params.boardWidth() - 1)];
+ // cache
+ var outBoardCornersList = outBoardCorners.toList();
+ var outObjPtsList = objPts.toList();
+
for (int i = 0; i < detectedIds.total(); i++) {
int id = (int) detectedIds.get(i, 0)[0];
- boardCorners[id] = outBoardCorners.toList().get(i);
- objectPoints[id] = objPts.toList().get(i);
+ boardCorners[id] = outBoardCornersList.get(i);
+ objectPoints[id] = outObjPtsList.get(i);
levels[id] = 1.0f;
}
+
for (int i = 0; i < boardCorners.length; i++) {
if (boardCorners[i] == null) {
boardCorners[i] = new Point(-1, -1);
@@ -320,7 +341,6 @@ public class FindBoardCornersPipe
objPoints.release();
detectedCorners.release();
detectedIds.release();
-
} else { // If not ChArUco then do chessboard
// Reduce the image size to be much more manageable
// Note that opencv will copy the frame if no resize is requested; we can skip
diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
index 1a694a1a7..ccb5e0c02 100644
--- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
+++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
@@ -279,6 +279,18 @@ public class Calibrate3dPipeTest {
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString());
System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
+ // calculate RMS error
+ double totalSquaredError = 0.0;
+ long totalPoints = 0;
+ for (var obs : cal.getObservations()) {
+ double sumErrorSq =
+ obs.reprojectionErrors.stream().mapToDouble(d -> d.x * d.x + d.y * d.y).sum();
+ totalSquaredError += sumErrorSq;
+ totalPoints += obs.reprojectionErrors.size();
+ }
+ double rmsError = Math.sqrt(totalSquaredError / totalPoints);
+ System.out.println("RMS Reprojection Error: " + rmsError);
+
// Confirm we didn't get leaky on our mat usage
// assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this
// doesn't
diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java
index a5a84a31f..2e8bf4afb 100644
--- a/photon-server/src/main/java/org/photonvision/Main.java
+++ b/photon-server/src/main/java/org/photonvision/Main.java
@@ -187,6 +187,15 @@ public class Main {
}
public static void main(String[] args) {
+ var logLevel = printDebugLogs ? LogLevel.TRACE : LogLevel.DEBUG;
+ Logger.setLevel(LogGroup.Camera, logLevel);
+ Logger.setLevel(LogGroup.WebServer, logLevel);
+ Logger.setLevel(LogGroup.VisionModule, logLevel);
+ Logger.setLevel(LogGroup.Data, logLevel);
+ Logger.setLevel(LogGroup.Config, logLevel);
+ Logger.setLevel(LogGroup.General, logLevel);
+ logger.info("Logging initialized in debug mode.");
+
logger.info(
"Starting PhotonVision version "
+ PhotonVersion.versionString
@@ -257,15 +266,6 @@ public class Main {
CVMat.enablePrint(false);
PipelineProfiler.enablePrint(false);
- var logLevel = printDebugLogs ? LogLevel.TRACE : LogLevel.DEBUG;
- Logger.setLevel(LogGroup.Camera, logLevel);
- Logger.setLevel(LogGroup.WebServer, logLevel);
- Logger.setLevel(LogGroup.VisionModule, logLevel);
- Logger.setLevel(LogGroup.Data, logLevel);
- Logger.setLevel(LogGroup.Config, logLevel);
- Logger.setLevel(LogGroup.General, logLevel);
- logger.info("Logging initialized in debug mode.");
-
// Add Linux kernel log->Photon logger
KernelLogLogger.getInstance();
diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
index a46215462..b6d09e17b 100644
--- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
@@ -32,7 +32,6 @@ import java.util.LinkedList;
import java.util.Optional;
import javax.imageio.ImageIO;
import org.apache.commons.io.FileUtils;
-import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.core.MatOfInt;
import org.opencv.core.Size;
@@ -525,7 +524,7 @@ public class RequestHandler {
public static void onDataCalibrationImportRequest(Context ctx) {
try {
DataCalibrationImportRequest request =
- kObjectMapper.readValue(ctx.body(), DataCalibrationImportRequest.class);
+ kObjectMapper.readValue(ctx.req().getInputStream(), DataCalibrationImportRequest.class);
var uploadCalibrationEvent =
new IncomingWebSocketEvent<>(
@@ -1000,6 +999,41 @@ public class RequestHandler {
ctx.status(204);
}
+ /**
+ * Get the calibration JSON for a specific observation. Excludes camera image data
+ *
+ * This is excluded from UICalibrationCoefficients by default to save bandwidth on large
+ * calibrations
+ */
+ public static void onCalibrationJsonRequest(Context ctx) {
+ String cameraUniqueName = ctx.queryParam("cameraUniqueName");
+ var width = Integer.parseInt(ctx.queryParam("width"));
+ var height = Integer.parseInt(ctx.queryParam("height"));
+
+ var module = VisionSourceManager.getInstance().vmm.getModule(cameraUniqueName);
+ if (module == null) {
+ ctx.status(404);
+ return;
+ }
+
+ CameraCalibrationCoefficients calList =
+ module.getStateAsCameraConfig().calibrations.stream()
+ .filter(
+ it ->
+ Math.abs(it.unrotatedImageSize.width - width) < 1e-4
+ && Math.abs(it.unrotatedImageSize.height - height) < 1e-4)
+ .findFirst()
+ .orElse(null);
+
+ if (calList == null) {
+ ctx.status(404);
+ return;
+ }
+
+ ctx.json(calList);
+ ctx.status(200);
+ }
+
private record CalibrationRemoveRequest(int width, int height, String cameraUniqueName) {}
public static void onCalibrationRemoveRequest(Context ctx) {
@@ -1065,28 +1099,18 @@ public class RequestHandler {
return;
}
- // encode as jpeg to save even more space. reduces size of a 1280p image from
- // 300k to 25k
+ // encode as jpeg to save even more space. reduces size of a 1280p image from 300k to 25k
+ var mat = calList.observations.get(observationIdx).annotateImage();
+ if (mat == null) {
+ ctx.status(404);
+ return;
+ }
+
var jpegBytes = new MatOfByte();
- Mat img = null;
- try {
- img =
- Imgcodecs.imread(
- calList.observations.get(observationIdx).snapshotDataLocation.toString());
- } catch (Exception e) {
- ctx.status(500);
- ctx.result("Unable to read calibration image");
- return;
- }
- if (img == null || img.empty()) {
- ctx.status(500);
- ctx.result("Unable to read calibration image");
- return;
- }
-
- Imgcodecs.imencode(".jpg", img, jpegBytes, new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60));
-
+ Imgcodecs.imencode(".jpg", mat, jpegBytes, new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60));
ctx.result(jpegBytes.toArray());
+
+ mat.release();
jpegBytes.release();
ctx.status(200);
diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java
index ee2922c26..fd0f164e7 100644
--- a/photon-server/src/main/java/org/photonvision/server/Server.java
+++ b/photon-server/src/main/java/org/photonvision/server/Server.java
@@ -68,7 +68,6 @@ public class Server {
it.anyHost();
});
}));
-
javalinConfig.requestLogger.http(
(ctx, ms) -> {
StringJoiner joiner =
@@ -129,6 +128,7 @@ public class Server {
app.post("/api/settings/camera", RequestHandler::onCameraSettingsRequest);
app.post("/api/settings/camera/setNickname", RequestHandler::onCameraNicknameChangeRequest);
app.get("/api/settings/camera/getCalibImages", RequestHandler::onCameraCalibImagesRequest);
+ app.get("/api/settings/camera/getCalibration", RequestHandler::onCalibrationJsonRequest);
// Utilities
app.post("/api/utils/offlineUpdate", RequestHandler::onOfflineUpdateRequest);