format

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<Point> 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<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
         var centroid = target.getMinAreaRect().center;
         Comparator<Point> compareCenterDist =
-                Comparator.comparingDouble(
-                        (Point point) -> distanceBetween(centroid, point));
+                Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point));
 
         MatOfPoint2f targetContour;
         if (convexHull) {

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.
- * 
- * <p>{@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.
+ *
+ * <p>{@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:
+ *
  * <ul>
  *   <li>(+x, +y, 0)
  *   <li>(-x, +y, 0)
  *   <li>(-x, -y, 0)
  *   <li>(+x, -y, 0)
  * </ul>
- * 
+ *
  * <p>AprilTag models are currently only used for drawing on the output stream.
  */
 public enum TargetModel implements Releasable {

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 <https://www.gnu.org/licenses/>.
+ */
+
 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");
     }
 }

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);