From 4ffd1fc600a30592f4586f66abadefef1286c463 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Sun, 16 Mar 2025 23:41:35 -0400 Subject: [PATCH] Use nicer array syntax --- .../networktables/NTDataPublisher.java | 4 +- .../vision/pipe/impl/ArucoDetectionPipe.java | 13 +++--- .../vision/pipe/impl/DrawCalibrationPipe.java | 15 ++++--- .../vision/pipeline/PipelineProfiler.java | 25 ++++++----- .../vision/target/TrackedTarget.java | 42 ++++++++----------- .../pipeline/CalibrationRotationPipeTest.java | 4 +- .../java/org/photonvision/PhotonCamera.java | 2 +- .../simulation/SimCameraProperties.java | 32 +++++++------- .../estimation/VisionEstimation.java | 13 +++--- .../test/java/ConstrainedSolvepnpTest.java | 9 ++-- 10 files changed, 72 insertions(+), 87 deletions(-) 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 a3a8971f9..92275a643 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 @@ -205,8 +205,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer { ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr()); ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr()); } else { - ts.cameraIntrinsicsPublisher.accept(new double[] {}); - ts.cameraDistortionPublisher.accept(new double[] {}); + ts.cameraIntrinsicsPublisher.accept(new double[0]); + ts.cameraDistortionPublisher.accept(new double[0]); } ts.heartbeatPublisher.set(acceptedResult.sequenceID); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java index 82dfd31c3..a360dd7c3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipe.java @@ -59,13 +59,12 @@ public class ArucoDetectionPipe for (var detection : detections) { double[] xCorners = detection.getXCorners(); double[] yCorners = detection.getYCorners(); - Point[] cornerPoints = - new Point[] { - new Point(xCorners[0], yCorners[0]), - new Point(xCorners[1], yCorners[1]), - new Point(xCorners[2], yCorners[2]), - new Point(xCorners[3], yCorners[3]) - }; + Point[] cornerPoints = { + new Point(xCorners[0], yCorners[0]), + new Point(xCorners[1], yCorners[1]), + new Point(xCorners[2], yCorners[2]), + new Point(xCorners[3], yCorners[3]) + }; double bltr = Math.hypot( cornerPoints[2].x - cornerPoints[0].x, cornerPoints[2].y - cornerPoints[0].y); 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 7005d66e8..c9112f445 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 @@ -32,14 +32,13 @@ import org.photonvision.vision.target.TrackedTarget; public class DrawCalibrationPipe extends MutatingPipe< Pair>, DrawCalibrationPipe.DrawCalibrationPipeParams> { - Scalar[] chessboardColors = - new Scalar[] { - ColorHelper.colorToScalar(Color.RED, 0.4), - ColorHelper.colorToScalar(Color.ORANGE, 0.4), - ColorHelper.colorToScalar(Color.GREEN, 0.4), - ColorHelper.colorToScalar(Color.BLUE, 0.4), - ColorHelper.colorToScalar(Color.MAGENTA, 0.4), - }; + Scalar[] chessboardColors = { + ColorHelper.colorToScalar(Color.RED, 0.4), + ColorHelper.colorToScalar(Color.ORANGE, 0.4), + ColorHelper.colorToScalar(Color.GREEN, 0.4), + ColorHelper.colorToScalar(Color.BLUE, 0.4), + ColorHelper.colorToScalar(Color.MAGENTA, 0.4), + }; @Override protected Void process(Pair> in) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java index 57c94b064..8d3fb88e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java @@ -37,19 +37,18 @@ public class PipelineProfiler { * output) 14 - draw2dTargetsPipe (on input) 15 - draw2dTargetsPipe (on output) 16 - * draw3dTargetsPipe (OPTIONAL, on input) 17 - draw3dTargetsPipe (OPTIONAL, on output) */ - private static final String[] ReflectivePipeNames = - new String[] { - "RotateImage", - "HSV", - "FindContours", - "SpeckleReject", - "FilterContours", - "GroupContours", - "SortContours", - "Collect2dTargets", - "CornerDetection", - "SolvePNP", - }; + private static final String[] ReflectivePipeNames = { + "RotateImage", + "HSV", + "FindContours", + "SpeckleReject", + "FilterContours", + "GroupContours", + "SortContours", + "Collect2dTargets", + "CornerDetection", + "SolvePNP", + }; public static final int ReflectivePipeCount = ReflectivePipeNames.length; 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 ee31696cf..1c5e22c05 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 @@ -122,11 +122,9 @@ public class TrackedTarget implements Releasable { tvec.put( 0, 0, - new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() - }); + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ()); setCameraRelativeTvec(tvec); // Opencv expects a 3d vector with norm = angle and direction = axis @@ -136,13 +134,12 @@ public class TrackedTarget implements Releasable { } double[] corners = tagDetection.getCorners(); - Point[] cornerPoints = - new Point[] { - new Point(corners[0], corners[1]), - new Point(corners[2], corners[3]), - new Point(corners[4], corners[5]), - new Point(corners[6], corners[7]) - }; + Point[] cornerPoints = { + new Point(corners[0], corners[1]), + new Point(corners[2], corners[3]), + new Point(corners[4], corners[5]), + new Point(corners[6], corners[7]) + }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints); @@ -198,13 +195,12 @@ public class TrackedTarget implements Releasable { double[] xCorners = result.getXCorners(); double[] yCorners = result.getYCorners(); - Point[] cornerPoints = - new Point[] { - new Point(xCorners[0], yCorners[0]), - new Point(xCorners[1], yCorners[1]), - new Point(xCorners[2], yCorners[2]), - new Point(xCorners[3], yCorners[3]) - }; + Point[] cornerPoints = { + new Point(xCorners[0], yCorners[0]), + new Point(xCorners[1], yCorners[1]), + new Point(xCorners[2], yCorners[2]), + new Point(xCorners[3], yCorners[3]) + }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints); @@ -236,11 +232,9 @@ public class TrackedTarget implements Releasable { tvec.put( 0, 0, - new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() - }); + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ()); setCameraRelativeTvec(tvec); var rvec = new Mat(3, 1, CvType.CV_64FC1); diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java index 296e70481..8e9f52c2b 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java @@ -123,7 +123,7 @@ public class CalibrationRotationPipeTest { 0.09625562194891251, -0.1860797479660746 }), - new double[] {}, + new double[0], List.of(), new Size(), 1, @@ -288,7 +288,7 @@ public class CalibrationRotationPipeTest { 0.04625562194891251, -0.0860797479660746 }), - new double[] {}, + new double[0], List.of(), new Size(), 1, diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index a60ec848f..4ca7c3077 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -138,7 +138,7 @@ public class PhotonCamera implements AutoCloseable { .getRawTopic("rawBytes") .subscribe( PhotonPipelineResult.photonStruct.getTypeString(), - new byte[] {}, + new byte[0], PubSubOption.periodic(0.01), PubSubOption.sendAll(true), PubSubOption.pollStorage(20)); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index f9b7f8a46..eb6414145 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -202,24 +202,20 @@ public class SimCameraProperties { this.distCoeffs = distCoeffs; // left, right, up, and down view planes - var p = - new Translation3d[] { - new Translation3d( - 1, - new Rotation3d(0, 0, getPixelYaw(0).plus(new Rotation2d(-Math.PI / 2)).getRadians())), - new Translation3d( - 1, - new Rotation3d( - 0, 0, getPixelYaw(resWidth).plus(new Rotation2d(Math.PI / 2)).getRadians())), - new Translation3d( - 1, - new Rotation3d( - 0, getPixelPitch(0).plus(new Rotation2d(Math.PI / 2)).getRadians(), 0)), - new Translation3d( - 1, - new Rotation3d( - 0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0)) - }; + Translation3d[] p = { + new Translation3d( + 1, new Rotation3d(0, 0, getPixelYaw(0).plus(new Rotation2d(-Math.PI / 2)).getRadians())), + new Translation3d( + 1, + new Rotation3d( + 0, 0, getPixelYaw(resWidth).plus(new Rotation2d(Math.PI / 2)).getRadians())), + new Translation3d( + 1, new Rotation3d(0, getPixelPitch(0).plus(new Rotation2d(Math.PI / 2)).getRadians(), 0)), + new Translation3d( + 1, + new Rotation3d( + 0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0)) + }; viewplanes.clear(); for (Translation3d translation3d : p) { viewplanes.add( diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 6031ec337..afd3f4c3a 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -248,13 +248,12 @@ public class VisionEstimation { } // fx fy cx cy - double[] cameraCal = - new double[] { - cameraMatrix.get(0, 0), - cameraMatrix.get(1, 1), - cameraMatrix.get(0, 2), - cameraMatrix.get(1, 2), - }; + double[] cameraCal = { + cameraMatrix.get(0, 0), + cameraMatrix.get(1, 1), + cameraMatrix.get(0, 2), + cameraMatrix.get(1, 2), + }; var guess2 = robotPoseSeed.toPose2d(); diff --git a/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java b/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java index fd7c1f1eb..40acdf2a9 100644 --- a/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java +++ b/photon-targeting/src/test/java/ConstrainedSolvepnpTest.java @@ -50,10 +50,9 @@ public class ConstrainedSolvepnpTest { @Test public void smoketest() { - double[] cameraCal = - new double[] { - 600, 600, 300, 150, - }; + double[] cameraCal = { + 600, 600, 300, 150, + }; var field2points = MatBuilder.fill( @@ -86,7 +85,7 @@ public class ConstrainedSolvepnpTest { MatBuilder.fill(Nat.N4(), Nat.N4(), 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1); // Initial guess for optimization - double[] x_guess = new double[] {0.2, 0.1, -.05}; + double[] x_guess = {0.2, 0.1, -.05}; var ret = ConstrainedSolvepnpJni.do_optimization(