diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue index 82f6d0248c..11330d5dad 100644 --- a/photon-client/src/components/cameras/CameraCalibrationCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue @@ -1,7 +1,12 @@ diff --git a/photon-client/src/components/common/LineChart.vue b/photon-client/src/components/common/LineChart.vue index e29dbc45c2..a8d8f54cf8 100644 --- a/photon-client/src/components/common/LineChart.vue +++ b/photon-client/src/components/common/LineChart.vue @@ -6,7 +6,7 @@ export default { props: { chartData: { type: Object, - default: null, + default: null }, min: String, max: String, @@ -22,8 +22,8 @@ export default { legend: { labels: { fontColor: "white", - fontSize: 12, - }, + fontSize: 12 + } }, scales: { yAxes: [ @@ -46,11 +46,11 @@ export default { // fontSize: 12, // } // } - ], - }, + ] + } }; - }, - }, + } + } }, computed: { chartOptions: { @@ -58,19 +58,15 @@ export default { const opts = this.options; if (this.min) { - opts.scales.yAxes.forEach( - (it) => (it.ticks.suggestedMin = parseFloat(this.min)) - ); + opts.scales.yAxes.forEach((it) => (it.ticks.suggestedMin = parseFloat(this.min))); } if (this.max) { - opts.scales.yAxes.forEach( - (it) => (it.ticks.suggestedMax = parseFloat(this.max)) - ); + opts.scales.yAxes.forEach((it) => (it.ticks.suggestedMax = parseFloat(this.max))); } return opts; - }, - }, + } + } }, mounted() { this.renderChart(this.chartData, this.chartOptions); @@ -83,7 +79,7 @@ export default { methods: { update() { this.renderChart(this.chartData, this.chartOptions); - }, - }, + } + } }; diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts index 44766033a2..28ce0bbf6b 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -80,32 +80,32 @@ export interface VideoFormat { } export interface JsonMat { - rows: number - cols: number - type: number - data: number[] + rows: number; + cols: number; + type: number; + data: number[]; } export interface Point3 { - x: number - y: number - z: number + x: number; + y: number; + z: number; } export interface Point2 { - x: number - y: number + x: number; + y: number; } export interface Pose3d { - translation: { x: number, y: number, z: number } - rotation: { quaternion: { W: number, X: number, Y: number, Z: number } } + translation: { x: number; y: number; z: number }; + rotation: { quaternion: { W: number; X: number; Y: number; Z: number } }; } export interface BoardObservation { - locationInObjectSpace: Point3[] - locationInImageSpace: Point2[] - reprojectionErrors: Point2[] - optimisedCameraToObject: Pose3d[] + locationInObjectSpace: Point3[]; + locationInImageSpace: Point2[]; + reprojectionErrors: Point2[]; + optimisedCameraToObject: Pose3d; } export interface CameraCalibrationResult { @@ -113,7 +113,7 @@ export interface CameraCalibrationResult { cameraIntrinsics: JsonMat; // TODO rename to be Right cameraExtrinsics: JsonMat; - observations: BoardObservation[] + observations: BoardObservation[]; } export interface ConfigurableCameraSettings { @@ -156,19 +156,46 @@ 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: 123, y: 456 }], + 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 }] + } + ] } ], - 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 3c4cd1bc5a..b62d2e788e 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -1,4 +1,11 @@ -import type { CameraCalibrationResult, 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"; 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 b36d85980b..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,6 +24,7 @@ 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/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 62d23d3c70..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 @@ -24,14 +24,10 @@ 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.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.WPIUtilJNI; import java.util.Arrays; import java.util.List; -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -import org.ejml.simple.SimpleMatrix; import org.opencv.core.Core; import org.opencv.core.Mat; 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 7c0de1d024..41d77a6eaa 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 @@ -23,12 +23,8 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.JsonNode; - import edu.wpi.first.math.geometry.Pose3d; - -import java.util.Arrays; import java.util.List; - import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Point; @@ -41,22 +37,21 @@ public class CameraCalibrationCoefficients implements Releasable { public static final class BoardObservation { @JsonProperty("locationInObjectSpace") public List locationInObjectSpace; // Expected feature 3d location in the camera frame + @JsonProperty("locationInImageSpace") public List locationInImageSpace; // Observed location in pixel space + @JsonProperty("reprojectionErrors") public List reprojectionErrors; // (measured location in pixels) - (expected from FK) + public Pose3d optimisedCameraToObject; // Solver optimized board poses @JsonCreator public BoardObservation( - @JsonProperty("locationInObjectSpace") - List locationInObjectSpace, - @JsonProperty("locationInImageSpace") - List locationInImageSpace, - @JsonProperty("reprojectionErrors") - List reprojectionErrors, - @JsonProperty("optimisedCameraToObject") - Pose3d optimisedCameraToObject) { + @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; diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index c33337fa6c..d0406e09f4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -216,8 +216,9 @@ public void setExposure(double exposure) { } else if (camera.getProperty("exposure_time_absolute").getKind() != Kind.kNone) { // Seems like the name changed at some point in v4l? set it instead var prop = camera.getProperty("exposure_time_absolute"); - var exposure_manual_val = MathUtils.map(Math.round(exposure), 0, 100, prop.getMin(), prop.getMax()); - prop.set((int)exposure_manual_val); + var exposure_manual_val = + MathUtils.map(Math.round(exposure), 0, 100, prop.getMin(), prop.getMax()); + prop.set((int) exposure_manual_val); } else { scaledExposure = (int) Math.round(exposure); logger.debug("Setting camera exposure to " + scaledExposure); 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 5625b01ac7..dcbda9b4ff 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 @@ -19,25 +19,21 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; - -import edu.wpi.first.math.geometry.Pose3d; - import java.util.ArrayList; import java.util.List; 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.opencv.core.*; -import org.opencv.imgproc.Imgproc; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import org.photonvision.vision.calibration.JsonMat; import org.photonvision.vision.calibration.CameraCalibrationCoefficients.BoardObservation; +import org.photonvision.vision.calibration.JsonMat; import org.photonvision.vision.pipe.CVPipe; public class Calibrate3dPipe @@ -118,7 +114,7 @@ protected CameraCalibrationCoefficients process(List> in) JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix); JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients); - + // For each observation, calc reprojection error Mat jac_temp = new Mat(); List observations = new ArrayList<>(); @@ -126,11 +122,19 @@ protected CameraCalibrationCoefficients process(List> in) 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 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); + 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; @@ -151,7 +155,6 @@ protected CameraCalibrationCoefficients process(List> in) observations.add(new BoardObservation(i_objPts, i_imgPts, reprojectionError, camToBoard)); } jac_temp.release(); - // Standard deviation of results try { 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 index 16b91a216f..a1207030a9 100644 --- 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 @@ -33,37 +33,29 @@ public class DrawCalibrationPipe 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()); + 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); + 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) { + 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 b1de0e7ea2..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 @@ -17,8 +17,6 @@ package org.photonvision.vision.pipe.impl; -import java.util.Arrays; -import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.lang3.tuple.Triple; import org.opencv.calib3d.Calib3d; 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 105e68525f..06bfee838b 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,10 +20,8 @@ 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; @@ -38,7 +36,6 @@ 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.CameraCalibrationCoefficients; import org.photonvision.vision.calibration.CameraCalibrationCoefficients.BoardObservation; import org.photonvision.vision.frame.Frame; @@ -158,12 +155,13 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se // new MultiTargetPNPResult(), new Frame( new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties), - getCornersList() - ); + getCornersList()); } List> getCornersList() { - return foundCornersList.stream().map(it -> ((MatOfPoint2f)it.getRight()).toList()).collect(Collectors.toList()); + return foundCornersList.stream() + .map(it -> ((MatOfPoint2f) it.getRight()).toList()) + .collect(Collectors.toList()); } public void deleteSavedImages() { 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 c2ed46c26e..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 @@ -115,7 +115,8 @@ protected void setPipeParams( resizeImagePipe.setParams( new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor)); - drawCalibrationPipe.setParams(new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor)); + drawCalibrationPipe.setParams( + new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor)); } public CVPipelineResult process( @@ -177,13 +178,13 @@ public CVPipelineResult process( pipeProfileNanos[8] = 0; } } else if (settings instanceof Calibration3dPipelineSettings) { - pipeProfileNanos[5] = 0; - pipeProfileNanos[6] = 0; + pipeProfileNanos[5] = 0; + pipeProfileNanos[6] = 0; - var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw)); - sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed; + var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw)); + sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed; - pipeProfileNanos[8] = 0; + 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/result/CalibrationPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java index 3eefb597da..7ad8ff83cf 100644 --- 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 @@ -19,19 +19,17 @@ 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) { + 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/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 2faf1593f9..982cd64864 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 @@ -38,7 +38,6 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; -import org.photonvision.common.util.file.JacksonUtils; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.camera.LibcameraGpuSource; 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 87de685cd5..c10927f0da 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 @@ -148,7 +148,8 @@ public TrackedTarget( 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)));; + 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();