From 0244febc37a989eeda553c0f40e0c524cd04a97a Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 20 Aug 2023 19:01:29 -0700 Subject: [PATCH] format --- .../vision/pipe/impl/CornerDetectionPipe.java | 9 ++- .../vision/target/TargetModel.java | 19 +++--- .../common/util/CoordinateConversionTest.java | 58 ++++++++++++++----- .../vision/pipeline/SolvePNPTest.java | 16 +++-- 4 files changed, 67 insertions(+), 35 deletions(-) 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 141dc4ed54..eb5427fa8d 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 @@ -28,8 +28,8 @@ import org.photonvision.vision.target.TrackedTarget; /** - * Determines the target corners of the {@link TrackedTarget}. The {@link CornerDetectionPipeParameters} - * affect how these corners are calculated. + * Determines the target corners of the {@link TrackedTarget}. The {@link + * CornerDetectionPipeParameters} affect how these corners are calculated. */ public class CornerDetectionPipe extends CVPipe< @@ -98,7 +98,7 @@ private List findBoundingBoxCorners(TrackedTarget target) { private static double distanceBetween(Point a, Point b) { double xDelta = a.x - b.x; double yDelta = a.y - b.y; - return Math.sqrt(xDelta*xDelta + yDelta*yDelta); + return Math.sqrt(xDelta * xDelta + yDelta * yDelta); } /** @@ -111,8 +111,7 @@ private static double distanceBetween(Point a, Point b) { private List detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) { var centroid = target.getMinAreaRect().center; Comparator compareCenterDist = - Comparator.comparingDouble( - (Point point) -> distanceBetween(centroid, point)); + Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point)); MatOfPoint2f targetContour; if (convexHull) { 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 5133030504..9a88ed056f 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,25 +25,26 @@ import org.opencv.core.MatOfPoint3f; import org.opencv.core.Point3; import org.photonvision.vision.opencv.Releasable; -import org.photonvision.vision.pipe.impl.SolvePNPPipe; 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: + * 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 { 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 index 20c9032a7b..a452f517f5 100644 --- a/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java +++ b/photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java @@ -1,15 +1,31 @@ +/* + * 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 org.junit.jupiter.api.BeforeAll; -import org.junit.jupiter.api.Test; -import org.photonvision.common.util.math.MathUtils; - 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 @@ -21,33 +37,43 @@ public static void Init() { 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 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"); + 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)"); + 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 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( + 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/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 58e879dfb7..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 @@ -25,7 +25,6 @@ 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; @@ -131,7 +130,9 @@ public void test2019() { 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"); + 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) @@ -183,14 +184,19 @@ public void test2020() { 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"); + 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)); + 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);