| Board warp, X/Y |
{{
@@ -278,7 +256,7 @@ const getObservationDetails = (): ObservationDetails[] | undefined => {
- ![observation image]()
+
|
diff --git a/photon-client/src/stores/settings/CameraSettingsStore.ts b/photon-client/src/stores/settings/CameraSettingsStore.ts
index 8cfbe5b36..ca7907043 100644
--- a/photon-client/src/stores/settings/CameraSettingsStore.ts
+++ b/photon-client/src/stores/settings/CameraSettingsStore.ts
@@ -416,6 +416,23 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
cameraIndex: number = useStateStore().currentCameraIndex
): CameraCalibrationResult | undefined {
return this.cameras[cameraIndex].completeCalibrations.find((v) => resolutionsAreEqual(v.resolution, resolution));
+ },
+ getCalImageUrl(host: string, resolution: Resolution, idx: number, cameraIdx = useStateStore().currentCameraIndex) {
+ const url = new URL(`http://${host}/api/utils/getCalSnapshot`);
+ url.searchParams.set("width", Math.round(resolution.width).toFixed(0));
+ url.searchParams.set("height", Math.round(resolution.height).toFixed(0));
+ url.searchParams.set("snapshotIdx", Math.round(idx).toFixed(0));
+ url.searchParams.set("cameraIdx", Math.round(cameraIdx).toFixed(0));
+
+ return url.href;
+ },
+ getCalJSONUrl(host: string, resolution: Resolution, cameraIdx = useStateStore().currentCameraIndex) {
+ const url = new URL(`http://${host}/api/utils/getCalibrationJSON`);
+ url.searchParams.set("width", Math.round(resolution.width).toFixed(0));
+ url.searchParams.set("height", Math.round(resolution.height).toFixed(0));
+ url.searchParams.set("cameraIdx", Math.round(cameraIdx).toFixed(0));
+
+ return url.href;
}
}
});
diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts
index 701e46a1c..69cf439bb 100644
--- a/photon-client/src/types/SettingTypes.ts
+++ b/photon-client/src/types/SettingTypes.ts
@@ -138,6 +138,9 @@ export interface CameraCalibrationResult {
distCoeffs: JsonMatOfDouble;
observations: BoardObservation[];
calobjectWarp?: number[];
+ // We might have to omit observations for bandwith, so backend will send us this
+ numSnapshots: number;
+ meanErrors: number[];
}
export enum ValidQuirks {
@@ -255,7 +258,9 @@ export const PlaceholderCameraSettings: CameraSettings = {
snapshotName: "img0.png",
snapshotData: { rows: 480, cols: 640, type: CvType.CV_8U, data: "" }
}
- ]
+ ],
+ numSnapshots: 1,
+ meanErrors: [123.45]
}
],
pipelineNicknames: ["Placeholder Pipeline"],
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
index d867b6dcf..dd1eabe3b 100644
--- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
+++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
@@ -31,7 +31,7 @@ import org.photonvision.common.util.SerializationUtils;
import org.photonvision.jni.RknnDetectorJNI;
import org.photonvision.mrcal.MrCalJNILoader;
import org.photonvision.raspi.LibCameraJNILoader;
-import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.calibration.UICameraCalibrationCoefficients;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
@@ -126,13 +126,6 @@ public class PhotonConfiguration {
settingsSubmap.put("networkSettings", netConfigMap);
- map.put(
- "cameraSettings",
- VisionModuleManager.getInstance().getModules().stream()
- .map(VisionModule::toUICameraConfig)
- .map(SerializationUtils::objectToHashMap)
- .collect(Collectors.toList()));
-
var lightingConfig = new UILightingConfig();
lightingConfig.brightness = hardwareSettings.ledBrightnessPercentage;
lightingConfig.supported = !hardwareConfig.ledPins.isEmpty();
@@ -181,7 +174,7 @@ public class PhotonConfiguration {
public HashMap> videoFormatList;
public int outputStreamPort;
public int inputStreamPort;
- public List calibrations;
+ public List calibrations;
public boolean isFovConfigurable = true;
public QuirkyCamera cameraQuirks;
public boolean isCSICamera;
diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
index f1d754d00..27a2770ae 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
+++ b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
@@ -26,6 +26,7 @@ import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
public class UIDataPublisher implements CVPipelineResultConsumer {
private static final Logger logger = new Logger(UIDataPublisher.class, LogGroup.VisionModule);
@@ -41,16 +42,22 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
public void accept(CVPipelineResult result) {
long now = System.currentTimeMillis();
- // only update the UI at 15hz
+ // only update the UI at 10hz
if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
var dataMap = new HashMap();
dataMap.put("fps", result.fps);
dataMap.put("latency", result.getLatencyMillis());
var uiTargets = new ArrayList>(result.targets.size());
- for (var t : result.targets) {
- uiTargets.add(t.toHashMap());
+
+ // We don't actually need to send targets during calibration and it can take up a lot (up to
+ // 1.2Mbps for 60 snapshots) of target results with no pitch/yaw/etc set
+ if (!(result instanceof CalibrationPipelineResult)) {
+ for (var t : result.targets) {
+ uiTargets.add(t.toHashMap());
+ }
}
+
dataMap.put("targets", uiTargets);
dataMap.put("classNames", result.objectDetectionClassNames);
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 6bfa8716b..8bf41c112 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
@@ -24,7 +24,7 @@ import java.util.List;
import org.opencv.core.Point;
import org.opencv.core.Point3;
-public final class BoardObservation {
+public final class BoardObservation implements Cloneable {
// Expected feature 3d location in the camera frame
@JsonProperty("locationInObjectSpace")
public List locationInObjectSpace;
@@ -68,4 +68,33 @@ public final class BoardObservation {
this.snapshotName = snapshotName;
this.snapshotData = snapshotData;
}
+
+ @Override
+ public String toString() {
+ return "BoardObservation [locationInObjectSpace="
+ + locationInObjectSpace
+ + ", locationInImageSpace="
+ + locationInImageSpace
+ + ", reprojectionErrors="
+ + reprojectionErrors
+ + ", optimisedCameraToObject="
+ + optimisedCameraToObject
+ + ", includeObservationInCalibration="
+ + includeObservationInCalibration
+ + ", snapshotName="
+ + snapshotName
+ + ", snapshotData="
+ + snapshotData
+ + "]";
+ }
+
+ @Override
+ public BoardObservation clone() {
+ try {
+ return (BoardObservation) super.clone();
+ } catch (CloneNotSupportedException e) {
+ System.err.println("Guhhh clone buh");
+ return null;
+ }
+ }
}
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 d1b938e68..e6b64101d 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
@@ -191,8 +191,8 @@ public class CameraCalibrationCoefficients implements Releasable {
+ cameraIntrinsics
+ ", distCoeffs="
+ distCoeffs
- + ", observations="
- + observations
+ + ", observationslen="
+ + observations.size()
+ ", calobjectWarp="
+ Arrays.toString(calobjectWarp)
+ ", intrinsicsArr="
@@ -201,4 +201,16 @@ public class CameraCalibrationCoefficients implements Releasable {
+ Arrays.toString(distCoeffsArr)
+ "]";
}
+
+ public UICameraCalibrationCoefficients cloneWithoutObservations() {
+ return new UICameraCalibrationCoefficients(
+ resolution,
+ cameraIntrinsics,
+ distCoeffs,
+ calobjectWarp,
+ observations,
+ calobjectSize,
+ calobjectSpacing,
+ lensmodel);
+ }
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonImageMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonImageMat.java
index 3a4555cc0..241004cca 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonImageMat.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonImageMat.java
@@ -76,4 +76,17 @@ public class JsonImageMat implements Releasable {
public void release() {
if (wrappedMat != null) wrappedMat.release();
}
+
+ @Override
+ public String toString() {
+ return "JsonImageMat [rows="
+ + rows
+ + ", cols="
+ + cols
+ + ", type="
+ + type
+ + ", datalen="
+ + data.length()
+ + "]";
+ }
}
diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java
index b5af61756..7dbb820ec 100644
--- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java
@@ -40,7 +40,7 @@ public class JsonMatOfDouble implements Releasable {
@JsonIgnore private Mat wrappedMat = null;
@JsonIgnore private Matrix wpilibMat = null;
- private MatOfDouble wrappedMatOfDouble;
+ @JsonIgnore private MatOfDouble wrappedMatOfDouble;
public JsonMatOfDouble(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
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
new file mode 100644
index 000000000..14cbfa51e
--- /dev/null
+++ b/photon-core/src/main/java/org/photonvision/vision/calibration/UICameraCalibrationCoefficients.java
@@ -0,0 +1,59 @@
+/*
+ * 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 .
+ */
+
+package org.photonvision.vision.calibration;
+
+import java.util.List;
+import java.util.stream.Collectors;
+import org.opencv.core.Size;
+
+public class UICameraCalibrationCoefficients extends CameraCalibrationCoefficients {
+ public int numSnapshots;
+ public List meanErrors;
+
+ public UICameraCalibrationCoefficients(
+ Size resolution,
+ JsonMatOfDouble cameraIntrinsics,
+ JsonMatOfDouble distCoeffs,
+ double[] calobjectWarp,
+ List observations,
+ Size calobjectSize,
+ double calobjectSpacing,
+ CameraLensModel lensmodel) {
+ // yeet observations, keep all else
+ super(
+ resolution,
+ cameraIntrinsics,
+ distCoeffs,
+ calobjectWarp,
+ List.of(),
+ calobjectSize,
+ calobjectSpacing,
+ lensmodel);
+
+ this.numSnapshots = observations.size();
+ this.meanErrors =
+ observations.stream()
+ .map(
+ it2 ->
+ it2.reprojectionErrors.stream()
+ .mapToDouble(it -> Math.sqrt(it.x * it.x + it.y * it.y))
+ .average()
+ .orElse(0))
+ .collect(Collectors.toList());
+ }
+}
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
index 906546ef5..7dfae0cf2 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -26,6 +26,7 @@ import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.function.BiConsumer;
+import java.util.stream.Collectors;
import org.opencv.core.Size;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
@@ -536,7 +537,10 @@ public class VisionModule {
ret.outputStreamPort = this.outputStreamPort;
ret.inputStreamPort = this.inputStreamPort;
- ret.calibrations = visionSource.getSettables().getConfiguration().calibrations;
+ ret.calibrations =
+ visionSource.getSettables().getConfiguration().calibrations.stream()
+ .map(CameraCalibrationCoefficients::cloneWithoutObservations)
+ .collect(Collectors.toList());
ret.isFovConfigurable =
!(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV()
diff --git a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
index 4282bd332..d2f6f07b2 100644
--- a/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/DataSocketHandler.java
@@ -350,8 +350,7 @@ public class DataSocketHandler {
}
}
- private void sendMessage(Object message, WsContext user) throws JsonProcessingException {
- ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(message));
+ private void sendMessage(ByteBuffer b, WsContext user) throws JsonProcessingException {
if (user.session.isOpen()) {
user.send(b);
}
@@ -359,16 +358,18 @@ public class DataSocketHandler {
public void broadcastMessage(Object message, WsContext userToSkip)
throws JsonProcessingException {
+ ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(message));
+
if (userToSkip == null) {
for (WsContext user : users) {
- sendMessage(message, user);
+ sendMessage(b, user);
}
} else {
var skipUserPort = ((InetSocketAddress) userToSkip.session.getRemoteAddress()).getPort();
for (WsContext user : users) {
var userPort = ((InetSocketAddress) user.session.getRemoteAddress()).getPort();
if (userPort != skipUserPort) {
- sendMessage(message, user);
+ sendMessage(b, user);
}
}
}
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 87eeb37f4..0ea9f05d7 100644
--- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
@@ -31,6 +31,9 @@ import java.util.HashMap;
import java.util.Optional;
import javax.imageio.ImageIO;
import org.apache.commons.io.FileUtils;
+import org.opencv.core.MatOfByte;
+import org.opencv.core.MatOfInt;
+import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.NetworkConfig;
import org.photonvision.common.dataflow.DataChangeDestination;
@@ -580,6 +583,77 @@ public class RequestHandler {
ctx.status(204);
}
+ public static void onCalibrationSnapshotRequest(Context ctx) {
+ logger.info(ctx.queryString().toString());
+
+ int idx = Integer.parseInt(ctx.queryParam("cameraIdx"));
+ var width = Integer.parseInt(ctx.queryParam("width"));
+ var height = Integer.parseInt(ctx.queryParam("height"));
+ var observationIdx = Integer.parseInt(ctx.queryParam("snapshotIdx"));
+
+ CameraCalibrationCoefficients calList =
+ VisionModuleManager.getInstance()
+ .getModule(idx)
+ .getStateAsCameraConfig()
+ .calibrations
+ .stream()
+ .filter(
+ it ->
+ Math.abs(it.resolution.width - width) < 1e-4
+ && Math.abs(it.resolution.height - height) < 1e-4)
+ .findFirst()
+ .orElse(null);
+
+ if (calList == null || calList.observations.size() < observationIdx) {
+ ctx.status(404);
+ return;
+ }
+
+ // encode as jpeg to save even more space. reduces size of a 1280p image from 300k to 25k
+ var jpegBytes = new MatOfByte();
+ Imgcodecs.imencode(
+ ".jpg",
+ calList.observations.get(observationIdx).snapshotData.getAsMat(),
+ jpegBytes,
+ new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60));
+
+ ctx.result(jpegBytes.toArray());
+ jpegBytes.release();
+
+ ctx.status(200);
+ }
+
+ public static void onCalibrationExportRequest(Context ctx) {
+ logger.info(ctx.queryString().toString());
+
+ int idx = Integer.parseInt(ctx.queryParam("cameraIdx"));
+ var width = Integer.parseInt(ctx.queryParam("width"));
+ var height = Integer.parseInt(ctx.queryParam("height"));
+
+ var cc = VisionModuleManager.getInstance().getModule(idx).getStateAsCameraConfig();
+
+ CameraCalibrationCoefficients calList =
+ cc.calibrations.stream()
+ .filter(
+ it ->
+ Math.abs(it.resolution.width - width) < 1e-4
+ && Math.abs(it.resolution.height - height) < 1e-4)
+ .findFirst()
+ .orElse(null);
+
+ if (calList == null) {
+ ctx.status(404);
+ return;
+ }
+
+ var filename = "photon_calibration_" + cc.uniqueName + "_" + width + "x" + height + ".json";
+ ctx.contentType("application/zip");
+ ctx.header("Content-Disposition", "attachment; filename=\"" + filename + "\"");
+ ctx.json(calList);
+
+ ctx.status(200);
+ }
+
public static void onImageSnapshotsRequest(Context ctx) {
var snapshots = new ArrayList>();
var cameraDirs = ConfigManager.getInstance().getImageSavePath().toFile().listFiles();
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 09f8c9044..0fc0d97f0 100644
--- a/photon-server/src/main/java/org/photonvision/server/Server.java
+++ b/photon-server/src/main/java/org/photonvision/server/Server.java
@@ -130,6 +130,8 @@ public class Server {
app.post("/api/utils/restartDevice", RequestHandler::onDeviceRestartRequest);
app.post("/api/utils/publishMetrics", RequestHandler::onMetricsPublishRequest);
app.get("/api/utils/getImageSnapshots", RequestHandler::onImageSnapshotsRequest);
+ app.get("/api/utils/getCalSnapshot", RequestHandler::onCalibrationSnapshotRequest);
+ app.get("/api/utils/getCalibrationJSON", RequestHandler::onCalibrationExportRequest);
// Calibration
app.post("/api/calibration/end", RequestHandler::onCalibrationEndRequest);
|