From c8c9e779ab9d8af5f42688c4d7f3284914822477 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 15 Oct 2023 10:44:47 -0700 Subject: [PATCH 01/10] [photon-core] 2D Detection data accuracy (#896) Use calibration data for 2d target info when available (principal point, FOV) Correct perspective distortion in 2d yaw/pitch info --- .../vision/frame/FrameStaticProperties.java | 55 ++++--- .../vision/target/TargetCalculations.java | 31 +++- .../vision/target/TrackedTarget.java | 55 ++++--- .../vision/target/TargetCalculationsTest.java | 147 +++++++++++++----- .../org/photonvision/VisionSystemSimTest.java | 8 +- 5 files changed, 200 insertions(+), 96 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index f03db3a60e..cedda04d22 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java @@ -39,7 +39,7 @@ public class FrameStaticProperties { * Instantiates a new Frame static properties. * * @param mode The Video Mode of the camera. - * @param fov The fov of the image. + * @param fov The FOV (Field Of Vision) of the image in degrees. */ public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoefficients cal) { this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cal); @@ -48,9 +48,9 @@ public FrameStaticProperties(VideoMode mode, double fov, CameraCalibrationCoeffi /** * Instantiates a new Frame static properties. * - * @param imageWidth The width of the image. - * @param imageHeight The width of the image. - * @param fov The fov of the image. + * @param imageWidth The width of the image in pixels. + * @param imageHeight The width of the image in pixels. + * @param fov The FOV (Field Of Vision) of the image in degrees. */ public FrameStaticProperties( int imageWidth, int imageHeight, double fov, CameraCalibrationCoefficients cal) { @@ -61,30 +61,47 @@ public FrameStaticProperties( imageArea = this.imageWidth * this.imageHeight; - // Todo -- if we have calibration, use it's center point? - centerX = ((double) this.imageWidth / 2) - 0.5; - centerY = ((double) this.imageHeight / 2) - 0.5; - centerPoint = new Point(centerX, centerY); - - // TODO if we have calibration use it here instead // pinhole model calculations - DoubleCouple horizVertViews = - calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight); + if (cameraCalibration != null && cameraCalibration.getCameraIntrinsicsMat() != null) { + // Use calibration data + var camIntrinsics = cameraCalibration.getCameraIntrinsicsMat(); + centerX = camIntrinsics.get(0, 2)[0]; + centerY = camIntrinsics.get(1, 2)[0]; + centerPoint = new Point(centerX, centerY); + horizontalFocalLength = camIntrinsics.get(0, 0)[0]; + verticalFocalLength = camIntrinsics.get(1, 1)[0]; + } else { + // No calibration data. Calculate from user provided diagonal FOV + centerX = (this.imageWidth / 2.0) - 0.5; + centerY = (this.imageHeight / 2.0) - 0.5; + centerPoint = new Point(centerX, centerY); - horizontalFocalLength = this.imageWidth / (2 * Math.tan(horizVertViews.getFirst() / 2)); - verticalFocalLength = this.imageHeight / (2 * Math.tan(horizVertViews.getSecond() / 2)); + DoubleCouple horizVertViews = + calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight); + double horizFOV = Math.toRadians(horizVertViews.getFirst()); + double vertFOV = Math.toRadians(horizVertViews.getSecond()); + horizontalFocalLength = (this.imageWidth / 2.0) / Math.tan(horizFOV / 2.0); + verticalFocalLength = (this.imageHeight / 2.0) / Math.tan(vertFOV / 2.0); + } } + /** + * Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size. + * + * @param diagonalFoV Diagonal FOV in degrees + * @param imageWidth Image width in pixels + * @param imageHeight Image height in pixels + * @return Horizontal and vertical FOV in degrees + */ public static DoubleCouple calculateHorizontalVerticalFoV( double diagonalFoV, int imageWidth, int imageHeight) { - double diagonalView = Math.toRadians(diagonalFoV); + diagonalFoV = Math.toRadians(diagonalFoV); double diagonalAspect = Math.hypot(imageWidth, imageHeight); double horizontalView = - Math.atan(Math.tan(diagonalView / 2) * (imageWidth / diagonalAspect)) * 2; - double verticalView = - Math.atan(Math.tan(diagonalView / 2) * (imageHeight / diagonalAspect)) * 2; + Math.atan(Math.tan(diagonalFoV / 2) * (imageWidth / diagonalAspect)) * 2; + double verticalView = Math.atan(Math.tan(diagonalFoV / 2) * (imageHeight / diagonalAspect)) * 2; - return new DoubleCouple(horizontalView, verticalView); + return new DoubleCouple(Math.toDegrees(horizontalView), Math.toDegrees(verticalView)); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java index 60b278aae4..5e64237443 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java @@ -23,14 +23,29 @@ import org.photonvision.vision.opencv.DualOffsetValues; public class TargetCalculations { - public static double calculateYaw( - double offsetCenterX, double targetCenterX, double horizontalFocalLength) { - return Math.toDegrees(Math.atan((offsetCenterX - targetCenterX) / horizontalFocalLength)); - } - - public static double calculatePitch( - double offsetCenterY, double targetCenterY, double verticalFocalLength) { - return -Math.toDegrees(Math.atan((offsetCenterY - targetCenterY) / verticalFocalLength)); + /** + * Calculates the yaw and pitch of a point in the image. Yaw and pitch must be calculated together + * to account for perspective distortion. Yaw is positive right, and pitch is positive up. + * + * @param offsetCenterX The X value of the offset principal point (cx) in pixels + * @param targetCenterX The X value of the target's center point in pixels + * @param horizontalFocalLength The horizontal focal length (fx) in pixels + * @param offsetCenterY The Y value of the offset principal point (cy) in pixels + * @param targetCenterY The Y value of the target's center point in pixels + * @param verticalFocalLength The vertical focal length (fy) in pixels + * @return The yaw and pitch from the principal axis to the target center, in degrees. + */ + public static DoubleCouple calculateYawPitch( + double offsetCenterX, + double targetCenterX, + double horizontalFocalLength, + double offsetCenterY, + double targetCenterY, + double verticalFocalLength) { + double yaw = Math.atan((targetCenterX - offsetCenterX) / horizontalFocalLength); + double pitch = + Math.atan((offsetCenterY - targetCenterY) / (verticalFocalLength / Math.cos(yaw))); + return new DoubleCouple(Math.toDegrees(yaw), Math.toDegrees(pitch)); } public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) { 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 72c5cdf600..922ec5e6ec 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 @@ -73,13 +73,16 @@ public TrackedTarget( TargetCalculationParameters params) { m_targetOffsetPoint = new Point(tagDetection.getCenterX(), tagDetection.getCenterY()); m_robotOffsetPoint = new Point(); - - m_pitch = - TargetCalculations.calculatePitch( - tagDetection.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength); - m_yaw = - TargetCalculations.calculateYaw( - tagDetection.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength); + var yawPitch = + TargetCalculations.calculateYawPitch( + params.cameraCenterPoint.x, + tagDetection.getCenterX(), + params.horizontalFocalLength, + params.cameraCenterPoint.y, + tagDetection.getCenterY(), + params.verticalFocalLength); + m_yaw = yawPitch.getFirst(); + m_pitch = yawPitch.getSecond(); var bestPose = new Transform3d(); var altPose = new Transform3d(); @@ -138,13 +141,16 @@ public TrackedTarget( public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) { m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY()); m_robotOffsetPoint = new Point(); - - m_pitch = - TargetCalculations.calculatePitch( - result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength); - m_yaw = - TargetCalculations.calculateYaw( - result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength); + var yawPitch = + TargetCalculations.calculateYawPitch( + params.cameraCenterPoint.x, + result.getCenterX(), + params.horizontalFocalLength, + params.cameraCenterPoint.y, + result.getCenterY(), + params.verticalFocalLength); + m_yaw = yawPitch.getFirst(); + m_pitch = yawPitch.getSecond(); double[] xCorners = result.getxCorners(); double[] yCorners = result.getyCorners(); @@ -246,7 +252,7 @@ public MatOfPoint2f getApproximateBoundingPolygon() { } public void calculateValues(TargetCalculationParameters params) { - // this MUST happen in this exact order! + // this MUST happen in this exact order! (TODO: document why) m_targetOffsetPoint = TargetCalculations.calculateTargetOffsetPoint( params.isLandscape, params.targetOffsetPointEdge, getMinAreaRect()); @@ -257,13 +263,18 @@ public void calculateValues(TargetCalculationParameters params) { params.dualOffsetValues, params.robotOffsetPointMode); - // order of this stuff doesn't matter though - m_pitch = - TargetCalculations.calculatePitch( - m_targetOffsetPoint.y, m_robotOffsetPoint.y, params.verticalFocalLength); - m_yaw = - TargetCalculations.calculateYaw( - m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength); + // order of this stuff doesnt matter though + var yawPitch = + TargetCalculations.calculateYawPitch( + m_robotOffsetPoint.x, + m_targetOffsetPoint.x, + params.horizontalFocalLength, + m_robotOffsetPoint.y, + m_targetOffsetPoint.y, + params.verticalFocalLength); + m_yaw = yawPitch.getFirst(); + m_pitch = yawPitch.getSecond(); + m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea * 100; m_skew = TargetCalculations.calculateSkew(params.isLandscape, getMinAreaRect()); diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index 0dd36d90e9..7b23d3a600 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -17,14 +17,19 @@ package org.photonvision.vision.target; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.stream.Stream; import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.opencv.core.RotatedRect; -import org.opencv.core.Size; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.numbers.DoubleCouple; @@ -33,9 +38,9 @@ public class TargetCalculationsTest { - private static final Size imageSize = new Size(800, 600); - private static final Point imageCenterPoint = - new Point(imageSize.width / 2, imageSize.height / 2); + private static Size imageSize = new Size(800, 600); + private static Point imageCenterPoint = + new Point(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5); private static final double diagFOV = Math.toRadians(70.0); private static final FrameStaticProperties props = @@ -52,43 +57,104 @@ public class TargetCalculationsTest { props.verticalFocalLength, imageSize.width * imageSize.height); - @BeforeEach - public void Init() { + @BeforeAll + public static void setup() { TestUtils.loadLibraries(); } @Test - public void yawTest() { - var targetPixelOffsetX = 100; - var targetCenterPoint = new Point(imageCenterPoint.x + targetPixelOffsetX, imageCenterPoint.y); - - var trueYaw = - Math.atan((imageCenterPoint.x - targetCenterPoint.x) / params.horizontalFocalLength); - - var yaw = - TargetCalculations.calculateYaw( - imageCenterPoint.x, targetCenterPoint.x, params.horizontalFocalLength); - - assertEquals(Math.toDegrees(trueYaw), yaw, 0.025, "Yaw not as expected"); + public void testYawPitchBehavior() { + double targetPixelOffsetX = 100; + double targetPixelOffsetY = 100; + var targetCenterPoint = + new Point(imageCenterPoint.x + targetPixelOffsetX, imageCenterPoint.y + targetPixelOffsetY); + + var targetYawPitch = + TargetCalculations.calculateYawPitch( + imageCenterPoint.x, + targetCenterPoint.x, + params.horizontalFocalLength, + imageCenterPoint.y, + targetCenterPoint.y, + params.verticalFocalLength); + + assertTrue(targetYawPitch.getFirst() > 0, "Yaw is not positive right"); + assertTrue(targetYawPitch.getSecond() < 0, "Pitch is not positive up"); + + var fovs = + FrameStaticProperties.calculateHorizontalVerticalFoV( + diagFOV, (int) imageSize.width, (int) imageSize.height); + var maxYaw = + TargetCalculations.calculateYawPitch( + imageCenterPoint.x, + 2 * imageCenterPoint.x, + params.horizontalFocalLength, + imageCenterPoint.y, + imageCenterPoint.y, + params.verticalFocalLength); + assertEquals(fovs.getFirst() / 2.0, maxYaw.getFirst(), 0.025, "Horizontal FOV check failed"); + var maxPitch = + TargetCalculations.calculateYawPitch( + imageCenterPoint.x, + imageCenterPoint.x, + params.horizontalFocalLength, + imageCenterPoint.y, + 0, + params.verticalFocalLength); + assertEquals(fovs.getSecond() / 2.0, maxPitch.getSecond(), 0.025, "Vertical FOV check failed"); } - @Test - public void pitchTest() { - var targetPixelOffsetY = 100; - var targetCenterPoint = new Point(imageCenterPoint.x, imageCenterPoint.y + targetPixelOffsetY); - - var truePitch = - Math.atan((imageCenterPoint.y - targetCenterPoint.y) / params.verticalFocalLength); - - var pitch = - TargetCalculations.calculatePitch( - imageCenterPoint.y, targetCenterPoint.y, params.verticalFocalLength); + private static Stream testYawPitchCalcArgs() { + return Stream.of( + // (yaw, pitch) in degrees + Arguments.of(0, 0), + Arguments.of(10, 0), + Arguments.of(0, 10), + Arguments.of(10, 10), + Arguments.of(-10, -10), + Arguments.of(30, 45), + Arguments.of(-45, -20)); + } - assertEquals(Math.toDegrees(truePitch) * -1, pitch, 0.025, "Pitch not as expected"); + private static double[] testCameraMatrix = {240, 0, 320, 0, 240, 320, 0, 0, 1}; + + @ParameterizedTest + @MethodSource("testYawPitchCalcArgs") + public void testYawPitchCalc(double yawDeg, double pitchDeg) { + Mat testCameraMat = new Mat(3, 3, CvType.CV_64F); + testCameraMat.put(0, 0, testCameraMatrix); + // Since we create this translation using the given yaw/pitch, we should see the same angles + // calculated + var targetTrl = + new Translation3d(1, new Rotation3d(0, Math.toRadians(pitchDeg), Math.toRadians(yawDeg))); + // NWU to EDN + var objectPoints = + new MatOfPoint3f(new Point3(-targetTrl.getY(), -targetTrl.getZ(), targetTrl.getX())); + var imagePoints = new MatOfPoint2f(); + // Project translation into camera image + Calib3d.projectPoints( + objectPoints, + new MatOfDouble(0, 0, 0), + new MatOfDouble(0, 0, 0), + testCameraMat, + new MatOfDouble(0, 0, 0, 0, 0), + imagePoints); + var point = imagePoints.toArray()[0]; + // Test if the target yaw/pitch calculation matches what the target was created with + var yawPitch = + TargetCalculations.calculateYawPitch( + point.x, + testCameraMatrix[2], + testCameraMatrix[0], + point.y, + testCameraMatrix[5], + testCameraMatrix[4]); + assertEquals(yawDeg, yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect"); + assertEquals(pitchDeg, yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect"); } @Test - public void targetOffsetTest() { + public void testTargetOffset() { Point center = new Point(0, 0); Size rectSize = new Size(10, 5); double angle = 30; @@ -105,11 +171,6 @@ public void targetOffsetTest() { assertEquals(2.17, result.y, 0.1, "Target offset Y not as expected"); } - public static void main(String[] args) { - TestUtils.loadLibraries(); - new TargetCalculationsTest().targetOffsetTest(); - } - @Test public void testSkewCalculation() { // Setup @@ -191,14 +252,14 @@ public void testSkewCalculation() { public void testCameraFOVCalculation() { final DoubleCouple glowormHorizVert = FrameStaticProperties.calculateHorizontalVerticalFoV(74.8, 640, 480); - var gwHorizDeg = Math.toDegrees(glowormHorizVert.getFirst()); - var gwVertDeg = Math.toDegrees(glowormHorizVert.getSecond()); + var gwHorizDeg = glowormHorizVert.getFirst(); + var gwVertDeg = glowormHorizVert.getSecond(); assertEquals(62.7, gwHorizDeg, .3); assertEquals(49, gwVertDeg, .3); } @Test - public void robotOffsetDualTest() { + public void testDualOffsetCrosshair() { final DualOffsetValues dualOffsetValues = new DualOffsetValues( new Point(400, 150), 10, diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 5ab7d00ce2..c53e006717 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -297,12 +297,12 @@ public void testYawAngles(double testYaw) { var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); - assertEquals(tgt.getYaw(), testYaw, kRotDeltaDeg); + assertEquals(testYaw, tgt.getYaw(), kRotDeltaDeg); } @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) - public void testCameraPitch(double testPitch) { + public void testPitchAngles(double testPitch) { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); @@ -331,7 +331,7 @@ public void testCameraPitch(double testPitch) { assertEquals(testPitch, tgt.getPitch(), kRotDeltaDeg); } - private static Stream distCalCParamProvider() { + private static Stream testDistanceCalcArgs() { // Arbitrary and fairly random assortment of distances, camera pitches, and heights return Stream.of( Arguments.of(5, -15.98, 0), @@ -354,7 +354,7 @@ private static Stream distCalCParamProvider() { } @ParameterizedTest - @MethodSource("distCalCParamProvider") + @MethodSource("testDistanceCalcArgs") public void testDistanceCalc(double testDist, double testPitch, double testHeight) { // Assume dist along ground and tgt height the same. Iterate over other parameters. From 7f949627918bf6c348797d40971d034fe5757d63 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 15 Oct 2023 13:45:30 -0400 Subject: [PATCH 02/10] Build photonlib json first (#952) --- photon-lib/build.gradle | 13 ++++++------- photon-lib/publish.gradle | 3 +-- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index d7952394df..baae8fcc7d 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -120,23 +120,22 @@ task generateVendorJson() { outputs.file photonlibFileOutput inputs.file photonlibFileInput - doLast { - println "Writing version ${pubVersion} to $photonlibFileOutput" + println "Writing vendor JSON ${pubVersion} to $photonlibFileOutput" if (photonlibFileOutput.exists()) { photonlibFileOutput.delete() } + photonlibFileOutput.parentFile.mkdirs() + def read = photonlibFileInput.text .replace('${photon_version}', pubVersion) .replace('${frc_year}', frcYear) - photonlibFileOutput.write(read) - } + photonlibFileOutput.text = read outputs.upToDateWhen { false } } -build.dependsOn generateVendorJson - +build.mustRunAfter generateVendorJson task writeCurrentVersion { def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in") @@ -147,7 +146,7 @@ task writeCurrentVersion { versionString) } -build.dependsOn writeCurrentVersion +build.mustRunAfter writeCurrentVersion tasks.withType(Javadoc) { options.encoding = 'UTF-8' diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle index 9967b24ee2..ca67c43b0e 100644 --- a/photon-lib/publish.gradle +++ b/photon-lib/publish.gradle @@ -198,8 +198,7 @@ model { } } -// So I don't actually know the _right_ way to tell gradle that the vendordep json publish requires generation first, so we're doing this -getTasksByName("publishVendorjsonPublicationToMavenLocal", false).each { +tasks.withType(PublishToMavenRepository) { it.mustRunAfter generateVendorJson } From ad4f462fd62fabfe5268ea58f7d5bd69c5d94875 Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 15 Oct 2023 10:46:55 -0700 Subject: [PATCH 03/10] [photon-core] [2024] Cleanup and document coordinate system conversion (#894) * cleanup and document coordinate conversion * spotless * bump wpilib version * coordinate conversion tests * fix/document SolvePNPPipe models and corners * format * run lint --------- Co-authored-by: Matthew Morley --- .../common/util/math/MathUtils.java | 93 +++++++------------ .../vision/pipe/impl/CornerDetectionPipe.java | 61 ++++++------ .../vision/pipe/impl/Draw3dTargetsPipe.java | 31 +++---- .../vision/pipe/impl/SolvePNPPipe.java | 29 +----- .../vision/pipeline/AprilTagPipeline.java | 6 +- .../vision/target/TargetModel.java | 93 ++++++++++--------- .../common/util/CoordinateConversionTest.java | 79 ++++++++++++++++ .../vision/pipeline/AprilTagTest.java | 13 ++- .../vision/pipeline/SolvePNPTest.java | 70 ++++++++------ 9 files changed, 258 insertions(+), 217 deletions(-) create mode 100644 photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java 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 526311afc5..2b6bb31ae3 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 @@ -17,10 +17,12 @@ package org.photonvision.common.util.math; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.CoordinateSystem; +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.util.Units; import edu.wpi.first.util.WPIUtilJNI; import java.util.Arrays; @@ -137,73 +139,46 @@ public static double lerp(double startValue, double endValue, double t) { return startValue + (endValue - startValue) * t; } - public static Pose3d EDNtoNWU(final Pose3d pose) { - // Change of basis matrix from EDN to NWU. Each column vector is one of the - // old basis vectors mapped to its representation in the new basis. - // - // E (+X) -> N (-Y), D (+Y) -> W (-Z), N (+Z) -> U (+X) - var R = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0); - - // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ - double w = Math.sqrt(1.0 + R.get(0, 0) + R.get(1, 1) + R.get(2, 2)) / 2.0; - double x = (R.get(2, 1) - R.get(1, 2)) / (4.0 * w); - double y = (R.get(0, 2) - R.get(2, 0)) / (4.0 * w); - double z = (R.get(1, 0) - R.get(0, 1)) / (4.0 * w); - var rotationQuat = new Rotation3d(new Quaternion(w, x, y, z)); - - return new Pose3d( - pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat)); - } - /** - * All our solvepnp code returns a tag with X left, Y up, and Z out of the tag To better match - * wpilib, we want to apply another rotation so that we get Z up, X out of the tag, and Y to the - * right. We apply the following change of basis: X -> Y Y -> Z Z -> X + * OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target + * transformation from EDN to NWU. + * + *

Note: The detected target's rvec and tvec perform a rotation-translation transformation + * which converts points in the target's coordinate system to the camera's. This means applying + * the transformation to the target point (0,0,0) for example would give the target's center + * relative to the camera. Conveniently, if we make a translation-rotation transformation out of + * these components instead, we get the transformation from the camera to the target. + * + * @param cameraToTarget3d A camera-to-target Transform3d in EDN. + * @return A camera-to-target Transform3d in NWU. */ - private static final Rotation3d WPILIB_BASE_ROTATION = - new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0)); - public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) { // TODO: Refactor into new pipe? - // CameraToTarget _should_ be in opencv-land EDN - var nwu = - CoordinateSystem.convert( - new Pose3d().transformBy(cameraToTarget3d), - CoordinateSystem.EDN(), - CoordinateSystem.NWU()); - return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation())); - } - - public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) { - // TODO: Refactor into new pipe? - // CameraToTarget _should_ be in opencv-land EDN - var nwu = - CoordinateSystem.convert( - new Pose3d().transformBy(cameraToTarget3d), - CoordinateSystem.EDN(), - CoordinateSystem.NWU()); - return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation())); + return CoordinateSystem.convert( + cameraToTarget3d, CoordinateSystem.EDN(), CoordinateSystem.NWU()); } /* - * The AprilTag pose rotation outputs are X left, Y down, Z away from the tag - * with the tag facing - * the camera upright and the camera facing the target parallel to the floor. - * But our OpenCV - * solvePNP code would have X left, Y up, Z towards the camera with the target - * facing the camera - * and both parallel to the floor. So we apply a base rotation to the rotation - * component of the - * apriltag pose to make it consistent with the EDN system that OpenCV uses, - * internally a 180 - * rotation about the X axis + * From the AprilTag repo: + * "The coordinate system has the origin at the camera center. The z-axis points from the camera + * center out the camera lens. The x-axis is to the right in the image taken by the camera, and + * y is down. The tag's coordinate frame is centered at the center of the tag, with x-axis to the + * right, y-axis down, and z-axis into the tag." + * + * This means our detected transformation will be in EDN. Our subsequent uses of the transformation, + * however, assume the tag's z-axis point away from the tag instead of into it. This means we + * have to correct the transformation's rotation. */ private static final Rotation3d APRILTAG_BASE_ROTATION = - new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180)); + new Rotation3d(VecBuilder.fill(0, 1, 0), Units.degreesToRadians(180)); /** - * Apply a 180-degree rotation about X to the rotation component of a given Apriltag pose. This - * aligns it with the OpenCV poses we use in other places. + * AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag + * instead of away from it and towards the camera. This means we have to correct the + * transformation's rotation. + * + * @param pose The Transform3d with translation and rotation directly from the {@link + * AprilTagPoseEstimate}. */ public static Transform3d convertApriltagtoOpenCV(Transform3d pose) { var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation()); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java index ff7f6fd3ea..bed3c6b5ef 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipe.impl; -import edu.wpi.first.math.geometry.Translation2d; import java.util.*; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; @@ -25,6 +24,10 @@ import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.target.TrackedTarget; +/** + * Determines the target corners of the {@link TrackedTarget}. The {@link + * CornerDetectionPipeParameters} affect how these corners are calculated. + */ public class CornerDetectionPipe extends CVPipe< List, @@ -49,15 +52,15 @@ protected List process(List targetList) { } /** - * @param target the target to find the corners of. - * @return the corners. left top, left bottom, right bottom, right top + * @param target The target to find the corners of. + * @return Corners: (bottom-left, bottom-right, top-right, top-left) */ private List findBoundingBoxCorners(TrackedTarget target) { // extract the corners var points = new Point[4]; target.m_mainContour.getMinAreaRect().points(points); - // find the tl/tr/bl/br corners + // find the bl/br/tr/tl corners // first, min by left/right var list_ = Arrays.asList(points); list_.sort(leftRightComparator); @@ -68,13 +71,12 @@ private List findBoundingBoxCorners(TrackedTarget target) { var right = new ArrayList<>(List.of(list_.get(2), list_.get(3))); right.sort(verticalComparator); - // tl tr bl br var tl = left.get(0); var bl = left.get(1); var tr = right.get(0); var br = right.get(1); - return List.of(tl, bl, br, tr); + return List.of(bl, br, tr, tl); } /** @@ -83,31 +85,22 @@ private List findBoundingBoxCorners(TrackedTarget target) { * @return The straight line distance between them. */ private static double distanceBetween(Point a, Point b) { - return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2)); - } - - /** - * @param a First point. - * @param b Second point. - * @return The straight line distance between them. - */ - private static double distanceBetween(Translation2d a, Translation2d b) { - return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2)); + double xDelta = a.x - b.x; + double yDelta = a.y - b.y; + return Math.sqrt(xDelta * xDelta + yDelta * yDelta); } /** - * Find the 4 most extreme corners, + * Find the 4 most extreme corners of the target's contour. * - * @param target the target to track. - * @param convexHull weather to use the convex hull of the target. - * @return the 4 extreme corners of the contour. + * @param target The target to track. + * @param convexHull Whether to use the convex hull of the contour instead. + * @return The 4 extreme corners of the contour: (bottom-left, bottom-right, top-right, top-left) */ private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) { var centroid = target.getMinAreaRect().center; - Comparator distanceProvider = - Comparator.comparingDouble( - (Point point) -> - Math.sqrt(Math.pow(centroid.x - point.x, 2) + Math.pow(centroid.y - point.y, 2))); + Comparator compareCenterDist = + Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point)); MatOfPoint2f targetContour; if (convexHull) { @@ -141,17 +134,17 @@ private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo // left top, left bottom, right bottom, right top var boundingBoxCorners = findBoundingBoxCorners(target); - var distanceToTlComparator = - Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0))); - - var distanceToTrComparator = + var compareDistToTl = Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3))); - // top left and top right are the poly corners closest to the bounding box tl and tr - pointList.sort(distanceToTlComparator); + var compareDistToTr = + Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(2))); + + // top left and top right are the poly corners closest to the bouding box tl and tr + pointList.sort(compareDistToTl); var tl = pointList.get(0); pointList.remove(tl); - pointList.sort(distanceToTrComparator); + pointList.sort(compareDistToTr); var tr = pointList.get(0); pointList.remove(tr); @@ -177,11 +170,11 @@ private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo } } if (leftList.isEmpty() || rightList.isEmpty()) return null; - leftList.sort(distanceProvider); - rightList.sort(distanceProvider); + leftList.sort(compareCenterDist); + rightList.sort(compareCenterDist); var bl = leftList.get(leftList.size() - 1); var br = rightList.get(rightList.size() - 1); - return List.of(tl, bl, br, tr); + return List.of(bl, br, tr, tl); } public static class CornerDetectionPipeParameters { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index b88bd0ba17..99927ea0f1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -127,7 +127,7 @@ protected Void process(Pair> in) { // Draw X, Y and Z axis MatOfPoint3f pointMat = new MatOfPoint3f(); - // Those points are in opencv-land, but we are in NWU + // OpenCV expects coordinates in EDN, but we want to visualize in NWU // NWU | EDN // X: Z // Y: -X @@ -136,11 +136,15 @@ protected Void process(Pair> in) { var list = List.of( new Point3(0, 0, 0), - new Point3(0, 0, AXIS_LEN), - new Point3(AXIS_LEN, 0, 0), - new Point3(0, AXIS_LEN, 0)); + new Point3(0, 0, AXIS_LEN), // x-axis + new Point3(-AXIS_LEN, 0, 0), // y-axis + new Point3(0, -AXIS_LEN, 0)); // z-axis pointMat.fromList(list); + // The detected target's rvec and tvec perform a rotation-translation transformation which + // converts points in the target's coordinate system to the camera's. This means applying + // the transformation to the target point (0,0,0) for example would give the target's center + // relative to the camera. Calib3d.projectPoints( pointMat, target.getCameraRelativeRvec(), @@ -152,19 +156,22 @@ protected Void process(Pair> in) { var axisPoints = tempMat.toList(); dividePointList(axisPoints); - // Red = x, green y, blue z + // XYZ is RGB + // y-axis = green Imgproc.line( in.getLeft(), axisPoints.get(0), axisPoints.get(2), ColorHelper.colorToScalar(Color.GREEN), 3); + // z-axis = blue Imgproc.line( in.getLeft(), axisPoints.get(0), axisPoints.get(3), ColorHelper.colorToScalar(Color.BLUE), 3); + // x-axis = red Imgproc.line( in.getLeft(), axisPoints.get(0), @@ -172,6 +179,7 @@ protected Void process(Pair> in) { ColorHelper.colorToScalar(Color.RED), 3); + // box edges perpendicular to tag for (int i = 0; i < bottomPoints.size(); i++) { Imgproc.line( in.getLeft(), @@ -180,6 +188,7 @@ protected Void process(Pair> in) { ColorHelper.colorToScalar(Color.blue), 3); } + // box edges parallel to tag for (int i = 0; i < topPoints.size(); i++) { Imgproc.line( in.getLeft(), @@ -265,18 +274,6 @@ private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) { dst.fromArray(pointArray); } - private void divideMat2f(MatOfPoint2f src, MatOfPoint2f dst) { - var hull = src.toArray(); - var pointArray = new Point[hull.length]; - for (int i = 0; i < hull.length; i++) { - var hullAtI = hull[i]; - pointArray[i] = - new Point( - hullAtI.x / (double) params.divisor.value, hullAtI.y / (double) params.divisor.value); - } - dst.fromArray(pointArray); - } - /** Scale a given point list by the current frame divisor. the point list is mutated! */ private void dividePointList(List points) { for (var p : points) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java index db773f6375..6a887d3a18 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java @@ -18,7 +18,6 @@ package org.photonvision.vision.pipe.impl; import edu.wpi.first.math.VecBuilder; -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; @@ -27,7 +26,6 @@ import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Scalar; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; @@ -99,33 +97,12 @@ private void calculateTargetPose(TrackedTarget target) { VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]), Core.norm(rVec)); - Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation)); - target.setBestCameraToTarget3d( - new Transform3d(targetPose.getTranslation(), targetPose.getRotation())); + Transform3d camToTarget = + MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation)); + target.setBestCameraToTarget3d(camToTarget); target.setAltCameraToTarget3d(new Transform3d()); } - Mat rotationMatrix = new Mat(); - Mat inverseRotationMatrix = new Mat(); - Mat pzeroWorld = new Mat(); - Mat kMat = new Mat(); - Mat scaledTvec; - - /** - * Element-wise scale a matrix by a given factor - * - * @param src the source matrix - * @param factor by how much to scale each element - * @return the scaled matrix - */ - @SuppressWarnings("SameParameterValue") - private static Mat matScale(Mat src, double factor) { - Mat dst = new Mat(src.rows(), src.cols(), src.type()); - Scalar s = new Scalar(factor); - Core.multiply(src, s, dst); - return dst; - } - public static class SolvePNPPipeParams { private final CameraCalibrationCoefficients cameraCoefficients; private final TargetModel targetModel; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index af1c5145d4..c7ffe1a83c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -146,8 +146,10 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting new TargetCalculationParameters( false, null, null, null, null, frameStaticProperties)); - var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d()); - var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d()); + var correctedBestPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d()); + var correctedAltPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d()); target.setBestCameraToTarget3d( new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation())); diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java index ea06638af8..198cbd5139 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -25,37 +25,50 @@ import org.opencv.core.MatOfPoint3f; import org.opencv.core.Point3; import org.photonvision.vision.opencv.Releasable; - +import org.photonvision.vision.pipe.impl.CornerDetectionPipe; +import org.photonvision.vision.pipe.impl.SolvePNPPipe; + +/** + * A model representing the vertices of targets with known shapes. The vertices are in the EDN + * coordinate system. When creating a TargetModel, the vertices must be supplied in a certain order + * to ensure correct correspondence with corners detected in 2D for use with SolvePNP. For planar + * targets, we expect the target's Z-axis to point towards the camera. + * + *

{@link SolvePNPPipe} expects 3d object points to correspond to the {@link CornerDetectionPipe} + * implementation. The 2d corner detection finds the 4 extreme corners (bottom-left, bottom-right, + * top-right, top-left). To match our expectations, this means the model vertices would look like: + * + *

    + *
  • (+x, +y, 0) + *
  • (-x, +y, 0) + *
  • (-x, -y, 0) + *
  • (+x, -y, 0) + *
+ * + *

AprilTag models are currently only used for drawing on the output stream. + */ public enum TargetModel implements Releasable { - k2020HighGoalOuter( - List.of( - new Point3(Units.inchesToMeters(-19.625), 0, 0), - new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(-17), 0), - new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(-17), 0), - new Point3(Units.inchesToMeters(19.625), 0, 0)), - Units.inchesToMeters(12)), - k2020HighGoalInner( + k2016HighGoal( List.of( - new Point3(Units.inchesToMeters(-19.625), 0, Units.inchesToMeters(2d * 12d + 5.25)), - new Point3( - Units.inchesToMeters(-9.819867), - Units.inchesToMeters(-17), - Units.inchesToMeters(2d * 12d + 5.25)), - new Point3( - Units.inchesToMeters(9.819867), - Units.inchesToMeters(-17), - Units.inchesToMeters(2d * 12d + 5.25)), - new Point3(Units.inchesToMeters(19.625), 0, Units.inchesToMeters(2d * 12d + 5.25))), - Units.inchesToMeters(12)), - + new Point3(Units.inchesToMeters(10), Units.inchesToMeters(6), 0), + new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(6), 0), + new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(-6), 0), + new Point3(Units.inchesToMeters(10), Units.inchesToMeters(-6), 0)), + Units.inchesToMeters(6)), k2019DualTarget( List.of( - new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0), - new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(-2.662), 0), - new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0), - new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0)), + new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(2.662), 0), + new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(2.662), 0), + new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(-2.662), 0), + new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(-2.662), 0)), 0.1), - + k2020HighGoalOuter( + List.of( + new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(8.5), 0), + new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(8.5), 0), + new Point3(Units.inchesToMeters(-19.625), Units.inchesToMeters(-8.5), 0), + new Point3(Units.inchesToMeters(19.625), Units.inchesToMeters(-8.5), 0)), + Units.inchesToMeters(12)), kCircularPowerCell7in( List.of( new Point3( @@ -94,36 +107,26 @@ public enum TargetModel implements Releasable { -Units.inchesToMeters(9.5) / 2, -Units.inchesToMeters(9.5) / 2)), 0), - k2016HighGoal( - List.of( - new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(12), 0), - new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(0), 0), - new Point3(Units.inchesToMeters(10), Units.inchesToMeters(0), 0), - new Point3(Units.inchesToMeters(10), Units.inchesToMeters(12), 0)), - Units.inchesToMeters(6)), - k200mmAprilTag( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + k200mmAprilTag( // Corners of the tag's inner black square (excluding white border) List.of( - new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0), new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0), - new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0), - new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)), + new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0), + new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0), + new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)), Units.inchesToMeters(3.25 * 2)), - kAruco6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + kAruco6in_16h5( // Corners of the tag's inner black square (excluding white border) List.of( new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0), new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), Units.inchesToMeters(3 * 2)), - k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + k6in_16h5( // Corners of the tag's inner black square (excluding white border) List.of( - new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0), new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0), - new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), - new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), + new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0), + new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), + new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), Units.inchesToMeters(3 * 2)); @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates; diff --git a/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java new file mode 100644 index 0000000000..a452f517f5 --- /dev/null +++ b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java @@ -0,0 +1,79 @@ +/* + * 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.common.util; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.photonvision.common.util.math.MathUtils; + +public class CoordinateConversionTest { + @BeforeAll + public static void Init() { + TestUtils.loadLibraries(); + } + + @Test + public void testAprilTagToOpenCV() { + // AprilTag and OpenCV both use the EDN coordinate system. AprilTag, however, assumes the tag's + // z-axis points away from the camera while we expect it to point towards the camera. + var apriltag = + new Transform3d( + new Translation3d(1, 2, 3), + new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15))); + var opencv = MathUtils.convertApriltagtoOpenCV(apriltag); + final var expectedTrl = new Translation3d(1, 2, 3); + assertEquals( + expectedTrl, opencv.getTranslation(), "AprilTag to OpenCV translation conversion failed"); + var apriltagXaxis = new Translation3d(1, 0, 0).rotateBy(apriltag.getRotation()); + var apriltagYaxis = new Translation3d(0, 1, 0).rotateBy(apriltag.getRotation()); + var apriltagZaxis = new Translation3d(0, 0, 1).rotateBy(apriltag.getRotation()); + var opencvXaxis = new Translation3d(1, 0, 0).rotateBy(opencv.getRotation()); + var opencvYaxis = new Translation3d(0, 1, 0).rotateBy(opencv.getRotation()); + var opencvZaxis = new Translation3d(0, 0, 1).rotateBy(opencv.getRotation()); + assertEquals( + apriltagXaxis.unaryMinus(), + opencvXaxis, + "AprilTag to OpenCV rotation conversion failed(X-axis)"); + assertEquals( + apriltagYaxis, opencvYaxis, "AprilTag to OpenCV rotation conversion failed(Y-axis)"); + assertEquals( + apriltagZaxis.unaryMinus(), + opencvZaxis, + "AprilTag to OpenCV rotation conversion failed(Z-axis)"); + } + + @Test + public void testOpenCVToPhoton() { + // OpenCV uses the EDN coordinate system while wpilib is in NWU. + var opencv = + new Transform3d( + new Translation3d(1, 2, 3), new Rotation3d(VecBuilder.fill(1, 2, 3), Math.PI / 8)); + var wpilib = MathUtils.convertOpenCVtoPhotonTransform(opencv); + final var expectedTrl = new Translation3d(3, -1, -2); + assertEquals( + expectedTrl, wpilib.getTranslation(), "OpenCV to WPILib translation conversion failed"); + var expectedRot = new Rotation3d(VecBuilder.fill(3, -1, -2), Math.PI / 8); + assertEquals(expectedRot, wpilib.getRotation(), "OpenCV to WPILib rotation conversion failed"); + } +} diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java index 1ece69fc55..fe1b7b9d9c 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java @@ -75,20 +75,19 @@ public void testApriltagFacingCamera() { // these numbers are not *accurate*, but they are known and expected var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2); - Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2); + Assertions.assertEquals(0.1, pose.getTranslation().getY(), 0.2); Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2); - var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(); - var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(); - var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(); + // We expect the object axes to be in NWU, with the x-axis coming out of the tag + // This visible tag is facing the camera almost parallel, so in world space: - // We expect the object X to be forward, or -X in world space + // The object's X axis should be (-1, 0, 0) Assertions.assertEquals( -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1); - // We expect the object Y axis to be right, or negative-Y in world space + // The object's Y axis should be (0, -1, 0) Assertions.assertEquals( -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1); - // We expect the object Z axis to be up, or +Z in world space + // The object's Z axis should be (0, 0, 1) Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1); } diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 02852308b9..03bcc2535d 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -19,11 +19,12 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import java.util.stream.Collectors; -import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.photonvision.common.util.TestUtils; @@ -115,28 +116,31 @@ public void test2019() { pipeline.getSettings().hueInverted); frameProvider.requestHsvSettings(hsvParams); - CVPipelineResult pipelineResult; - - pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); + CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera); printTestResults(pipelineResult); - // these numbers are not *accurate*, but they are known and expected - var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); - Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05); - Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05); - - // We expect the object X to be forward, or -X in world space - Assertions.assertEquals( - -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.05); - // We expect the object Y axis to be right, or negative-Y in world space - Assertions.assertEquals( - -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.05); - // We expect the object Z axis to be up, or +Z in world space - Assertions.assertEquals( - 1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.05); + // Draw on input + var outputPipe = new OutputStreamPipeline(); + outputPipe.process( + pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets); TestUtils.showImage( - pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999); + pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999); + + var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); + // these numbers are not *accurate*, but they are known and expected + var expectedTrl = new Translation3d(1.1, -0.05, -0.05); + assertTrue( + expectedTrl.getDistance(pose.getTranslation()) < 0.05, + "SolvePNP translation estimation failed"); + // We expect the object axes to be in NWU, with the x-axis coming out of the tag + // This target is facing the camera almost parallel, so in world space: + // The object's X axis should be (-1, 0, 0) + assertEquals(-1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.05); + // The object's Y axis should be (0, -1, 0) + assertEquals(-1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.05); + // The object's Z axis should be (0, 0, 1) + assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.05); } @Test @@ -175,15 +179,27 @@ public void test2020() { outputPipe.process( pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets); - // these numbers are not *accurate*, but they are known and expected - var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); - Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05); - Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05); - // Z rotation should be mostly facing us - Assertions.assertEquals(Units.degreesToRadians(-140), pose.getRotation().getZ(), 1); - TestUtils.showImage( - pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999); + pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999); + + var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); + // these numbers are not *accurate*, but they are known and expected + var expectedTrl = + new Translation3d( + Units.inchesToMeters(236), Units.inchesToMeters(36), Units.inchesToMeters(-53)); + assertTrue( + expectedTrl.getDistance(pose.getTranslation()) < 0.05, + "SolvePNP translation estimation failed"); + // We expect the object axes to be in NWU, with the x-axis coming out of the tag + // Rotation around Z axis (yaw) should be mostly facing us + var xAxis = new Translation3d(1, 0, 0); + var yAxis = new Translation3d(0, 1, 0); + var zAxis = new Translation3d(0, 0, 1); + var expectedRot = + new Rotation3d(Math.toRadians(-20), Math.toRadians(-20), Math.toRadians(-120)); + assertTrue(xAxis.rotateBy(expectedRot).getDistance(xAxis.rotateBy(pose.getRotation())) < 0.1); + assertTrue(yAxis.rotateBy(expectedRot).getDistance(yAxis.rotateBy(pose.getRotation())) < 0.1); + assertTrue(zAxis.rotateBy(expectedRot).getDistance(zAxis.rotateBy(pose.getRotation())) < 0.1); } private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { From 67d8680a329bfa3fc031331bf376b97ce3dd4b0c Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 15 Oct 2023 15:11:23 -0400 Subject: [PATCH 04/10] Remove empty tab groups in UI (#948) * Remove empty tab groups * Chain list comprehension * Further condense * add comment --------- Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> --- .../components/dashboard/ConfigOptions.vue | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/photon-client/src/components/dashboard/ConfigOptions.vue b/photon-client/src/components/dashboard/ConfigOptions.vue index 33a17ebe14..6ed0156f9f 100644 --- a/photon-client/src/components/dashboard/ConfigOptions.vue +++ b/photon-client/src/components/dashboard/ConfigOptions.vue @@ -104,17 +104,19 @@ const tabGroups = computed(() => { const isAprilTag = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.AprilTag; const isAruco = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.Aruco; - return getTabGroups().map((tabGroup) => - tabGroup.filter( - (tabConfig) => - !(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated - !((!allow3d || isAprilTag || isAruco) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags - !((isAprilTag || isAruco) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags - !((isAprilTag || isAruco) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags - !(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags - !(!isAruco && tabConfig.tabName === "Aruco") //Filter out aruco unless we actually are doing Aruco + return getTabGroups() + .map((tabGroup) => + tabGroup.filter( + (tabConfig) => + !(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated + !((!allow3d || isAprilTag || isAruco) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags + !((isAprilTag || isAruco) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags + !((isAprilTag || isAruco) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags + !(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags + !(!isAruco && tabConfig.tabName === "Aruco") //Filter out aruco unless we actually are doing Aruco + ) ) - ); + .filter((it) => it.length); // Remove empty tab groups }); onBeforeUpdate(() => { From cd83e220d7d3ffeffe9ac5f9250be48eda4d4252 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 16 Oct 2023 07:56:14 -0400 Subject: [PATCH 05/10] [NFC] Reorgranize CI tasks into their own files (#953) Closes #950 --- .github/workflows/{main.yml => build.yml} | 299 ++++++---------------- .github/workflows/lint-format.yml | 88 +++++++ 2 files changed, 164 insertions(+), 223 deletions(-) rename .github/workflows/{main.yml => build.yml} (57%) create mode 100644 .github/workflows/lint-format.yml diff --git a/.github/workflows/main.yml b/.github/workflows/build.yml similarity index 57% rename from .github/workflows/main.yml rename to .github/workflows/build.yml index 94a5e5f075..9a72b229b2 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/build.yml @@ -1,9 +1,5 @@ -# This workflow builds the client (UI), the server, builds the JAR. +name: Build -name: CI - -# Controls when the action will run. Triggers the workflow on push or pull request -# events but only for the master branch on: push: branches: [ master ] @@ -11,212 +7,117 @@ on: - 'v*' pull_request: branches: [ master ] - merge_group: jobs: - photonclient-build: - # Let all steps run within the photon-client dir. + build-client: + name: "PhotonClient Build" defaults: run: working-directory: photon-client - - # The type of runner that the job will run on. - runs-on: ubuntu-22.04 - - steps: - # Checkout code. - - uses: actions/checkout@v3 - - # Setup Node.js - - name: Setup Node.js - uses: actions/setup-node@v3 - with: - node-version: 17 - - # Run npm - - name: Install Dependencies - run: npm ci - - - name: Build Production Client - run: npm run build - - # Upload client artifact. - - uses: actions/upload-artifact@master - with: - name: built-client - path: photon-client/dist/ - photonclient-check-lint: - # Let all steps run within the photon-client dir. - defaults: - run: - working-directory: photon-client - - # The type of runner that the job will run on. runs-on: ubuntu-22.04 - steps: - # Checkout code. - - uses: actions/checkout@v3 - - # Setup Node.js - - name: Setup Node.js - uses: actions/setup-node@v3 - with: - node-version: 17 - - - name: Install Dependencies - run: npm ci - - - name: Check Linting - run: npm run lint-ci - - - name: Check Formatting - run: npm run format-ci - photon-build-examples: - runs-on: ubuntu-22.04 + - uses: actions/checkout@v3 + - name: Setup Node.js + uses: actions/setup-node@v3 + with: + node-version: 18 + - name: Install Dependencies + run: npm ci + - name: Build Production Client + run: npm run build + - uses: actions/upload-artifact@master + with: + name: built-client + path: photon-client/dist/ + build-examples: name: "Build Examples" - + runs-on: ubuntu-22.04 steps: - # Checkout code. - name: Checkout code uses: actions/checkout@v3 with: fetch-depth: 0 - - # Fetch tags. - name: Fetch tags run: git fetch --tags --force - - # Install Java 17. - name: Install Java 17 uses: actions/setup-java@v3 with: java-version: 17 distribution: temurin - # Need to publish to maven local first, so that C++ sim can pick it up - # Still haven't figure out how to make the vendordep file be copied before trying to build examples + # Still haven't figured out how to make the vendordep file be copied before trying to build examples - name: Publish photonlib to maven local run: | chmod +x gradlew ./gradlew publishtomavenlocal -x check - - name: Build Java examples working-directory: photonlib-java-examples run: | chmod +x gradlew ./gradlew copyPhotonlib -x check ./gradlew build -x check --max-workers 2 - - name: Build C++ examples working-directory: photonlib-cpp-examples run: | chmod +x gradlew ./gradlew copyPhotonlib -x check ./gradlew build -x check --max-workers 2 - photon-build-all: - # The type of runner that the job will run on. + build-gradle: + name: "Gradle Build" runs-on: ubuntu-22.04 - steps: # Checkout code. - name: Checkout code uses: actions/checkout@v3 with: fetch-depth: 0 - - # Fetch tags. - name: Fetch tags run: git fetch --tags --force - - # Install Java 17. - name: Install Java 17 uses: actions/setup-java@v3 with: java-version: 17 distribution: temurin - - # Run only build tasks, no checks?? - name: Gradle Build run: | chmod +x gradlew ./gradlew photon-server:build photon-lib:build -x check --max-workers 2 - - # Run Gradle Tests. - name: Gradle Tests run: ./gradlew testHeadless -i --max-workers 1 --stacktrace - - # Generate Coverage Report. - name: Gradle Coverage run: ./gradlew jacocoTestReport --max-workers 1 - - # Publish Coverage Report. - - name: Publish Server Coverage Report + - name: Publish Coverage Report uses: codecov/codecov-action@v3 with: file: ./photon-server/build/reports/jacoco/test/jacocoTestReport.xml - - name: Publish Core Coverage Report uses: codecov/codecov-action@v3 with: file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml - photonserver-build-offline-docs: + build-offline-docs: + name: "Build Offline Docs" runs-on: ubuntu-22.04 - steps: - # Checkout docs. - uses: actions/checkout@v3 with: repository: 'PhotonVision/photonvision-docs.git' ref: master - - # Install Python. - uses: actions/setup-python@v4 with: python-version: '3.9' - - name: Install dependencies run: | python -m pip install --upgrade pip pip install sphinx sphinx_rtd_theme sphinx-tabs sphinxext-opengraph doc8 pip install -r requirements.txt - - # Don't check the docs. If a PR was merged to the docs repo, it ought to pass CI. No need to re-check here. - # - name: Check the docs - # run: | - # make linkcheck - # make lint - - name: Build the docs run: | make html - - # Upload docs artifact. - uses: actions/upload-artifact@master with: name: built-docs path: build/html - photonserver-check-lint: - # The type of runner that the job will run on. - runs-on: ubuntu-22.04 - - steps: - # Checkout code. - - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - # Install Java 17. - - uses: actions/setup-java@v3 - with: - java-version: 17 - distribution: temurin - - # Check server code with Spotless. - - run: | - chmod +x gradlew - ./gradlew spotlessCheck - photonlib-build-host: + build-photonlib-host: env: MACOSX_DEPLOYMENT_TARGET: 10.14 strategy: @@ -232,13 +133,14 @@ jobs: - os: ubuntu-22.04 artifact-name: Linux - runs-on: ${{ matrix.os }} name: "Photonlib - Build Host - ${{ matrix.artifact-name }}" + runs-on: ${{ matrix.os }} steps: - uses: actions/checkout@v3 with: fetch-depth: 0 - - uses: actions/setup-java@v3 + - name: Install Java 17 + uses: actions/setup-java@v3 with: java-version: 17 distribution: temurin @@ -251,7 +153,7 @@ jobs: env: ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} if: github.event_name == 'push' - photonlib-build-docker: + build-photonlib-docker: strategy: fail-fast: false matrix: @@ -284,85 +186,50 @@ jobs: env: ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }} if: github.event_name == 'push' - photonlib-wpiformat: - name: "wpiformat" - runs-on: ubuntu-22.04 - steps: - - uses: actions/checkout@v3 - - name: Fetch all history and metadata - run: | - git fetch --prune --unshallow - git checkout -b pr - git branch -f master origin/master - - name: Set up Python 3.8 - uses: actions/setup-python@v4 - with: - python-version: 3.8 - - name: Install wpiformat - run: pip3 install wpiformat - - name: Check index.html not changed - run: git --no-pager diff --exit-code origin/master photon-server/src/main/resources/web/index.html - - name: Run - run: wpiformat - - name: Check output - run: git --no-pager diff --exit-code HEAD - - name: Generate diff - run: git diff HEAD > wpiformat-fixes.patch - if: ${{ failure() }} - - uses: actions/upload-artifact@v3 - with: - name: wpiformat fixes - path: wpiformat-fixes.patch - if: ${{ failure() }} - photon-build-package: - needs: [photonclient-build, photon-build-all, photonserver-build-offline-docs] + build-package: + needs: [build-client, build-gradle, build-offline-docs] strategy: - fail-fast: false - matrix: - include: - - os: windows-latest - artifact-name: Win64 - architecture: x64 - arch-override: none - - os: macos-latest - artifact-name: macOS - architecture: x64 - arch-override: none - - os: ubuntu-latest - artifact-name: Linux - architecture: x64 - arch-override: none - - os: macos-latest - artifact-name: macOSArm - architecture: x64 - arch-override: macarm64 - - os: ubuntu-latest - artifact-name: LinuxArm32 - architecture: x64 - arch-override: linuxarm32 - - os: ubuntu-latest - artifact-name: LinuxArm64 - architecture: x64 - arch-override: linuxarm64 + fail-fast: false + matrix: + include: + - os: windows-latest + artifact-name: Win64 + architecture: x64 + arch-override: none + - os: macos-latest + artifact-name: macOS + architecture: x64 + arch-override: none + - os: ubuntu-latest + artifact-name: Linux + architecture: x64 + arch-override: none + - os: macos-latest + artifact-name: macOSArm + architecture: x64 + arch-override: macarm64 + - os: ubuntu-latest + artifact-name: LinuxArm32 + architecture: x64 + arch-override: linuxarm32 + - os: ubuntu-latest + artifact-name: LinuxArm64 + architecture: x64 + arch-override: linuxarm64 - # The type of runner that the job will run on. runs-on: ${{ matrix.os }} name: "Build fat JAR - ${{ matrix.artifact-name }}" steps: - # Checkout code. - uses: actions/checkout@v3 with: fetch-depth: 0 - - # Install Java 17. - - uses: actions/setup-java@v3 + - name: Install Java 17 + uses: actions/setup-java@v3 with: java-version: 17 distribution: temurin - - # Clear any existing web resources. - run: | rm -rf photon-server/src/main/resources/web/* mkdir -p photon-server/src/main/resources/web/docs @@ -371,20 +238,14 @@ jobs: del photon-server\src\main\resources\web\*.* mkdir photon-server\src\main\resources\web\docs if: ${{ (matrix.os) == 'windows-latest' }} - - # Download client artifact to resources folder. - uses: actions/download-artifact@v3 with: name: built-client path: photon-server/src/main/resources/web/ - - # Download docs artifact to resources folder. - uses: actions/download-artifact@v3 with: name: built-docs path: photon-server/src/main/resources/web/docs - - # Build fat jar for both pi and everything - run: | chmod +x gradlew ./gradlew photon-server:shadowJar --max-workers 2 -PArchOverride=${{ matrix.arch-override }} @@ -393,62 +254,55 @@ jobs: chmod +x gradlew ./gradlew photon-server:shadowJar --max-workers 2 if: ${{ (matrix.arch-override == 'none') }} - - # Upload final fat jar as artifact. - uses: actions/upload-artifact@v3 with: name: jar-${{ matrix.artifact-name }} path: photon-server/build/libs - photon-image-generator: - needs: [photon-build-package] + build-image: + needs: [build-package] + if: ${{ github.event_name != 'pull_request' }} strategy: - fail-fast: false - matrix: - include: - - os: ubuntu-latest - artifact-name: LinuxArm64 - image_suffix: RaspberryPi - image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.3_arm64 - - os: ubuntu-latest - artifact-name: LinuxArm64 - image_suffix: limelight2 - image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.2.2_limelight-arm64 + fail-fast: false + matrix: + include: + - os: ubuntu-latest + artifact-name: LinuxArm64 + image_suffix: RaspberryPi + image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.3_arm64 + - os: ubuntu-latest + artifact-name: LinuxArm64 + image_suffix: limelight2 + image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.2.2_limelight-arm64 runs-on: ${{ matrix.os }} name: "Build image - ${{ matrix.image_url }}" steps: - # Checkout code. - name: Checkout code uses: actions/checkout@v3 with: fetch-depth: 0 - - uses: actions/download-artifact@v2 with: name: jar-${{ matrix.artifact-name }} - - name: Generate image run: | chmod +x scripts/generatePiImage.sh ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }} - - uses: actions/upload-artifact@v3 name: Upload image with: name: image-${{ matrix.image_suffix }} path: photonvision*.xz - photon-release: - needs: [photon-build-package, photon-image-generator] + release: + needs: [build-package, build-image] runs-on: ubuntu-22.04 steps: # Download literally every single artifact. This also downloads client and docs, # but the filtering below won't pick these up (I hope) - uses: actions/download-artifact@v2 - - run: find - # Push to dev release - uses: pyTooling/Actions/releaser@r0 with: @@ -459,7 +313,6 @@ jobs: **/*.xz **/*.jar if: github.event_name == 'push' - # Upload all jars and xz archives - uses: softprops/action-gh-release@v1 with: diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml new file mode 100644 index 0000000000..0fd220a02e --- /dev/null +++ b/.github/workflows/lint-format.yml @@ -0,0 +1,88 @@ +name: Lint and Format + +on: + push: + branches: [ master ] + tags: + - 'v*' + pull_request: + branches: [ master ] + +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} + cancel-in-progress: true + +jobs: + wpiformat: + name: "wpiformat" + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - name: Fetch all history and metadata + run: | + git fetch --prune --unshallow + git checkout -b pr + git branch -f master origin/master + - name: Set up Python 3.8 + uses: actions/setup-python@v4 + with: + python-version: 3.8 + - name: Install wpiformat + run: pip3 install wpiformat + - name: Run + run: wpiformat + - name: Check output + run: git --no-pager diff --exit-code HEAD + - name: Generate diff + run: git diff HEAD > wpiformat-fixes.patch + if: ${{ failure() }} + - uses: actions/upload-artifact@v3 + with: + name: wpiformat fixes + path: wpiformat-fixes.patch + if: ${{ failure() }} + javaformat: + name: "Java Formatting" + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + - uses: actions/setup-java@v3 + with: + java-version: 17 + distribution: temurin + - run: | + chmod +x gradlew + ./gradlew spotlessCheck + + client-lint-format: + name: "PhotonClient Lint and Formatting" + defaults: + run: + working-directory: photon-client + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - name: Setup Node.js + uses: actions/setup-node@v3 + with: + node-version: 18 + - name: Install Dependencies + run: npm ci + - name: Check Linting + run: npm run lint-ci + - name: Check Formatting + run: npm run format-ci + server-index: + name: "Check server index.html not changed" + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - name: Fetch all history and metadata + run: | + git fetch --prune --unshallow + git checkout -b pr + git branch -f master origin/master + - name: Check index.html not changed + run: git --no-pager diff --exit-code origin/master photon-server/src/main/resources/web/index.html From 1aa6bc80c9b2ebd2c77986dd8c2d0726fa449890 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 17 Oct 2023 09:02:13 -0400 Subject: [PATCH 06/10] File upload robustness (#956) Fixes issues uploading multiple files manually --- .../components/settings/DeviceControlCard.vue | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/photon-client/src/components/settings/DeviceControlCard.vue b/photon-client/src/components/settings/DeviceControlCard.vue index 29ae2e92d0..b7dfeff249 100644 --- a/photon-client/src/components/settings/DeviceControlCard.vue +++ b/photon-client/src/components/settings/DeviceControlCard.vue @@ -63,19 +63,18 @@ const offlineUpdate = ref(); const openOfflineUpdatePrompt = () => { offlineUpdate.value.click(); }; -const handleOfflineUpdate = (payload: Event) => { +const handleOfflineUpdate = (payload: Event & { target: (EventTarget & HTMLInputElement) | null }) => { + if (payload.target === null || !payload.target.files) return; + + const formData = new FormData(); + formData.append("jarData", payload.target.files[0]); + useStateStore().showSnackbarMessage({ message: "New Software Upload in Progress...", color: "secondary", timeout: -1 }); - const formData = new FormData(); - - if (payload.target == null || !payload.target?.files) return; - const files: FileList = payload.target.files as FileList; - formData.append("jarData", files[0]); - axios .post("/utils/offlineUpdate", formData, { headers: { "Content-Type": "multipart/form-data" }, @@ -138,21 +137,17 @@ enum ImportType { HardwareSettings, NetworkConfig } - const showImportDialog = ref(false); const importType = ref(-1); -const importFile = ref(null); +const importFile = ref(null); const handleSettingsImport = () => { if (importType.value === -1 || importFile.value === null) return; const formData = new FormData(); formData.append("data", importFile.value); - let settingsEndpoint; + let settingsEndpoint: string; switch (importType.value) { - case ImportType.AllSettings: - settingsEndpoint = ""; - break; case ImportType.HardwareConfig: settingsEndpoint = "/hardwareConfig"; break; @@ -162,6 +157,10 @@ const handleSettingsImport = () => { case ImportType.NetworkConfig: settingsEndpoint = "/networkConfig"; break; + default: + case ImportType.AllSettings: + settingsEndpoint = ""; + break; } axios @@ -245,21 +244,22 @@ const handleSettingsImport = () => { Import Settings Upload and apply previously saved or exported PhotonVision settings to this device - + Date: Tue, 17 Oct 2023 09:08:25 -0400 Subject: [PATCH 07/10] Take Snapshots from PhotonClient (#940) --- .../components/app/photon-camera-stream.vue | 74 +++++++++++++--- .../cameras/CameraCalibrationCard.vue | 2 +- .../src/components/cameras/CamerasView.vue | 6 +- .../stores/settings/CameraSettingsStore.ts | 29 ++++++- .../frame/consumer/FileSaveFrameConsumer.java | 86 ++++++++++--------- .../vision/processes/VisionModule.java | 8 ++ .../VisionModuleChangeSubscriber.java | 6 ++ .../server/DataSocketHandler.java | 24 ++++++ .../server/DataSocketMessageType.java | 2 + 9 files changed, 177 insertions(+), 60 deletions(-) diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 2868ae7180..1349da2f9d 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -4,13 +4,14 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore"; import { useStateStore } from "@/stores/StateStore"; import loadingImage from "@/assets/images/loading.svg"; import type { StyleValue } from "vue/types/jsx"; +import CvIcon from "@/components/common/cv-icon.vue"; const props = defineProps<{ streamType: "Raw" | "Processed"; id?: string; }>(); -const src = computed(() => { +const streamSrc = computed(() => { const port = useCameraSettingsStore().currentCameraSettings.stream[props.streamType === "Raw" ? "inputPort" : "outputPort"]; @@ -20,25 +21,74 @@ const src = computed(() => { return `http://${inject("backendHostname")}:${port}/stream.mjpg`; }); -const alt = computed(() => `${props.streamType} Stream View`); - -const style = computed(() => { +const streamDesc = computed(() => `${props.streamType} Stream View`); +const streamStyle = computed(() => { if (useStateStore().colorPickingMode) { - return { cursor: "crosshair" }; - } else if (src.value !== loadingImage) { - return { cursor: "pointer" }; + return { width: "100%", cursor: "crosshair" }; + } else if (streamSrc.value !== loadingImage) { + return { width: "100%", cursor: "pointer" }; } - return {}; + return { width: "100%" }; }); -const handleClick = () => { - if (!useStateStore().colorPickingMode && src.value !== loadingImage) { - window.open(src.value); +const overlayStyle = computed(() => { + if (useStateStore().colorPickingMode || streamSrc.value == loadingImage) { + return { display: "none" }; + } else { + return {}; + } +}); + +const handleStreamClick = () => { + if (!useStateStore().colorPickingMode && streamSrc.value !== loadingImage) { + window.open(streamSrc.value); + } +}; +const handleCaptureClick = () => { + if (props.streamType === "Raw") { + useCameraSettingsStore().saveInputSnapshot(); + } else { + useCameraSettingsStore().saveOutputSnapshot(); } }; + + diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue index fb12224656..969dffa86b 100644 --- a/photon-client/src/components/cameras/CameraCalibrationCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue @@ -384,7 +384,7 @@ const endCalibration = () => { color="secondary" style="width: 100%" :disabled="!settingsValid" - @click="isCalibrating ? useCameraSettingsStore().takeCalibrationSnapshot(true) : startCalibration()" + @click="isCalibrating ? useCameraSettingsStore().takeCalibrationSnapshot() : startCalibration()" > {{ isCalibrating ? "Take Snapshot" : "Start Calibration" }} diff --git a/photon-client/src/components/cameras/CamerasView.vue b/photon-client/src/components/cameras/CamerasView.vue index 8b7f1785dc..c9e3d32cb9 100644 --- a/photon-client/src/components/cameras/CamerasView.vue +++ b/photon-client/src/components/cameras/CamerasView.vue @@ -41,7 +41,7 @@ const fpsTooLow = computed(() => {