From ecae647d6281b8ef5b173255c81a05d6515d854d Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 27 Dec 2023 23:04:49 -0800 Subject: [PATCH] Serialize all calibration data --- .../cameras/CameraCalibrationCard.vue | 204 ++++++++++++++++-- .../stores/settings/CameraSettingsStore.ts | 14 +- photon-client/src/types/SettingTypes.ts | 106 +++++++-- photon-client/src/types/WebsocketDataTypes.ts | 12 +- .../configuration/PhotonConfiguration.java | 4 +- .../networktables/NTDataPublisher.java | 2 +- .../photonvision/common/util/ColorHelper.java | 4 + .../common/util/file/JacksonUtils.java | 14 ++ .../common/util/math/MathUtils.java | 13 ++ .../vision/calibration/BoardObservation.java | 54 +++++ .../CameraCalibrationCoefficients.java | 41 ++-- .../vision/pipe/impl/Calibrate3dPipe.java | 86 +++++--- .../vision/pipe/impl/DrawCalibrationPipe.java | 62 ++++++ .../pipe/impl/FindBoardCornersPipe.java | 40 ++-- .../vision/pipeline/Calibrate3dPipeline.java | 72 ++++--- .../vision/pipeline/OutputStreamPipeline.java | 16 +- .../vision/pipeline/UICalibrationData.java | 30 +-- .../result/CalibrationPipelineResult.java | 35 +++ .../vision/processes/PipelineManager.java | 14 +- .../vision/processes/VisionModule.java | 25 +-- .../VisionModuleChangeSubscriber.java | 13 +- .../vision/target/TrackedTarget.java | 8 + .../vision/pipeline/Calibrate3dPipeTest.java | 33 ++- .../vision/processes/PipelineManagerTest.java | 2 +- 24 files changed, 693 insertions(+), 211 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue index 0bbfbe3df9..a777d84a7c 100644 --- a/photon-client/src/components/cameras/CameraCalibrationCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue @@ -1,7 +1,13 @@ diff --git a/photon-client/src/stores/settings/CameraSettingsStore.ts b/photon-client/src/stores/settings/CameraSettingsStore.ts index 861b192ec3..e47489f1af 100644 --- a/photon-client/src/stores/settings/CameraSettingsStore.ts +++ b/photon-client/src/stores/settings/CameraSettingsStore.ts @@ -1,9 +1,9 @@ import { defineStore } from "pinia"; import type { CalibrationBoardTypes, - CameraCalibrationResult, CameraSettings, ConfigurableCameraSettings, + Resolution, RobotOffsetType, VideoFormat } from "@/types/SettingTypes"; @@ -64,6 +64,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", { updateCameraSettingsFromWebsocket(data: WebsocketCameraSettingsUpdate[]) { this.cameras = data.map((d) => ({ nickname: d.nickname, + uniqueName: d.uniqueName, fov: { value: d.fov, managedByVendor: !d.isFovConfigurable @@ -89,16 +90,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", { standardDeviation: v.standardDeviation, mean: v.mean })), - completeCalibrations: d.calibrations.map((calib) => ({ - resolution: { - height: calib.height, - width: calib.width - }, - distCoeffs: calib.distCoeffs, - standardDeviation: calib.standardDeviation, - perViewErrors: calib.perViewErrors, - intrinsics: calib.intrinsics - })), + completeCalibrations: d.calibrations, pipelineNicknames: d.pipelineNicknames, currentPipelineIndex: d.currentPipelineIndex, pipelineSettings: d.currentPipelineSettings diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts index 7119b8cd90..a8f59782f2 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -76,16 +76,55 @@ export interface VideoFormat { diagonalFOV?: number; horizontalFOV?: number; verticalFOV?: number; - standardDeviation?: number; mean?: number; } +export enum CvType { + CV_8U = 0, + CV_8S = 1, + CV_16U = 2, + CV_16S = 3, + CV_32S = 4, + CV_32F = 5, + CV_64F = 6, + CV_16F = 7 +} + +export interface JsonMat { + rows: number; + cols: number; + type: CvType; + data: number[]; +} + +export interface CvPoint3 { + x: number; + y: number; + z: number; +} +export interface CvPoint { + x: number; + y: number; +} + +export interface Pose3d { + translation: { x: number; y: number; z: number }; + rotation: { quaternion: { W: number; X: number; Y: number; Z: number } }; +} + +export interface BoardObservation { + locationInObjectSpace: CvPoint3[]; + locationInImageSpace: CvPoint[]; + reprojectionErrors: CvPoint[]; + optimisedCameraToObject: Pose3d; +} + export interface CameraCalibrationResult { resolution: Resolution; - distCoeffs: number[]; - standardDeviation: number; - perViewErrors: number[]; - intrinsics: number[]; + cameraIntrinsics: JsonMat; + // TODO rename to be Right + cameraExtrinsics: JsonMat; + observations: BoardObservation[]; } export interface ConfigurableCameraSettings { @@ -94,6 +133,7 @@ export interface ConfigurableCameraSettings { export interface CameraSettings { nickname: string; + uniqueName: string; fov: { value: number; @@ -115,6 +155,7 @@ export interface CameraSettings { export const PlaceholderCameraSettings: CameraSettings = { nickname: "Placeholder Camera", + uniqueName: "Placeholder Name", fov: { value: 70, managedByVendor: true @@ -128,19 +169,54 @@ export const PlaceholderCameraSettings: CameraSettings = { resolution: { width: 1920, height: 1080 }, fps: 60, pixelFormat: "RGB" - }, - { - resolution: { width: 1280, height: 720 }, - fps: 60, - pixelFormat: "RGB" - }, + } + // { + // resolution: { width: 1280, height: 720 }, + // fps: 60, + // pixelFormat: "RGB" + // }, + // { + // resolution: { width: 640, height: 480 }, + // fps: 30, + // pixelFormat: "RGB" + // } + ], + completeCalibrations: [ { - resolution: { width: 640, height: 480 }, - fps: 30, - pixelFormat: "RGB" + resolution: { width: 1920, height: 1080 }, + cameraIntrinsics: { + rows: 1, + cols: 1, + type: 1, + data: [1, 2, 3, 4, 5, 6, 7, 8, 9] + }, + cameraExtrinsics: { + rows: 1, + cols: 1, + type: 1, + data: [10, 11, 12, 13] + }, + observations: [ + { + locationInImageSpace: [ + { x: 100, y: 100 }, + { x: 210, y: 100 }, + { x: 320, y: 101 }, + ], + 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 } } + }, + reprojectionErrors: [ + { x: 1, y: 1 }, + { x: 2, y: 1 }, + { x: 3, y: 1 }, + ] + } + ] } ], - completeCalibrations: [], pipelineNicknames: ["Placeholder Pipeline"], lastPipelineIndex: 0, currentPipelineIndex: 0, diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts index 0114e19e79..d63e3fd8f8 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -1,4 +1,11 @@ -import type { GeneralSettings, LightingSettings, LogLevel, MetricData, NetworkSettings } from "@/types/SettingTypes"; +import type { + CameraCalibrationResult, + GeneralSettings, + LightingSettings, + LogLevel, + MetricData, + NetworkSettings +} from "@/types/SettingTypes"; import type { ActivePipelineSettings } from "@/types/PipelineTypes"; import type { AprilTagFieldLayout, PipelineResult } from "@/types/PhotonTrackingTypes"; @@ -46,13 +53,14 @@ export type WebsocketVideoFormat = Record< >; export interface WebsocketCameraSettingsUpdate { - calibrations: WebsocketCompleteCalib[]; + calibrations: CameraCalibrationResult[]; currentPipelineIndex: number; currentPipelineSettings: ActivePipelineSettings; fov: number; inputStreamPort: number; isFovConfigurable: boolean; nickname: string; + uniqueName: string; outputStreamPort: number; pipelineNicknames: string[]; videoFormatList: WebsocketVideoFormat; 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 cfd3b677a8..89f9cb9063 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 @@ -28,6 +28,7 @@ import org.photonvision.common.networking.NetworkUtils; import org.photonvision.common.util.SerializationUtils; import org.photonvision.raspi.LibCameraJNI; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.processes.VisionModule; import org.photonvision.vision.processes.VisionModuleManager; import org.photonvision.vision.processes.VisionSource; @@ -166,13 +167,14 @@ public static class UICameraConfiguration { public double fov; public String nickname; + public String uniqueName; public HashMap currentPipelineSettings; public int currentPipelineIndex; public List pipelineNicknames; public HashMap> videoFormatList; public int outputStreamPort; public int inputStreamPort; - public List> calibrations; + public List calibrations; public boolean isFovConfigurable = true; } } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 0f668a417c..e759c07ecc 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -180,7 +180,7 @@ public void accept(CVPipelineResult result) { && result.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) { var fsp = result.inputAndOutputFrame.frameStaticProperties; ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr()); - ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr()); + ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr()); } else { ts.cameraIntrinsicsPublisher.accept(new double[] {}); ts.cameraDistortionPublisher.accept(new double[] {}); diff --git a/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java b/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java index 1ce4c55f18..1d6fbaa3b6 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java +++ b/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java @@ -24,4 +24,8 @@ public class ColorHelper { public static Scalar colorToScalar(Color color) { return new Scalar(color.getBlue(), color.getGreen(), color.getRed()); } + + public static Scalar colorToScalar(Color color, double alpha) { + return new Scalar(color.getBlue(), color.getGreen(), color.getRed(), alpha); + } } diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java b/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java index 818a0e51ec..ae48a103ae 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java @@ -32,6 +32,7 @@ import java.io.IOException; import java.nio.file.Path; import java.util.HashMap; +import java.util.Map; public class JacksonUtils { public static class UIMap extends HashMap {} @@ -61,6 +62,19 @@ public static void serialize(Path path, T object, boolean forceSync) throws saveJsonString(json, path, forceSync); } + public static T deserialize(Map s, Class ref) throws IOException { + PolymorphicTypeValidator ptv = + BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build(); + ObjectMapper objectMapper = + JsonMapper.builder() + .configure(JsonReadFeature.ALLOW_JAVA_COMMENTS, true) + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT) + .build(); + + return objectMapper.convertValue(s, ref); + } + public static T deserialize(String s, Class ref) throws IOException { PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build(); diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 2b6bb31ae3..85e5121957 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -23,10 +23,12 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.WPIUtilJNI; import java.util.Arrays; import java.util.List; +import org.opencv.core.Core; import org.opencv.core.Mat; public class MathUtils { @@ -198,4 +200,15 @@ public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) { var axis = rotation.getAxis().times(angle); rvecOutput.put(0, 0, axis.getData()); } + + public static Pose3d opencvRTtoPose3d(Mat rVec, Mat tVec) { + Translation3d translation = + new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]); + Rotation3d rotation = + new Rotation3d( + VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]), + Core.norm(rVec)); + + return new Pose3d(translation, rotation); + } } 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 new file mode 100644 index 0000000000..a052016151 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java @@ -0,0 +1,54 @@ +/* + * 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 com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.List; +import org.opencv.core.Point; +import org.opencv.core.Point3; + +public final class BoardObservation { + // Expected feature 3d location in the camera frame + @JsonProperty("locationInObjectSpace") + public List locationInObjectSpace; + + // Observed location in pixel space + @JsonProperty("locationInImageSpace") + public List locationInImageSpace; + + // (measured location in pixels) - (expected from FK) + @JsonProperty("reprojectionErrors") + public List reprojectionErrors; + + // Solver optimized board poses + public Pose3d optimisedCameraToObject; + + @JsonCreator + public BoardObservation( + @JsonProperty("locationInObjectSpace") List locationInObjectSpace, + @JsonProperty("locationInImageSpace") List locationInImageSpace, + @JsonProperty("reprojectionErrors") List reprojectionErrors, + @JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject) { + this.locationInObjectSpace = locationInObjectSpace; + this.locationInImageSpace = locationInImageSpace; + this.reprojectionErrors = reprojectionErrors; + this.optimisedCameraToObject = optimisedCameraToObject; + } +} 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 4b4ae5a8cc..d65acab454 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 @@ -20,13 +20,16 @@ import com.fasterxml.jackson.annotation.JsonAlias; 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 com.fasterxml.jackson.databind.JsonNode; +import java.util.List; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; import org.photonvision.vision.opencv.Releasable; +@JsonIgnoreProperties(ignoreUnknown = true) public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("resolution") public final Size resolution; @@ -38,32 +41,31 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonAlias({"cameraExtrinsics", "distCoeffs"}) public final JsonMat distCoeffs; - @JsonProperty("perViewErrors") - public final double[] perViewErrors; - - @JsonProperty("standardDeviation") - public final double standardDeviation; + @JsonProperty("observations") + public final List observations; @JsonIgnore private final double[] intrinsicsArr = new double[9]; - - @JsonIgnore private final double[] extrinsicsArr = new double[5]; + @JsonIgnore private final double[] distCoeffsArr = new double[5]; @JsonCreator public CameraCalibrationCoefficients( @JsonProperty("resolution") Size resolution, @JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics, @JsonProperty("cameraExtrinsics") JsonMat distCoeffs, - @JsonProperty("perViewErrors") double[] perViewErrors, - @JsonProperty("standardDeviation") double standardDeviation) { + @JsonProperty("observations") List observations) { this.resolution = resolution; this.cameraIntrinsics = cameraIntrinsics; this.distCoeffs = distCoeffs; - this.perViewErrors = perViewErrors; - this.standardDeviation = standardDeviation; + + // Legacy migration just to make sure that observations is at worst empty and never null + if (observations == null) { + observations = List.of(); + } + this.observations = observations; // do this once so gets are quick getCameraIntrinsicsMat().get(0, 0, intrinsicsArr); - getDistCoeffsMat().get(0, 0, extrinsicsArr); + getDistCoeffsMat().get(0, 0, distCoeffsArr); } @JsonIgnore @@ -82,18 +84,13 @@ public double[] getIntrinsicsArr() { } @JsonIgnore - public double[] getExtrinsicsArr() { - return extrinsicsArr; - } - - @JsonIgnore - public double[] getPerViewErrors() { - return perViewErrors; + public double[] getDistCoeffsArr() { + return distCoeffsArr; } @JsonIgnore - public double getStandardDeviation() { - return standardDeviation; + public List getPerViewErrors() { + return observations; } @Override @@ -138,6 +135,6 @@ public static CameraCalibrationCoefficients parseFromCalibdbJson(JsonNode json) var height = json.get("img_size").get(1).doubleValue(); return new CameraCalibrationCoefficients( - new Size(width, height), cam_jsonmat, distortion_jsonmat, new double[] {error}, 0); + new Size(width, height), cam_jsonmat, distortion_jsonmat, List.of()); } } 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 bfca09d6fd..398e5f9608 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 @@ -24,11 +24,14 @@ import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Triple; import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.math.MathUtils; +import org.photonvision.vision.calibration.BoardObservation; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.calibration.JsonMat; import org.photonvision.vision.pipe.CVPipe; @@ -79,16 +82,18 @@ protected CameraCalibrationCoefficients process(List> in) && it.getMiddle() != null && it.getRight() != null) .collect(Collectors.toList()); + + List objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList()); + List imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList()); + if (objPoints.size() != imgPts.size()) { + logger.error("objpts.size != imgpts.size"); + return null; + } + try { // FindBoardCorners pipe outputs all the image points, object points, and frames to calculate // imageSize from, other parameters are output Mats - var objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList()); - var imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList()); - if (objPoints.size() != imgPts.size()) { - logger.error("objpts.size != imgpts.size"); - return null; - } calibrationAccuracy = Calib3d.calibrateCameraExtended( objPoints, @@ -106,15 +111,52 @@ protected CameraCalibrationCoefficients process(List> in) e.printStackTrace(); return null; } + JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix); JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients); - // Create a new CameraCalibrationCoefficients object to pass onto SolvePnP - double[] perViewErrorsArray = - new double[(int) perViewErrors.total() * perViewErrors.channels()]; - perViewErrors.get(0, 0, perViewErrorsArray); + + // For each observation, calc reprojection error + Mat jac_temp = new Mat(); + List observations = new ArrayList<>(); + for (int i = 0; i < objPoints.size(); i++) { + 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(); + + var img_pts_reprojected = new MatOfPoint2f(); + try { + Calib3d.projectPoints( + i_objPtsNative, + rvecs.get(i), + tvecs.get(i), + cameraMatrix, + distortionCoefficients, + img_pts_reprojected, + jac_temp, + 0.0); + } catch (Exception e) { + e.printStackTrace(); + continue; + } + var img_pts_reprojected_list = img_pts_reprojected.toList(); + + var reprojectionError = new ArrayList(); + for (int j = 0; j < img_pts_reprojected_list.size(); j++) { + // error = (measured - expected) + var measured = img_pts_reprojected_list.get(j); + var expected = i_imgPts.get(j); + 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)); + + observations.add(new BoardObservation(i_objPts, i_imgPts, reprojectionError, camToBoard)); + } + jac_temp.release(); // Standard deviation of results - double stdDev = calculateSD(perViewErrorsArray); try { // Print calibration successful logger.info( @@ -126,32 +168,12 @@ protected CameraCalibrationCoefficients process(List> in) + new ObjectMapper().writeValueAsString(cameraMatrixMat) + "\ndistortionCoeffs:\n" + new ObjectMapper().writeValueAsString(distortionCoefficientsMat) - + "\nWith Standard Deviation Of\n" - + stdDev + "\n"); } catch (JsonProcessingException e) { logger.error("Failed to parse calibration data to json!", e); } return new CameraCalibrationCoefficients( - params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray, stdDev); - } - - // Calculate standard deviation of the RMS error of the snapshots - private static double calculateSD(double[] numArray) { - double sum = 0.0, standardDeviation = 0.0; - int length = numArray.length; - - for (double num : numArray) { - sum += num; - } - - double mean = sum / length; - - for (double num : numArray) { - standardDeviation += Math.pow(num - mean, 2); - } - - return Math.sqrt(standardDeviation / length); + params.resolution, cameraMatrixMat, distortionCoefficientsMat, observations); } public static class CalibratePipeParams { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java new file mode 100644 index 0000000000..a1207030a9 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java @@ -0,0 +1,62 @@ +/* + * 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.pipe.impl; + +import java.awt.Color; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.util.ColorHelper; +import org.photonvision.vision.frame.FrameDivisor; +import org.photonvision.vision.pipe.MutatingPipe; +import org.photonvision.vision.target.TrackedTarget; + +public class DrawCalibrationPipe + extends MutatingPipe< + Pair>, DrawCalibrationPipe.DrawCalibrationPipeParams> { + @Override + protected Void process(Pair> in) { + var image = in.getLeft(); + + for (var target : in.getRight()) { + for (var c : target.getTargetCorners()) { + c = + new Point( + c.x / params.divisor.value.doubleValue(), c.y / params.divisor.value.doubleValue()); + var r = 4; + var r2 = r / Math.sqrt(2); + var color = ColorHelper.colorToScalar(Color.RED, 0.4); + Imgproc.circle(image, c, r, color, 1); + Imgproc.line(image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color); + Imgproc.line(image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color); + } + } + + return null; + } + + public static class DrawCalibrationPipeParams { + private final FrameDivisor divisor; + + public DrawCalibrationPipeParams(FrameDivisor divisor) { + this.divisor = divisor; + } + } +} 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 678566c707..2c576f562d 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 @@ -113,9 +113,6 @@ public void createObjectPoints() { */ @Override protected Triple process(Pair in) { - // Create the object points - createObjectPoints(); - return findBoardCorners(in); } @@ -228,10 +225,12 @@ private Triple findBoardCorners(Pair in) { boolean boardFound = false; if (params.type == UICalibrationData.BoardType.CHESSBOARD) { - // This is for chessboards - // Reduce the image size to be much more manageable - Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame)); + if (params.divisor != FrameDivisor.NONE) { + Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame)); + } else { + smallerInFrame = inFrame; + } // Run the chessboard corner finder on the smaller image boardFound = @@ -244,14 +243,13 @@ private Triple findBoardCorners(Pair in) { } } else if (params.type == UICalibrationData.BoardType.DOTBOARD) { - // For dot boards boardFound = Calib3d.findCirclesGrid( inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID); } if (!boardFound) { - // If we can't find a chessboard/dot board, just return + // If we can't find a chessboard/dot board, give up return null; } @@ -264,31 +262,25 @@ private Triple findBoardCorners(Pair in) { // Get the size of the inFrame this.imageSize = new Size(inFrame.width(), inFrame.height()); - // Do sub corner pix for drawing chessboard - Imgproc.cornerSubPix( - inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria); + // Do sub corner pix for drawing chessboard when using OpenCV + if (params.divisor != FrameDivisor.NONE) { + Imgproc.cornerSubPix( + inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria); + } - // convert back to BGR - // Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_GRAY2BGR); // draw the chessboard, doesn't have to be different for a dot board since it just re projects // the corners we found Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true); - // // Add the 3D points and the points of the corners found - // if (addToSnapList) { - // this.listOfObjectPoints.add(objectPoints); - // this.listOfImagePoints.add(boardCorners); - // } - return Triple.of(inFrame.size(), objPts, outBoardCorners); } public static class FindCornersPipeParams { - private final int boardHeight; - private final int boardWidth; - private final UICalibrationData.BoardType type; - private final double gridSize; - private final FrameDivisor divisor; + final int boardHeight; + final int boardWidth; + final UICalibrationData.BoardType type; + final double gridSize; + final FrameDivisor divisor; public FindCornersPipeParams( int boardHeight, diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index e1a3bc8aae..4f3caef569 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -20,11 +20,13 @@ import edu.wpi.first.math.util.Units; import java.nio.file.Path; import java.util.ArrayList; -import java.util.Collections; import java.util.List; +import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.lang3.tuple.Triple; import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; import org.opencv.core.Size; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.configuration.ConfigManager; @@ -34,7 +36,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.file.FileUtils; -import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.vision.calibration.BoardObservation; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -44,13 +46,14 @@ import org.photonvision.vision.pipe.impl.Calibrate3dPipe; import org.photonvision.vision.pipe.impl.FindBoardCornersPipe; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.pipeline.result.CalibrationPipelineResult; public class Calibrate3dPipeline extends CVPipeline { // For logging private static final Logger logger = new Logger(Calibrate3dPipeline.class, LogGroup.General); - // Only 2 pipes needed, one for finding the board corners and one for actually calibrating + // Find board corners decides internally between opencv and mrgingham private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe(); private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe(); private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); @@ -69,19 +72,21 @@ public class Calibrate3dPipeline private boolean calibrating = false; // Path to save images - private final Path imageDir = ConfigManager.getInstance().getCalibDir(); + private final Path imageDir; private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; - public Calibrate3dPipeline() { - this(12); + public Calibrate3dPipeline(String uniqueName) { + this(12, uniqueName); } - public Calibrate3dPipeline(int minSnapshots) { + public Calibrate3dPipeline(int minSnapshots, String uniqueName) { super(PROCESSING_TYPE); this.settings = new Calibration3dPipelineSettings(); this.foundCornersList = new ArrayList<>(); this.minSnapshots = minSnapshots; + this.imageDir = + Path.of(ConfigManager.getInstance().getCalibDir().toAbsolutePath().toString(), uniqueName); } @Override @@ -99,11 +104,6 @@ protected void setPipeParamsImpl() { new Calibrate3dPipe.CalibratePipeParams( new Size(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight)); calibrate3dPipe.setParams(calibratePipeParams); - - // if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && LibCameraJNI.isSupported()) { - // LibCameraJNI.setRotation(settings.inputImageRotationMode.value); - // // LibCameraJNI.setShouldCopyColor(true); - // } } @Override @@ -119,7 +119,8 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se // Check if the frame has chessboard corners var outputColorCVMat = new CVMat(); inputColorMat.copyTo(outputColorCVMat.getMat()); - var findBoardResult = + + Triple findBoardResult = findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output; var fpsResult = calculateFPSPipe.run(null); @@ -131,9 +132,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se if (findBoardResult != null) { foundCornersList.add(findBoardResult); - Imgcodecs.imwrite( - Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(), - inputColorMat); + saveCalImage(inputColorMat); // update the UI broadcastState(); @@ -143,19 +142,40 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se frame.release(); // Return the drawn chessboard if corners are found, if not, then return the input image. - return new CVPipelineResult( + return new CalibrationPipelineResult( sumPipeNanosElapsed, fps, // Unused but here in case - Collections.emptyList(), - new MultiTargetPNPResult(), + // Collections.emptyList(), + // new MultiTargetPNPResult(), new Frame( - new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); + new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties), + getCornersList()); + } + + private void saveCalImage(Mat img) { + var folder = Path.of(imageDir.toString(), img.size().toString()); + if (!folder.toFile().exists()) { + folder.toFile().mkdirs(); + } + if (!folder.toFile().exists()) { + logger.error("Could not create save folder! " + folder); + } + Imgcodecs.imwrite( + Path.of(folder.toAbsolutePath().toString(), "img" + foundCornersList.size() + ".png") + .toString(), + img); + } + + List> getCornersList() { + return foundCornersList.stream() + .map(it -> ((MatOfPoint2f) it.getRight()).toList()) + .collect(Collectors.toList()); } - public void deleteSavedImages() { - imageDir.toFile().mkdirs(); - imageDir.toFile().mkdir(); - FileUtils.deleteDirectory(imageDir); + public void deleteSavedImages(Size resolution) { + var folder = Path.of(imageDir.toString(), resolution.toString()); + folder.toFile().mkdirs(); + FileUtils.deleteDirectory(folder); } public boolean hasEnough() { @@ -188,8 +208,8 @@ public void takeSnapshot() { takeSnapshot = true; } - public double[] perViewErrors() { - return calibrationOutput.output.perViewErrors; + public List perViewErrors() { + return calibrationOutput.output.observations; } public void finishCalibration() { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index 238333ee6a..bf5b73a4ea 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -38,6 +38,7 @@ public class OutputStreamPipeline { private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe(); private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe(); private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe(); + private final DrawCalibrationPipe drawCalibrationPipe = new DrawCalibrationPipe(); private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe(); private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe(); @@ -113,6 +114,9 @@ protected void setPipeParams( resizeImagePipe.setParams( new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor)); + + drawCalibrationPipe.setParams( + new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor)); } public CVPipelineResult process( @@ -149,8 +153,9 @@ public CVPipelineResult process( sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed; if (!(settings instanceof AprilTagPipelineSettings) - && !(settings instanceof ArucoPipelineSettings)) { - // If we're processing anything other than Apriltags... + && !(settings instanceof ArucoPipelineSettings) + && !(settings instanceof Calibration3dPipelineSettings)) { + // If we're processing anything other than Apriltags.. var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw)); sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed; @@ -172,7 +177,14 @@ public CVPipelineResult process( pipeProfileNanos[7] = 0; pipeProfileNanos[8] = 0; } + } else if (settings instanceof Calibration3dPipelineSettings) { + pipeProfileNanos[5] = 0; + pipeProfileNanos[6] = 0; + + var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw)); + sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed; + pipeProfileNanos[8] = 0; } else if (settings instanceof AprilTagPipelineSettings) { // If we are doing apriltags... if (settings.solvePNPEnabled) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java index 287e271974..6890ddc49e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java @@ -19,15 +19,19 @@ import java.util.Map; +import org.opencv.core.Size; + public class UICalibrationData { - public final int videoModeIndex; + public int videoModeIndex; public int count; - public final int minCount; - public final boolean hasEnough; - public final double squareSizeIn; - public final int patternWidth; - public final int patternHeight; - public final BoardType boardType; // + public int minCount; + public boolean hasEnough; + public double squareSizeIn; + public int patternWidth; + public int patternHeight; + public BoardType boardType; + + public UICalibrationData() {} public UICalibrationData( int count, @@ -53,18 +57,6 @@ public enum BoardType { DOTBOARD } - public static UICalibrationData fromMap(Map map) { - return new UICalibrationData( - ((Number) map.get("count")).intValue(), - ((Number) map.get("videoModeIndex")).intValue(), - ((Number) map.get("minCount")).intValue(), - (boolean) map.get("hasEnough"), - ((Number) map.get("squareSizeIn")).doubleValue(), - ((Number) map.get("patternWidth")).intValue(), - ((Number) map.get("patternHeight")).intValue(), - BoardType.values()[(int) map.get("boardType")]); - } - @Override public String toString() { return "UICalibrationData{" diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java new file mode 100644 index 0000000000..7ad8ff83cf --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java @@ -0,0 +1,35 @@ +/* + * 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.pipeline.result; + +import java.util.List; +import java.util.stream.Collectors; +import org.opencv.core.Point; +import org.photonvision.vision.frame.Frame; +import org.photonvision.vision.target.TrackedTarget; + +public class CalibrationPipelineResult extends CVPipelineResult { + private static List cornersToTarget(List> corners) { + return corners.stream().map(TrackedTarget::new).collect(Collectors.toList()); + } + + public CalibrationPipelineResult( + double latencyNanos, double fps, Frame outputFrame, List> corners) { + super(latencyNanos, fps, cornersToTarget(corners), outputFrame); + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index e4f6df3a97..52144e20eb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -37,7 +37,7 @@ public class PipelineManager { public static final int CAL_3D_INDEX = -2; protected final List userPipelineSettings; - protected final Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(); + protected final Calibrate3dPipeline calibration3dPipeline; protected final DriverModePipeline driverModePipeline = new DriverModePipeline(); /** Index of the currently active pipeline. Defaults to 0. */ @@ -56,21 +56,23 @@ public class PipelineManager { /** * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided * pipelines. - * - * @param userPipelines Pipelines to add to the manager. */ - public PipelineManager( - DriverModePipelineSettings driverSettings, List userPipelines) { + PipelineManager( + DriverModePipelineSettings driverSettings, + List userPipelines, + String uniqueName) { this.userPipelineSettings = new ArrayList<>(userPipelines); // This is to respect the default res idx for vendor cameras this.driverModePipeline.setSettings(driverSettings); if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective); + + calibration3dPipeline = new Calibrate3dPipeline(uniqueName); } public PipelineManager(CameraConfiguration config) { - this(config.driveModeSettings, config.pipelineSettings); + this(config.driveModeSettings, config.pipelineSettings, config.uniqueName); } /** 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 483f6a7c0c..b6f69f81c0 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,8 @@ import java.util.LinkedList; import java.util.List; import java.util.function.BiConsumer; + +import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.PhotonConfiguration; @@ -330,7 +332,11 @@ void setDriverMode(boolean isDriverMode) { public void startCalibration(UICalibrationData data) { var settings = pipelineManager.calibration3dPipeline.getSettings(); - pipelineManager.calibration3dPipeline.deleteSavedImages(); + + var videoMode = visionSource.getSettables().getAllVideoModes().get(data.videoModeIndex); + var resolution = new Size(videoMode.width, videoMode.height); + + pipelineManager.calibration3dPipeline.deleteSavedImages(resolution); settings.cameraVideoModeIndex = data.videoModeIndex; visionSource.getSettables().setVideoModeIndex(data.videoModeIndex); logger.info( @@ -342,6 +348,7 @@ public void startCalibration(UICalibrationData data) { settings.boardHeight = data.patternHeight; settings.boardWidth = data.patternWidth; settings.boardType = data.boardType; + settings.resolution = resolution; // Disable gain if not applicable if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) { @@ -495,6 +502,7 @@ public PhotonConfiguration.UICameraConfiguration toUICameraConfig() { ret.fov = visionSource.getSettables().getFOV(); ret.nickname = visionSource.getSettables().getConfiguration().nickname; + ret.uniqueName = visionSource.getSettables().getConfiguration().uniqueName; ret.currentPipelineSettings = SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings()); ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex(); @@ -522,20 +530,7 @@ public PhotonConfiguration.UICameraConfiguration toUICameraConfig() { ret.outputStreamPort = this.outputStreamPort; ret.inputStreamPort = this.inputStreamPort; - var calList = new ArrayList>(); - for (var c : visionSource.getSettables().getConfiguration().calibrations) { - var internalMap = new HashMap(); - - internalMap.put("perViewErrors", c.perViewErrors); - internalMap.put("standardDeviation", c.standardDeviation); - internalMap.put("width", c.resolution.width); - internalMap.put("height", c.resolution.height); - internalMap.put("intrinsics", c.cameraIntrinsics.data); - internalMap.put("distCoeffs", c.distCoeffs.data); - - calList.add(internalMap); - } - ret.calibrations = calList; + ret.calibrations = visionSource.getSettables().getConfiguration().calibrations; ret.isFovConfigurable = !(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV() diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java index e824ce89dc..8d763ffbe2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java @@ -17,6 +17,7 @@ package org.photonvision.vision.processes; +import java.io.IOException; import java.util.ArrayList; import java.util.Map; import org.apache.commons.lang3.tuple.Pair; @@ -26,6 +27,7 @@ import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; @@ -103,9 +105,14 @@ public void onDataChangeEvent(DataChangeEvent event) { parentModule.saveAndBroadcastAll(); return; case "startCalibration": - var data = UICalibrationData.fromMap((Map) newPropValue); - parentModule.startCalibration(data); - parentModule.saveAndBroadcastAll(); + // var data = UICalibrationData.fromMap((Map) newPropValue); + try { + var data = JacksonUtils.deserialize((Map)newPropValue, UICalibrationData.class); + parentModule.startCalibration(data); + parentModule.saveAndBroadcastAll(); + } catch (Exception e) { + logger.error("Error deserailizing start-cal request", e); + } return; case "saveInputSnapshot": parentModule.saveInputSnapshot(); diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 2a28d05e56..61354c6873 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -146,6 +146,14 @@ public TrackedTarget( m_skew = 0; } + public TrackedTarget(List corners) { + m_mainContour = new Contour(new MatOfPoint()); + m_mainContour.mat.fromList(List.of(new Point(0, 0), new Point(0, 1), new Point(1, 0))); + this.setTargetCorners(corners); + m_targetOffsetPoint = new Point(); + m_robotOffsetPoint = new Point(); + } + public TrackedTarget( ArucoDetectionResult result, AprilTagPoseEstimate tagPose, 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 72547829f1..ed20670fc9 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 @@ -24,7 +24,6 @@ import java.io.File; import java.nio.file.Path; import java.util.ArrayList; -import java.util.Arrays; import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.lang3.tuple.Triple; @@ -78,9 +77,9 @@ public void perViewErrorsTest() { calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480))); var calibrate3dPipeOutput = calibrate3dPipe.run(foundCornersList); - assertTrue(calibrate3dPipeOutput.output.perViewErrors.length > 0); - System.out.println( - "Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors)); + assertTrue(calibrate3dPipeOutput.output.observations.size() > 0); + // System.out.println("Per View Errors: " + + // Arrays.toString(calibrate3dPipeOutput.output.perViewErrors)); for (var f : frames) { f.release(); @@ -94,7 +93,7 @@ public void calibrationPipelineTest() { File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString()); File[] directoryListing = dir.listFiles(); - Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20); + Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "unique_name_lol"); calibration3dPipeline.getSettings().boardHeight = 11; calibration3dPipeline.getSettings().boardWidth = 4; calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.DOTBOARD; @@ -139,13 +138,13 @@ public void calibrationPipelineTest() { calibration3dPipeline.finishCalibration(); assertNotNull(cal); - assertNotNull(cal.perViewErrors); - System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); + assertNotNull(cal.observations); + // System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString()); System.out.println("Dist Coeffs: " + cal.distCoeffs.toString()); - System.out.println("Standard Deviation: " + cal.standardDeviation); - System.out.println( - "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); + // System.out.println("Standard Deviation: " + cal.standardDeviation); + // System.out.println("Mean: " + + // Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); // Confirm we didn't get leaky on our mat usage // assertTrue(CVMat.getMatCount() == startMatCount); // TODO Figure out why this doesn't work in @@ -260,7 +259,7 @@ public void calibrateSquaresCommon( assertTrue(directoryListing.length >= 25); - Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20); + Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "test_squares_common"); calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD; calibration3dPipeline.getSettings().resolution = imgRes; calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height); @@ -298,7 +297,7 @@ public void calibrateSquaresCommon( // Confirm we have indeed gotten valid calibration objects assertNotNull(cal); - assertNotNull(cal.perViewErrors); + assertNotNull(cal.observations); // Confirm the calibrated center pixel is fairly close to of the "expected" location at the // center of the sensor. @@ -310,12 +309,12 @@ public void calibrateSquaresCommon( assertTrue(centerXErrPct < 10.0); assertTrue(centerYErrPct < 10.0); - System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); - System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics); + // System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); + System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString()); System.out.println("Dist Coeffs: " + cal.distCoeffs.toString()); - System.out.println("Standard Deviation: " + cal.standardDeviation); - System.out.println( - "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); + // System.out.println("Standard Deviation: " + cal.standardDeviation); + // System.out.println("Mean: " + + // Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); // 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-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java index 7b90ce027d..1fb5b536f6 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java @@ -29,7 +29,7 @@ public class PipelineManagerTest { @Test public void testUniqueName() { TestUtils.loadLibraries(); - PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of()); + PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name"); manager.addPipeline(PipelineType.Reflective, "Another"); // We now have ["New Pipeline", "Another"]