From e180832c9a111425bcf4b9cf243bdf0e5b70a0d6 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 4 Oct 2022 11:29:14 -0400
Subject: [PATCH 1/4] Fix AprilTag rotation reversal bug

---
 .../photonvision/common/util/TestUtils.java   |   8 +-
 .../common/util/math/MathUtils.java           |  38 ++++---
 .../vision/pipe/impl/Draw3dTargetsPipe.java   |  45 ++++++++
 .../vision/pipeline/AprilTagPipeline.java     |   2 +-
 .../vision/processes/PipelineManager.java     |   7 ++
 .../vision/target/TargetModel.java            |   2 +-
 .../vision/target/TrackedTarget.java          |   7 +-
 .../common/configuration/ConfigTest.java      |  13 +++
 .../vision/pipeline/SolvePNPTest.java         |   8 ++
 photon-server/build.gradle                    |   1 +
 .../vision/pipeline/AprilTagTest.java         | 104 ++++++++++++++++++
 .../testimages/apriltag/tag1_640_480.jpg      | Bin 0 -> 15576 bytes
 12 files changed, 212 insertions(+), 23 deletions(-)
 create mode 100644 photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
 create mode 100644 test-resources/testimages/apriltag/tag1_640_480.jpg

diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
index ba8c988c98..57310f0f47 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
+++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
@@ -33,7 +33,6 @@ public static void loadLibraries() {
         try {
             CameraServerCvJNI.forceLoad();
             //        PicamJNI.forceLoad();
-            // AprilTagJNI.forceLoad();
         } catch (IOException ex) {
             // ignored
         }
@@ -165,7 +164,8 @@ Path getPath() {
     }
 
     public enum ApriltagTestImages {
-        kRobots;
+        kRobots,
+        kTag1_640_480;
 
         public final Path path;
 
@@ -233,6 +233,10 @@ public static Path getPolygonImagePath(PolygonTestImages image, boolean testMode
         return getTestImagesPath(testMode).resolve(image.path);
     }
 
+    public static Path getApriltagImagePath(ApriltagTestImages image, boolean testMode) {
+        return getTestImagesPath(testMode).resolve(image.path);
+    }
+
     public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) {
         return getPowercellPath(testMode).resolve(image.path);
     }
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 759038a364..5ae94c42ea 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
@@ -19,14 +19,19 @@
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.CoordinateSystem;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
 import java.util.List;
+import org.opencv.core.Mat;
 
 public class MathUtils {
     MathUtils() {}
@@ -159,25 +164,26 @@ public static Pose3d EDNtoNWU(final Pose3d pose) {
     // TODO: Refactor into new pipe?
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
         // CameraToTarget _should_ be in opencv-land EDN
-
-        var pose =
-                CoordinateSystem.convert(
-                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
-
-        return pose;
+        return CoordinateSystem.convert(
+                new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
     }
 
-    public static Pose3d convertApriltagtoPhotonPose(Transform3d cameraToTarget3d) {
-        // CameraToTarget _should_ be in opencv-land EDN
-        var pose =
-                CoordinateSystem.convert(
-                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+    // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
+    // to a camera facing along the +X axis of the field parallel with the ground plane
+    // So we need a 180 flip about X axis
+    private static final Vector<N3> APRILTAG_AXIS = VecBuilder.fill(1, 0, 0);
+    private static final double APRILTAG_ANGLE = Units.degreesToRadians(180);
+    private static final Rotation3d APRILTAG_TRANSFORM =
+            new Rotation3d(APRILTAG_AXIS, APRILTAG_ANGLE);
 
-        // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
-        // to a camera facing along the +X axis of the field parallel with the ground plane
-        // So we need a 180 flip about X axis
-        var newRotation = pose.getRotation().rotateBy(new Rotation3d(0, Math.PI, 0));
+    public static Pose3d convertApriltagtoOpencv(Pose3d pose) {
+        var ocvRotation = APRILTAG_TRANSFORM.rotateBy(pose.getRotation());
+        return new Pose3d(pose.getTranslation(), ocvRotation);
+    }
 
-        return new Pose3d(pose.getTranslation(), newRotation);
+    public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
+        var angle = rotation.getAngle();
+        var axis = rotation.getAxis().times(angle);
+        rvecOutput.put(0, 0, axis.getData());
     }
 }
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 97c55e3d3c..0bf5c36636 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
@@ -25,7 +25,9 @@
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.MatOfPoint3f;
 import org.opencv.core.Point;
+import org.opencv.core.Point3;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
@@ -127,8 +129,51 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
                             ColorHelper.colorToScalar(Color.orange),
                             3);
                 }
+
+                // Draw X, Y and Z axis
+                MatOfPoint3f pointMat = new MatOfPoint3f();
+                var list =
+                        List.of(
+                                new Point3(0, 0, 0),
+                                new Point3(0.2, 0, 0),
+                                new Point3(0, 0.2, 0),
+                                new Point3(0, 0, 0.2));
+                pointMat.fromList(list);
+
+                Calib3d.projectPoints(
+                        pointMat,
+                        target.getCameraRelativeRvec(),
+                        target.getCameraRelativeTvec(),
+                        params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
+                        params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
+                        tempMat,
+                        jac);
+                var axisPoints = tempMat.toList();
+                dividePointList(axisPoints);
+
+                // Red = x, green y, blue z
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(1),
+                        ColorHelper.colorToScalar(Color.RED),
+                        3);
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(2),
+                        ColorHelper.colorToScalar(Color.GREEN),
+                        3);
+                Imgproc.line(
+                        in.getLeft(),
+                        axisPoints.get(0),
+                        axisPoints.get(3),
+                        ColorHelper.colorToScalar(Color.BLUE),
+                        3);
+
                 tempMat.release();
                 jac.release();
+                pointMat.release();
             }
 
             // draw corners
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 63ff35cb59..b6c0a0ceda 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
@@ -140,7 +140,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
                             new TargetCalculationParameters(
                                     false, null, null, null, null, frameStaticProperties));
 
-            var correctedPose = MathUtils.convertApriltagtoPhotonPose(target.getCameraToTarget3d());
+            var correctedPose = MathUtils.convertOpenCVtoPhotonPose(target.getCameraToTarget3d());
             target.setCameraToTarget3d(
                     new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));
 
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
index 973a4225f7..36a5129a3e 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
+++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
@@ -181,16 +181,23 @@ private void setPipelineInternal(int index) {
             var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
             switch (desiredPipelineSettings.pipelineType) {
                 case Reflective:
+                    logger.debug("Creatig Reflective pipeline");
                     currentUserPipeline =
                             new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
                     break;
                 case ColoredShape:
+                    logger.debug("Creatig ColoredShape pipeline");
                     currentUserPipeline =
                             new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
                     break;
                 case AprilTag:
+                    logger.debug("Creatig AprilTag pipeline");
                     currentUserPipeline =
                             new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
+                    break;
+                default:
+                    // Can be calib3d or drivermode, both of which are special cases
+                    break;
             }
         }
     }
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 fa7621ae89..cc8cfc216e 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
@@ -108,7 +108,7 @@ public enum TargetModel implements Releasable {
                     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));
+            Units.inchesToMeters(3.25 * 2));
 
     @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
     @JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();
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 87934b682d..baf82ddb0c 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
@@ -26,6 +26,7 @@
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.apriltag.DetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
@@ -79,6 +80,8 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params)
             bestPose = result.getPoseResult2();
         }
 
+        bestPose = MathUtils.convertApriltagtoOpencv(bestPose);
+
         m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
 
         double[] corners = result.getCorners();
@@ -113,9 +116,7 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params)
 
         // Opencv expects a 3d vector with norm = angle and direction = axis
         var rvec = new Mat(3, 1, CvType.CV_64FC1);
-        var angle = bestPose.getRotation().getAngle();
-        var axis = bestPose.getRotation().getAxis().times(angle);
-        rvec.put(0, 0, axis.getData());
+        MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
         setCameraRelativeRvec(rvec);
     }
 
diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java
index 909f53507e..e9a6e391d6 100644
--- a/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java
+++ b/photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java
@@ -30,6 +30,7 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.file.JacksonUtils;
+import org.photonvision.vision.pipeline.AprilTagPipelineSettings;
 import org.photonvision.vision.pipeline.ColoredShapePipelineSettings;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.target.TargetModel;
@@ -40,6 +41,7 @@ public class ConfigTest {
             new CameraConfiguration("TestCamera", "/dev/video420");
     private static ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS;
     private static ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS;
+    private static AprilTagPipelineSettings APRIL_TAG_PIPELINE_SETTINGS;
 
     @BeforeAll
     public static void init() {
@@ -51,6 +53,7 @@ public static void init() {
 
         REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings();
         COLORED_SHAPE_PIPELINE_SETTINGS = new ColoredShapePipelineSettings();
+        APRIL_TAG_PIPELINE_SETTINGS = new AprilTagPipelineSettings();
 
         REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape";
         REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.k2019DualTarget;
@@ -58,8 +61,12 @@ public static void init() {
         COLORED_SHAPE_PIPELINE_SETTINGS.pipelineNickname = "2019Cargo";
         COLORED_SHAPE_PIPELINE_SETTINGS.pipelineIndex = 1;
 
+        APRIL_TAG_PIPELINE_SETTINGS.pipelineNickname = "apriltag";
+        APRIL_TAG_PIPELINE_SETTINGS.pipelineIndex = 2;
+
         cameraConfig.addPipelineSetting(REFLECTIVE_PIPELINE_SETTINGS);
         cameraConfig.addPipelineSetting(COLORED_SHAPE_PIPELINE_SETTINGS);
+        cameraConfig.addPipelineSetting(APRIL_TAG_PIPELINE_SETTINGS);
     }
 
     @Test
@@ -90,9 +97,12 @@ public void deserializeConfig() {
                 configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(0);
         var coloredShapePipelineSettings =
                 configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(1);
+        var apriltagPipelineSettings =
+                configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(2);
 
         Assertions.assertEquals(REFLECTIVE_PIPELINE_SETTINGS, reflectivePipelineSettings);
         Assertions.assertEquals(COLORED_SHAPE_PIPELINE_SETTINGS, coloredShapePipelineSettings);
+        Assertions.assertEquals(APRIL_TAG_PIPELINE_SETTINGS, apriltagPipelineSettings);
 
         Assertions.assertTrue(
                 reflectivePipelineSettings instanceof ReflectivePipelineSettings,
@@ -100,6 +110,9 @@ public void deserializeConfig() {
         Assertions.assertTrue(
                 coloredShapePipelineSettings instanceof ColoredShapePipelineSettings,
                 "Conig loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!");
+        Assertions.assertTrue(
+                apriltagPipelineSettings instanceof AprilTagPipelineSettings,
+                "Conig loaded pipeline settings for index 2 not of expected type AprilTagPipelineSettings!");
     }
 
     @AfterAll
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 d808c6003a..8c56846c38 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
@@ -150,6 +150,14 @@ public void test2020() {
         CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
         printTestResults(pipelineResult);
 
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputFrame,
+                pipelineResult.outputFrame,
+                pipeline.getSettings(),
+                pipelineResult.targets);
+
         // these numbers are not *accurate*, but they are known and expected
         var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
diff --git a/photon-server/build.gradle b/photon-server/build.gradle
index ac67691674..9f8dc5622e 100644
--- a/photon-server/build.gradle
+++ b/photon-server/build.gradle
@@ -13,6 +13,7 @@ apply from: "${rootDir}/shared/common.gradle"
 
 dependencies {
     implementation project(':photon-core')
+    implementation project(':photon-targeting')
 
     implementation "io.javalin:javalin:4.2.0"
 
diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
new file mode 100644
index 0000000000..5d70294af4
--- /dev/null
+++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
@@ -0,0 +1,104 @@
+/*
+ * 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.vision.pipeline;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import java.io.IOException;
+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;
+import org.photonvision.vision.apriltag.AprilTagJNI;
+import org.photonvision.vision.camera.QuirkyCamera;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class AprilTagTest {
+    @BeforeEach
+    public void Init() throws IOException {
+        TestUtils.loadLibraries();
+        AprilTagJNI.forceLoad();
+    }
+
+    @Test
+    public void testApriltagFacingCamera() {
+        var pipeline = new AprilTagPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
+                        TestUtils.WPI2020Image.FOV,
+                        TestUtils.get2020LifeCamCoeffs(false));
+
+        CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+        printTestResults(pipelineResult);
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputFrame,
+                pipelineResult.outputFrame,
+                pipeline.getSettings(),
+                pipelineResult.targets);
+
+        TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
+        Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 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();
+        System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ);
+
+        // We expect the object X axis to be to the right, or negative-Y in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.08);
+        // We expect the object Y axis to be up, or +Z in world space
+        Assertions.assertEquals(
+                1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.08);
+        // We expect the object Z axis to towards the camera, or negative-X in world space
+        Assertions.assertEquals(
+                -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.08);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}
diff --git a/test-resources/testimages/apriltag/tag1_640_480.jpg b/test-resources/testimages/apriltag/tag1_640_480.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..a18957d65702d9f089c246431edb863eccdb4ff2
GIT binary patch
literal 15576
zcmZX5Wn5K3_wF1*B%~Vzq`SL8y1PrdBo5uEba!_*QW8=kNOwpm(v5T@-1FWK|9kIy
z=fgR(Xa6Sl%(K>-XU&@D#pfr0rS#6zf`^}#gNvJ$gPk2buYkAUB?<~E3hJx>oiH&l
z|4+ik#>T_LgZ*M+Vq@dt;^7ex5Wosxe?&w?WMrhIR8-VVOkB_XAP+zQ96SU90soWW
z;1LlZFOXhB!2eqg00a&m0TC0x!ND@{FJ7V|zJyi%Pcat!8w6|)M2UCmi8wD@IVnHj
zHsCdWn=VY&(vZSmrz)C3lC(63x+Ph-(@=A9pT6a3I{UbxiS?guVGaDJoBw2CEn#uI
zfs;^&$A0HZiNN{cxffu)gsi~@!$T+m95#T*2G7d?4ORu#BNh+?w~$`6_HM8BiJyL=
z08(FfZ?i3&+7%4B`*WhAVd7N)>Q}@XytU_oOz%eka#ynEEkJ8xtzKaA&B%|-5rm(V
zw5FPWs+>A-cOmv&)mN;znCXc2@SouAn@c)wQWg2W!8Bj9?~4<Oq0aOdSuAO8^aJBu
zR<pDlGb`6@){IKB(s62uO5_b6+nOQ4KHkb-RvlLwSr0ZRIX=B=4*%C8L`|g2S?W5&
zZ8P+gG8rRf`LqV5GPGQ4Q@`{RY8E3ZIn}#(6QT6c8wU8*(kk&)bZ$>vk<`Oj(vd{F
z?{&2M-Ty`;NutiRErjzcq8CmmKuzL`6780cX~Tb{%2yRP8}`LD*Gu>unB(+SVZ~z0
z+8)0qU-;qD15av0pFvRf44yD5Hx3Rz)-*8E$U%!P-9AmE&cjV$ouXf$rnysk9C+->
zOzr=Q3fZO#b<%PeWi&2yCR6a_Dt6@6X2{|kvsdLh=AMy1^wm$SOy($OSbC8YolbP5
zchLtOw!&X6twLxMM$BEg6PeSGW%(29gg=@lsAE*f<wmwb{R~R)2c4f>7!CcO!JNS$
zv*#r*u6<(G!0bUzey^oPYlwi<#3Qb%0Efoq#ph*J+Lmoj!Y^36qJb|bG>U{NZ-VNQ
zuL!G(2-gwi@j2aPV!5b8Dl}(^bLz4CBWWzolS>(28Bp~a$`Th;wHEcTtyPt}3RN>-
zCyo@;3^Pbn-Re)k%{H_AB{OTBuKJ2br}gtai-2ZN_~DjK7Key}*!dEEs>H;+Otv*$
z`uJ+RB+DYdX}<n5F!w&zuz@rA3LtX_baxGDLAv&jR{-MEAnHs%EFh+N3Xla2G62#A
zs_N}Z^%TH=G;y}ip|pj}ja4h+8-D0T>%4(BKeV}bcX&w*>6K*8N-fqN&uACoRldvZ
zdbbq1W1~zJH7%l#FOQQ?+^?<~8-_=zBYW<rxJ%QKqNs31Lr@u7rQ3glk1rKXQ)+L=
zFZS_TuK}r^DkAF&C33kOsh_@4xE*W#m}dB#_H4@9fr|EP?2z>t4;(oqj{XV@5d(fL
znq)c#6;z<}ZB*3<Unkv24_pSip)SPFU1*3n#UVA&lwe%FOuS0&Pt!|F2r$%aDciFN
zgwMU$DaU4>hf`+kSDBuXK-_zGR7zTCNh64%z&h`(vKLOwy_-p#G$FO_otN3jneS66
z(zj)?opr2Vv$KNA{S&#J`T8zvlD4L*WQi4<UtlDeMP_U0w6)UrLqj}T>9C$tR+rD1
zrm_W#+7TD~qVk15tqh5<+EMGDYaDM48>x$gZvuOXN#UyQZ6(Qn=O*ZBmZWagQ`&4U
zIc~2%f4p|?%8p{N8)~8UjP6AJX2(<{RY4SBLi9#f9)jpkjjo+$>q@JGf5~oN7HO+5
zw~eEXPFpaaBac(uNgHUOZJ^oWu_#e)^_Mvv{bO>|$G16^>z1$but{lFXWP8l)Qt(p
z4^qC|>Bi8U6IsV9_dDZuDMdenRFnQn|0Lzr64Z0ak{M~9x2rt|C3Xuso)+OMKmaLU
z7xk_e*!b3R$;<`Ai~2D)lvDut1RnqhUa%U4rUk<#RW9ZS1Hp-NWY+Ih5g+~<*<7mS
z^AwH>1-?A#bjqk(`yH92^znq6e1nayR4DF32-5>E5M4ijzd0?`Hj4l^RkrTTx}}g?
z*D^J2<dwp08lE-_DuHXsPKsUE8+)NjxV}OOT}p*WTiMS!l4Wkooqf`b%u=<lWisL}
zD=t&qJ)LC5B5UHZpqZj&r%E++MCVp1<^*4dmg4N~uNd9MGa0GXNuv}qp_Vg~;R5to
zLY9ul1x?x+RXC0Kx<3+YnTL^*S!UMTcC?uzJu`k~hm7l{CY#qIQFd-ymgiE*=#Sg+
zQDR9jn&asRe9<^gyX#Wij2cHKoF$$PEwUH+Axkz(eJ6Xs>3#33vUSpytxz7Sn;*v@
z`oogL5vr(DamoJurEQY*BTb6ZRz^*LDAs~OPutKS_Q#Tnj!1Q~cEXt^-9GxyYca9J
zcv5jmY!nGXQWf|L`H6K@aWpw}wth4GM3?l%rE$#1xVe(4)5KOIblwu)63W;`TJn_q
z+8LJRA*lrFWZ3TWmyBho`*^2Ia{2rJn4*@2hMK}6#MDl#{nH9{)YC){YhU$8L2D7;
zBo%Ld&A)FaxXC)VwKoZS8&tN-?Qp++p0O$8=3_cBNC9R8)9Ifw9|0oQ>gg846u?km
z2WXE~(5^@T=@;xd;CylcFJeW$cY6c4<Sv|_HXZ&B#R|albw^!|)c)M?KthprJ$n~f
zH8j=GE?4QJpf#NXeUkst0e?y6+(R_AiSGQd=nBn1z1=GkT4tTg{L4FicY~<3kXf$J
zSEA15(65=?l3(Qq;>XE!lM{9Dai`52o#_t0VrY6$PqV1lD%*3dwQz`Falcid4gT4Y
zJ21UPOj+XLMxMk-XSYPe9Zjj5#<}j#_$T;QWv{Bi$P25-J~MGJ_xiy5aj{9pNLfTV
zM(9^Yb?buIrPhq^osCJcfnn;#TeRZ#AEibof88ByR$LxNRKg+}<3?RZ__IBAiX9i#
z8i$T>TI3Cx*LYpmQu3n4;zUBW{++nnO$!PThNALv9PcL1M2p~yqzMP4AMzCwlkU}0
znw?$V1YS%HZ?0H`)0}3fD2DK$%-NR`)2c+`X>g}bGt%{iq8m|Q$sOYq$-m-WXDUuH
zeH&6C8OrS$-KH{iAVFW;Y~1G=4wqC`!J=q;{KD?hK$}NOG5i%ym^-7TPr6#M21QjU
z{%I6##jrLCi@K1EjgP*+x>TagOFNXhk6P2jEXAo~c@!v7224rKj@&wfr=RisKUK-J
zwx1~G)`wDI`=WnEe|`C9oBH5u^#MD<qJTEuaKFUtr@3-L9XWu)^qu){b1;~EuN9G_
zlL6qamb*Dk-U9^XtC4c?JwQ<5)n-)g0&v19$PWwF=%A^&nL8aSH8n}@w$%IH8XNwR
z_R|K5gS36P(ssJ_UKyK_H7`nB5#LwJ_9ta=e_$ME=^b*dZrnOajHE94No)BN!%vY1
zcQl2Benr`8cbtg2V!~at>e@reLOMlt9Y5plXjFz$nRG@v5<Q0MUOTtQ4o42L3*Ww>
zznNM6Ofb<LVNOL!3My|A?bsM=JlPdluXZ{Sy+oRE{x4L37jw8qBbsAsBc6({*~v5C
z3aB!6v3-j*4>9}tsvMnrn!2L#fLXkW@y=G(f{z7{4TmY-Dvl@cf+1`}eQM}Cvt=jp
zmv5TLj%Ml9h7#O&I4>1`TVwO<MTw(R%k5JK!^uQyG8Z=!n$4FehP24UO~vmM*I_Hr
z%u7{RzEZ|shrPqa)a~O}lzurhPanlvoWm`kAyXAG-5^Wf-GG2QRdgUAjTNfpT6Dla
zvf7JM6zNhl=Na~rVV$bak*82kWmxKiHbEnvH2jQY1gB#(-8=1-sR+(=tFb!ilmd$#
zlSFBMSA}^6WSaHwW9K#3CP{<Wi>3bt&!3upG)DarGqN53RQEj?1=;I^4+S7D>}L}P
z=&J-GcCG+I>mLa-`1oae%>b$XTbli2Fy!4{A_XvsWrqUf6heUbsfAn|bk~`;Q@~BU
zCM-O$gff8_)p*`2kCvYHxwo0o?EFnNcD9a>PAuov1gUrDZAvjEBi`Gu?I&dH6{ZF8
zS%az;T=^}3_O8Uta&*vosAT4D`5tQ?c%*9-{)k0N9LCh72{gZH{u5K4t7S@H`;9s6
zimzeYS&HVhvSl5|&QL3I75hcbYHUKB3Bj0zTIYi97~SYwXXohgKR&VF+k7+qH8tf6
zQ}zR+zG&cFtCT?Prg<4S(-*q%sUq<;xWej-HqD93aj4=s(_O<^qp7PHoAkGhLuS}d
zhv~1gZ!gM^(cG|^om|q?hnk0D0JUD?obnq?K7M(1>c|-)%&LQ7SkW?kaO!CktAK>~
zOPAti{epD>R~D!Ko|N!KD5>R8M6XmV4&?w^><GX7E0mPa!9~c47WdQ;Y_#K|avt-q
z1Ys%(jo4$A2X?9+FRwqSq|O+|CP3x-RsGg#X43vTx6!Z8Bg9;m*{iw|b~yG<GH+ca
zGk9$%bTP@ef4@3v7-e=6oC#9j(%<(@vZM7WADg^3eaz7c-{ryr=&I{w_S{VXVjvGd
zw*J3k-3B^w(C*&(eKk=WqU9r?I1vozm#VE<MFH@%M%*g^jV8uRfR$S_13>u96kxY_
z+Zl&b%UmH`Q3N1+C9j2~6fNiuaVwB?%-uA#PcuC)n}~>H;uNXM>P9G&ZP!0<MF>}o
zqtk6jMbB&Fo8MXbpJ_E7R+b?_-fdcIER0%YZdTnY<9y-qh`jW_99H$}Z!>Q>6NpAQ
zck?BjS=`R?;gv9nlAuY=?#o;CXZdCN%NLC`p6FG7UeerUwsGHZDbH05+Z^L%f~O*D
zB<`B$UJsvH6Iv9{G$#XoiA_<o${>A%bna9xD%#tK1O3-3^VhlSl6|Gp$L`jf!<lG)
zA(x4<)WL6*E@?}eS!7;txq8NgiA-8xm2gI6$^8tI>z3)86OUySl6d`63j1y5s8A6D
zEee8nXwe4{&A){H1|EHB^o^C1yk`uid?Tw|k$%xvWt%n$Apw`+?k%?oD(oAUYvR9d
z0i_a<O+My*&#b23(w-S*OPY*$Z3+|FO{;Dtc8YESp40j-H0kx;Fv!;$GF&BFD*l$8
zje2DAAL2HavJKiR?P|%h<}r2cAFW=a*f6alx~UBNcE#h}ESxQpuNHf}ZTyE}v%bHc
zN;`Z&2QVF>p#Zq}X=DJ&4TB~C+`TjX;)!5@>(35U>}Ofj071kXyBz?&$s+y+z*G?$
z1yFD!0MH>bGyF#*btUKWfdUZTa)0cf*26>1E&MJ1nJSje)&(G`V1n15Hnsc#9E`IX
zRTE1_Q?#@WU9M2)ioMw>dQ??za0tt$o^#gyB<a&)8}_>#r`;owyT%$Xji%5*`ir!%
zzE(6Af}wmkZK@zF6OGNKtr9Z5%pX|@uhJHGou*c#q^=Q@<x5nf*~Y}46Z=c^#`fhW
zCL8`(QL1hzQ#L%suLkQ+zXmANI|3qnrYFWY)1=2)XVK5cuTur&BZfE|e@PHG{<<Z$
zK>&Ie<0<VbMoWzRy71hHzERAD65(NZq($%5<jhY?s%^{UmXl`RnuaE8=Q83~SSl_T
zRbtkV5|_VL(fz<lb#<n|pl(0piN8!Ci|K~0zT09nk|mwgd`U51D<4*0(YZ;~)x*Kv
ziGfG;+X?3aYCIU8L`K|d7sLn<ajn0y1H>I%lJF@&PfO<lP(SJ3(wTwLw5W?;vQt2d
zYI+agbVZJ|lL9b)EZf)wGoYy|>5<Ps9OBL-S^{3a-p6Hj{w;0eQOKZ<4v<3G6e$2j
z4bu=Rp1K|xJfOkdnt7W$y_=%FQ-oH%zqBAG(0l9i$175|zlqiL<h5<A>6wE#5s{<Y
z4-ThS3{W02-mj5)I-eRO=HAz!*TlFe9~3Pml{|y#7Y!}(+-sLshgP|Rk&-G8c1;#L
z$MdF{LrG^L>2s<qV#z5D$x9x57*K;-E`fNr*ZsL}!U$~*D(&39#_=tKr#?!|+8TM8
zw)(|9o7|(*SzPp3+*vHro(72l_>@&8(dr0#(uk)niB_1*auAvsIW-!4iiP3KP50O;
zZmtJ8N%%_Ui*!34+7Ze`e{<b$is&_R8FE=~Kds}0%TG^Zh1d^Gzehn-qpZR;lx0#+
zMW~8X_aL>cRbq!JRbKi!DT>MzF1upmJ%A`hpUvcS3Fm-=m%v;_IgeI49i`)vX^6Fg
zheA2>8y`wBfP&HCTAAusui0lgqfw8ux}}i4EgLdX1-(i!wQK6xR03q$yY1C&lMAZ=
zj4A{Jrn2_=V{+Ic8BqfqW&KNAK%)lLF$0+E{$*-Fi#|tt3r2P#G(Q8lXuJY|;vOGf
z$fqUV!J(nr=GM5jwYRMSGoyH00K1C47sSD#p3<HV(7f+BPa%p+ZqB>$34bBDIm}A<
zhpFtTP(@oneYhoIJl_?~o>ZAt`<E;(-<_7xn<V)Mws3{3E{u}EPbZD_exj(iN--js
z2W<_qq+Z_M7DuK1u{OrB-&lm`w$Fz*4!Gon_3eeb2G6HU&t9!4nAp3o=6?C7uypM_
zZBn=1cjMg|+rt`DB^q(1S87<p+hx(J*>oPiQ(Zs0+5RpYO=CE=?-?*?()TxxsD}u2
zD?WqatH-Ahk5>c2*)c4&LP6>(V&XS#r9^|gsH&YkG$K~gQSE`<HoIOKbUWVVM9^B2
zpKI0{hING)Uaazmi!(Z1M)WsJ0muhLoHcntNZ3{O4+3s<ZI?n-PUQGX4?gmY$EF%Q
z&j2ex@v@||&Ki2#y&pKel(l3Pg6A7|^^}=i>MZ=jhDg4T%SgC#|B7(PkACN7I&fl|
z`1WFDWph2N*^%1kb=ig<wA87zSj2bBsw;AZ=V|zHK9Yd4Voy)IvSu5(XigV<E)uz1
zL#}xnn@|b8KSM5|vLdlq(WPt4QiF#}RtpoWWfRAmJ_M@)*Lbb{1EthnQ%F`sK~9EF
zA}jU?t}gi4jm<r}w~d+%(U4M(!{(OSHe)DGF;r3uP@{PTw`}807&BZ>nf6sVvMjHZ
zlQx(Ar<K()p2*7qo+MgEOCk%(;at{nhZ)>yV&?9RCV}5Z8OX2kLp7vhYQHc{|1<5&
ztxTFt+~8Aa92Vi}5SeQyBM%~98NdEBr|LVK=&f6s7!7dv1u~M-KLe?)Csg!cxXc3%
zGcZ$rI{ERc0ML|2hi(CML=zZE%jR*Mhp8NXMz-$&?$hv&<drx?`pPZ^pe1YJ51f{1
z8UCXhCv!@XEbbr$1kqlS+N<Kgs_c*-z0`s#Vf!bB{$5L6>+FM=W4DZtfv0c0Owe6k
zUj8QGlcrp(R&n^jFZ&_`;M-N2UfuF$&_)F<*lsiVBIoa~T5nw{_gDwZ=t{uv4j^|I
zcOR>(Hv@%i0~pA~V(If`pWfBqrXg*%nz}DtATmn%RbL5uKII*Vsn+kbUZ15IO+_6C
z)!blBQ+wxrIVTay#~|@_EB4x5*$y8StCaRH<216{4SfceHl}GoO+T+pzTuay`6vav
zSI}nG>eQ<LyKc&G;Hs>`s6NkQm&x4_7P{R)5v@*PApf83S~R9Ox?rLxW^@uf1d*7>
z?QLT6I>Tt(3@#>)3>Gi0;(C0d?{~Vzkp?N^U(~nCw@2sQYSvr6p1Ko>S$k-FFV{s#
zeeItrN!a%nJ89m~e;RSgy?B4Fbj@a7{d?dU<Y|ogArAy8;6|~ZHLPs;C$kv}=OHy4
zvJv?Z9I_Y+8+WR%KOg|_(8hprlX+HmztWc8o<e`#Cz$7u@PR{dE(`%9w-fg=`W(O0
zUnG2L4F4IRdCw*70Hn#As1l2@pX%}GQs(r@?3D*vGi<h%(Vs+!qF!<vjO^hEZ<O0b
zMYLEYQ7mayoj7PxqBSh(S;tWdUny%9Ep6V1IJnXr1i2n=)tq-JD*gPG;L{WmVCSdg
zv*QA<_^n~}#TuZ*s4A`j@C~vH6aWQlCK$Yv0uYgJmjG`1N<IZ(G~saroNxE*M)?Uq
z!v=XhFc|d8ISv{*{^UCw5QZKz)hfw1Gvu%pefc;}+_jg8*qpc=F;q*gg)g3uV#Dt-
z#n60e;uzVbR4z4n=2Y4=wz1W)JIG2{ol<$thJ)vB-GTZHq8-VvbHwB*y>)BnSL$b?
zXVBO#j8E1~m-R3H68A(q2Z5(qp>$SZ!tG@0ls9<L<gLVy|ETwR;?2%m&Wk>!BhN3K
z^^AMve+aVslUICOc5!N26TeR<ZJNJ!-t+m>Wp-ix<Z0l|X&Auwzak&t%m4V#CeUZ*
ziLUdJ{E#s)`BM7sn*7fbPJYS1SAhZ1!(waX=bXXqS5ZONhLhi(8hi5oYzY0iWNWUy
z*Pcq>U3><!$+u{EgH=a@>Er`kgM5gtE|rYkCNmPk(afCwLbI{EKW;K;b~xA9`!*k>
zl^=c%-8G4RdmM;8;T7B3y0^VkYH&%9Xf0c1B|pq&BAZQEw;nZtyt$lQH!&Ql!qD9R
zClWNW{tQr_!6d_lOq+pw070px=JMOrI_jU{or@=kV^GZ-&IK!dUq<O^8S|2~5`)B{
zY%J;iikuk5O>FhpZ1oRX9h^GF)?V+L!)%9#O;HU*FJ2RpWtYo_Vc&{=K8JeBYUE8I
z`z1sLmb__l$Va)NqDMg>c3qKnd!NqC|I=o5G#Fs2MQtUr|Ic9w0K|qBXVgxqmhrIj
ziCzy|mvP^C{^`SxRZ${|+u~6)HR7>Pb1kXd6ut_?OzEzlNAe1?t3rORzo9mnR(!S3
zQlO(VpC+F`cA2A0I`2=Hbr3jXQ{hYIP-Q&$U87wo>Q_XUa4?wtC!_v2?<@}LB;=QZ
z;d<78coo6yX+1|tR;D-z&FXAqaseplREVR^0QH{od=}0%9t-YKF;g>yYWl<;LN}hs
zeGn1PQ~>e_Z0s-v5g`NS_2}7cU1@f;%10(+AxunDwYp>Kt5Z}uo}vg@QdlfV!R^Si
zLR$PYGM$aag>WNHF(a3{#+nvS7vcNGC{`V!xr&z0pn3h?Hseb^(czVB3Cg|bGkA=A
z4DY>{RUBR5bABJ?`bo{r$*7Y1tBB1W*_ZlK=k#m;9_jpiK@8mhf`GGpuHD%|W+!2P
z3<J+x#oj>k&VQ<zEgZ)`3k<t|uaaLX9t#H<?Jafi`g=E8#!2(L{!rImQI)k>_C+e;
zIP4*;{rYXk|BpcBA98{l3+?Z#-p;B;t0y)vW36=c3~rdJ@<n{@67v_rl`e%`=RCc=
zOo<2M1T6ZC*0yFk=Fh17*b02kEOHhuNOR1$P2N|R2Bj9Bbm9LjbQBwP7#1MQQrC=*
z-kMciXR|?5r9<=yl!n^O^`tjGgRDPn*LqLMeD{-uE3fiZe*JZ|93PA($<fMkQ2CDY
z*yS<D=P>Bh^MKXb{o)yz*ezCj@&;vOISKmrEbW-czdaUJPs$}Xc(Rn~exLu)A9TNT
z=8>>-)z@TE4pX3x75`vUnNm8o@iSoq#Zqx9u`APVrR$5E#ae&KqvCzwfuDbsT?_ev
z3H{V~b}aoZo}`v~+cR)H^8IRR@-2y|pd86Ads-Mn^0&*5cRs`UGr-n8C%b1Is;c?)
zjtcssM*VBdSg`5K1E(J}-qT%ipa44t?Rr>?C^m+hi^M|>gS(u9SNuhA@c#w$|72bE
z0Q~UPd>P-Nu%XeU&rixcVdt)Yvf;MbBe6VnBL=)gS_#CFwbTzN2G;Yq4RkY~Ti!}T
zr@B(*YDi^<QtRdus@SZg)~hR*%**OOfAiqZp?uhz<Xe3w7~|dW$dK5Hm$_W?R45w;
zh+k&ji2^{XCEXPa*0i2rj6sC@ZXst1a8BEf0QjUeNf`Nt*7Z7?^NgD9<;=Sv>fGfB
zF7@Vz(rNAYzL5SF{2G5<kFTh1DWN}Aan2$pZ)I`#;6Z_NTbRf4ipeRCS3JXkMK4oG
z8lOJ{KreIhLU4`Rv3NI$rCoBxx_JMI2G#y(Mdf_WaeDHl{<|pM!(MgRsAr|DYviqE
z_>8pH=n4tP&xalp*+ZvVGS=))(<9D*FZ^+)<MLY$0#x|<u%Mp?1ZJEBe^hIq<_8@x
zUJJEse&U<5*?E<>>CyB{mUV^tY&*)HGfH{Zpvtbi+_gNfwCbOm$M@YT&DQ<E+!<bD
zrU`#tlJJ~5rev@09+L|bCmk|_l?vFuHZJ0jE4mpNRhwkuN%q1u4-AjKW}HAL8DpsE
z1^PG^_J5+82T)JmpU76G+;lN<)P7kpUw#*c#<4bts2*3>(!F}^A6|2BJa=vU)~$7b
znUSzj0ZWw}r_|_oCY!=du7z`VjHizm;n`h9%g%fqlULy3-yMH1=*8Bkz)nhOl7^F$
z$!zi&GEeK_1v3)WoO2i3!OHJ1#@+H8YolaDE*t9Zchz64o<aJ>Gl;S=5%RiGp$p4b
zUL7RK`Er+{(MpF*V~Ttflz#kpmZ&Sz{FEY>t5ALEikNX}I_B{}miG*La(yf?KHnN@
z1?=8MBe8tx>v@K8?NJ1OLA~nC7~<J+HtdC3PJfm(?bk{YEiFC^JUrByPDbxpDPv_^
z$Lcjn7R%OdlgnZz&?`Fzw9^bc6<1B<^>=?#Zb_Rsuu1#*`I{s-F|CHt{i%AWnRxGe
z&G!rp&_zyf(i3M*bo`eD`CNx@Glo{>TomJ59a~q7M{D%8^0y9&e5jMp$--XV(lWGE
zIkB}}`LQ;1_Rd?mnI?+`_?T3WxV3b<e>yz<kod90=Wp^H)f@-k_iy~VxPCmxs=b%d
zi8(rEbmH*;C0GA>WH)|i^0EgxKsMY}<V0KdOZU}H9##xi3w1?dHm5W;AtEkkC4M7X
zmc3Zi>!M63Yf&Yo-O48dezj>s=|(1ZLMv&Vy?*Csu*bI=Q<KcGvey$m`J=l@%!s`D
z&4lL4ZUNlvu8Rpq4S-uaQe^~i16ksr{)M?GKs(c~0>F5&IT$GAqW2bBqKr}=3|Q&S
zNL=uPSeTdWt;Qa|JIgoo3J)rV)@x4R^aTrJD5s%ya?;T{DNf_Sqeba4XR!^F{GJ13
zacmD@rSOwXZP{kKCUIT&_*r~?2HDk`_nZzEpW_G~^k9e(W2EvW{eE2KQFW(O&Um{2
z?@{V$rQrc7pl;IbNyW5a@}n<9GCphT7atqK!EnZnWCgyR-}aYR<o>@zS;}-b!-D*Y
z?ljb^PGnW1lj464_~iMPUfqZ(-Yn{>Mty4w&$*cD;YoBooqY^<e>?U5hth-cAJS4*
z(rLvg6;(Zl@uv0s7v(em&=38{Qscc|?dV2ND3TW3(sA!|KZE@r%APo0Le75;;p#CO
zc2|!*$C;N-H=MP~`3HKK!~{GZ+s-{|SB*Hsl$^P(h@^iR%kXLcu{NBUC~ZFQd-U;a
z@_9qb$f|-9-qpVvO>O?*kAc3mS+!$ve;n(7yVsc$SbmVGTJB5$GldfQc$Zl9c?1vL
zrfEp6zkO|{FODuOVwwcDzVn~nRkHc2+1KumUkH;IvUcpSj(*P!JP1-J>7X8VZ?=ke
zc<l8TV#$7*dU}M4Qi<YEet8D$KOeaqANiP0LgGI$G*|T77#u%??W7f=r|8GuPzQ~d
zTV-mym9+a;yCUijB8Sf0>2M~rnI!KI<k1-g#GJjbI?JLrbZT1o%;)t6lryqjWNMU$
zptR~COulF%oBlB}BI?P5<+ekoo2$M8?H7iN5<Om&E`i;y50f?dFP6vq)E#7U%L2aI
z<vMxZHa;0?26n5S5LAZm=j<zghJfJz5BKmybE(HRhw&rcMC1|&<kz#eqME&mYWy<u
z)}8Iy8a!K62n$XlmZsEdy=0-xRmKA|)3_+(QCidvl4*vOT9LW5hm!NEZ{K>s{DDk-
z%RF2yJKF~snl8uRF|vYD5|-|sfp$B9@Lsc@SOX+vp_zJdU@3f=07zeveVGM_*p>s!
z0ABr!mkGQ&?!>Jdr?b1zx8`ozl;3mdu+P3q#b-WdO0o!kUG$dcpUZ&`{T|-2n9D9J
z5t{b_tNb%iDf=XIFjn}YQ=8bQ@1n0{HM!k@O~(c9pScHrojAZRmEbR56z?JAx?H(}
z*3XW1O#k98eLs&~+;-Q4{kYC{{b>4k{eghge`f}(W1~OyetqoD@gMv+UD*k->Gp6>
z%6;DS>I(eXK<v{2Z>+FBQRl<3sALxyUg;XVWe6g&41jaUyB7Nx)E{8`y5~<i){KAk
zB|2N))gxi|Qtm%6aC`X-;K?^dOa4&GTHS2_sEzdRxkPfl-7)<pdmA$;f2ry`<K^=E
zt*BB`jAfe@Vd)9ukw}}M$U~Y%Zuu2Q_RcdHLkyt9IsNtaBqn}xobNTpv}o>+dzM?9
zfvfzywRx9x6rFCQ!x)F=iI&3dGrBQV;&n0-)4hLz-7T?3qgYFfmYAVt9KOub4C#f=
zW~qi)fp4KAVL8Sm92P=E<jgT>VO&J$->Hy>-UN@CrF{wTnm)=g<ii-sez+19i#!Ox
znEpL8n7Wac=Rw`mU)SB>8}3bUj|uw!KP&+-c=8O68~w={h0)scPCT5^{>g@>bMzut
zbA~x8m6m`PP4xDos}+^O?1d@Aly|u7RQ#1Q(ky916_EUSW#V=Zt@rXN@_Mre>-FVh
z;)t$D*);(knod(d)yCk?h8|G5ll&1{_Fq{9WOk?7Bf-F>M9&?-y>AdErvRoZtS-T$
zj_J6MWz-{%<j0~f#DufTt%#G$=?>{Vtbdr=OB3UVN<<XNatpS{rKR-?90*1(3$jR6
zU0kXE(E6Q}n!uA@$S+#~W0Y^@0h~(aiv^y-Do07J$g5Lb)%~>j!06PL;v<ImFY=ry
zrp?XR?_^c~=1K>ybelHtvR?byY=#p|$?V7NH*PGxCx1XQl_0|YG~7AuQ-DOGs51j3
zrqpf-3545z*10|?JPEWsS|F{zx&9Em@*;eKgzbk*AU3(3=D}})K#x!VY{kihd&(K_
zNgtZt1$CL~-~GWcQhJyyGgf`qw%6WZ{%a#oUiFm>=a)lD=f7fNKeQG}o$rJG$(dHy
zYb+dPS=VeYLiHK+6-KhZuwW=&4?lAJ`_tr`LzRWAiT$hH`;ze%rA2VP3H^7q#n@t8
zSfjg96#qv@Dtol~sAk80&%7+dDPLPlS9=TBJ?C%XaI0fngiHC$maZuV{<(S1I9z^S
zL+-V*b<zRZHf@GESi>tLir<P|&W(|{?7dfxG)!N_ABfl`!2FXh3I7Wx7SIQrrar}N
z(NBqfO;=pK%6?;JVLYXB3yCdM`h|pH3s#QpS;xT;P5MN~OhvQY9)dH?Pz?sBXzCHN
z!VXBm@5V2CM_JMuc5&ljVij3;1;E6bzRL&y#0&^f-yOt>LvXDRrA`5yvC;usNie(w
zq8dQSis%IJYX%uUD`krBs2ib2*6b@QTNj>#ii!Oi0?tt;joCtcjx}{S8aOQynDAYx
zI@q2bf=mcQG=e%E6Tl9R9w5@t!XgTepcL|gl&G}Npc=-x1$4_)P<FJjhcq)DhlJ1n
z{!6zI@{ON0CNDT9S6$w^$j%SKrT&?lWpdt8a>>P3;C|<)Hq2(C-&506@C?pKFmC)!
zrOC+;M3X%n{eJhq`I_^FbcqVLI4&tB%XIpkvrTrLkRQ=AsCfpah9>du-M?PlpIhFS
zMe9oLJ6za4a%RI$vov?Rd&DMMRUbJXC>*vA{yCwXJGFj3YGsi2kHlUG><}yK84LKi
zqmp@BC)RUJbHS%%n)U9S95OKHyYP2G<i`z@(X9=i?g6j=Pt$y7H5gqo-JpA@%F}ad
zeX>4%nAtZLfiAB-1F68iAikp>{BO7X|H5~d9PhW<Yp=YvhOfrv_YGq*?!U~&(<<{3
zZn83rs`_SmKEaDP9F8J8I{0^Y?FWuJ#21{<q45`NoN{mC=Cz*<W*P79?C-_TlBMr{
z7yZy31C><y+@Yb|F&TT!b-^oZmx!>$!?|}}I5Cfx16OR|lw4BtVfyX9nARd&Eb&l>
za1^z=q3u#M59N47%<NR`Q1;?)!rMKkfdT{~N28eT7iJCJ-<hJsi!j_~_&8Q}qy6*Z
z=O#<?NmjPXGB4xOzv{a(J1@txUfstqR^r+K1jPTsDI%v%>4B(M@=9RSK1Sk2S6|H%
zK}h}xC#0M{Yiv2WSj8y}g*Z}!KGA`}%gj`1`COXQ{rxCuo9VlH`g%UkmshSDG4_q&
zg))xG3W+r^HO^!Tw{&NO^;Jjf)+t7G(=Cs?KGe%ItJ;fluc?G(TO3j4ftS5x2ml6{
zE`w{{ngW&!rxtqv^W$rPu&TXKU6peW5TnMpf<Y#ST`N)nkS`YpwAC9^0Bx5^n^hbT
z!9D>KPzf1v?EuIlGJ3gEE6Kki2I_2skha%>j!lF<^;2Cqe!1@BS^CyVKazCUmPK`w
zDeuNdR4>xD{@k@X&MtfW-kBvHjHl~pXOLFt(yij~;lrTbW)QJl3H)-Li><n=E||;n
z<hFdjeKX(njp1~2GVcAkf~)gTX7FW{im{-<c^==*^tq$quvt<LNlbB7z|QJs_1{+h
zTh5)7jjOVv7Nf3|*li0Yp_wlIdb_>r8=L0ux!dEUZaIY|&C1e(^<++y4xhZj-Oz>g
zsrY{zoQc9UPPfJf<s%pjq|*xRey6ROExE72zJB0!YM8xlHxc>aM8w!!DB$+<-hBGJ
zUIZ;yS(W6*s(mBczS-w}zz-bO9Utm#0Rj07XPuGG{c^pD8H~>!zov|Z1XGfr>gvXg
z{;uxnz5ezZUzzh{nclAi3Xi|@+r&Cth|&U>@}%J^uMv0Ou5~i&jH+w+rVT_A#?tJk
zSMwzLCYE1Sgw)^vismb%A|a63qP8u`v07*1I~)DnFEQO7r=z59Bt({sb|oV7U1*Sd
zR#=At%JrZ>D-~nSblA4jD9LGDHHsFbnvTCLhQj&cjoE)eP}HwWyJMuM`{|!*A-U**
zpwmTA^ntiI$1L}j)2V_lwJhsWAFD=^srL{=RI-4H2DU?LmQcK4Nsgml-J5k4<IyCS
z=IzP99D%;-(H}h{6}YmYDb3uCBOEhFQQ4y^+EjV2gUdP&!-uvTLlLUN=9i>YU6Q}O
zDBWUpx$sY|qPGhZF0SjQwa#yw<b71BohblMXOXeZyH#<BZgG?TYyrsWsMXplF96)}
z<$k#Uz>y~a)hVw~HDI7Wx;Rk)n58wg09IF?XXq6Gnl=DnH5aW0gII1@tZ{xXV9#&|
z$<aoG2sBb>x;eTqzYg|jlE!;aO2U67@2!RjAF7xZ)YkRdzw3scx?;_fco}*e-jDmp
zD@1)~P$}BBL_SsQ4VJJ{-fHu}ZUnZ=ZvfoUOkVr}?9kbG#N7bAOmHzm^+hiZ?uZgA
zp^etY#h3Y!QHS!y%#o2tlPzNC#D>I`m%?r!{fA5_>M$o(TG_P#O<~k0KJz6H$<mww
zDxNS8*~~#fxsi?GO7b(kD9`S%(|k;{3>L@nU8iCTzFfQA@m96tfm$>-nxY|YvzKlm
zcMuh&wlk*e`kt*fuYL%0L^n!)w<PxA^KQ6ks`d))*Z}a3#FK6xf{x;HcXc*VpW5!k
z!6KrG7y0(!NSx1+?)O2G(-t#e1Q`YJ<uQL`ozF5SG^5`iBySzG@O89mTwq=3ZGWd5
zzb^fKozFkE&8lQ7&VSj?=MugCQSQR9>m)B(e#U^XGm}$hKI4i-#+-4?c~Go#gUgAZ
zD)0r(<VR08=G)@2Ec%O~E-k2nK7pupO$tY?DS<0@Tk%8^4^;7UG`xwgX;vK+x>ZSh
zf6nTottmy~ycF>a5?iq{7B)>(!`tG!XifxWO@x<7+J<R<%aVR2d>a$8{Q;mWvGM79
zm7fCqnnP`KzBNGIpBoIM+iFB#0@S==%@kX}NLniZJ4vSAvjy}~cmQD~N-!6|Q2;P|
zr4<Z|3t@*-zfR~2fwmOwJ=8L)+(phNSvqTdQ-?BzCQU0O8eS9Bw2u^xG&V0g^j1%=
zZg|@rT!t`3>NoMkXIp0w^dHdKMz5MKozgsmG&P7p+LwNs{ztj=A6QT2x<het6&)($
zB3i2>)p!7z*5w}nZ{7t%vTRp-LB!`?h<{qTJSxtu!o(uFc4+mqy>$;3l=M^6FGX`s
z&-dT5IDE4?+(jX!U*Vw`{ev<xb2J+cUAV!V3F)p01B@K(i9N_bIqW6?mBrcRk2Ci9
zK)$AiDANodD*g#V>9A$&eO|u)wfA<Yt!#y-k(BgREX!ZC%>BW8XI0ahZ~RWK3Mq8u
z_DWaP)bg^_R>O&u&>tO<2f87X=~UOrsy%NtCly2nr|Iam<!QfO@su_%Tk4!yoa+ut
zPcmc+vF(c(oQnD?h35y-wcpj7pIn=EiG5w=K6e)>`IpY`TN<P==)ciZHs*DDPEP31
zlIWKyIy+~zXXvY)Oc^M8_|VKPY8KP(6yyf;VobgkVEa+$@W5On%F=&3qp|+Q^1_2T
zBOMJY_SG~j%Jl7IoGxkGRywQQV{E`~`Y)^X?C70lftnwy#br8y&X;MMOrvykEJ{XY
z3$a;u&?fC|=e#nyxCknH*OaKv-^7z1z3nL?Tl%?eW$l%}YI7C~Qhzt$F|>8e6|){}
z+hx^z{IYUT%9N&Q99g^c^~UY2Qq7DT&TP_*lAF|~dvBFQlXGKLOmr@6#3)=bFvynj
zSO3fY`O7*I59=bAA&XVTsQR~o-ka%qdyjrZJ7-;v6K_S-$qKI0$Tm!Dfl-EIEh_*+
zr2up*9#rbX0!VYIMw9@GOOGD(C3txrz<8Pq5b<+hsC0@0U>~ypNW2vX@GJlxQQX6g
zAHpJ>(zZCLEV>zM%snkJYbqAPJCS(+P2H26fHzhqs}{9=ufo4x$~&&<M$0E?S10w4
zzoKpKj1JK=LZ?|jzO}#j7L~JReKX48^MX+2H?4WvDS$AN0q}4iPh9|7Z~?Fs2WTpl
zngvK|uf+9?u|t$Or%N27i!?Ibl{em;ySCuib8>64Db4CnXc*RnP~T-wB}T_QylNX(
zw{kY)B7|xhG@oRNCbu52+KG6`4zKO!g`*qgIkAd_B^BxNwbus7PKk=qOc>z#2ei5-
zuIC*<Jy>3qwi$7c``VD6d0$sowD<Tbs=}L--1}b&Gkakk^XmLX9Uw>!3)0lO!Kk1a
zZfy6EXH_O+_-j7yvz_wQo%JsKEZwzEq^6^&J}}59X7p$Aiv|9XppMS^BjHwJ|0ToG
zIFV=I^7w0Cn4{aHTZC@Xr%_PN`=`Tock$4P8^czzap!oZQ=^y_b!QopPm+e$%5p`%
ze>&a$V|8ClCR6j2&!0%6a-(q~#mxK)5eHlRt&x+qg_QOS#*|K!zY)6URiUj0O(vRD
z4Emah?9ypTa}~$aeKkWS?P45(uT>%yloJr3t|gC+U4q$0v7wpgJRihH<!5DrrT}H6
z2RwjqX+e?2fqYoaoiu>UWu)Cf18@Z;i<QA3M(EH9pD`GsX9aVMuLT1{y8U;-a39QI
z_g&yA0PIK#k84Y+c<jh8KvIywH;zs|iqtMwTCs1H+!Ys}ZgNgXhwL(w!(*8(CHqH&
za#^tyZWcN6mgE<EI?aOLIhb;Ft&VH(C9y|!wg8SZ7=Wr?0FnS8rxXVeF>P^xz=W-I
zVh1<$IlNC5pLWz-bF70lhjv5z%3tyCxJRLRqHU$%N=$5q=!GQhoPj}>*OLF<r#JY%
zuhZn(nzv;$ViFsO8k{Y&aj%H*1;p${HIw4R_Os6O?8fUXA2+!}28l&5vQCuh)YW>L
zzcC+srgqle#w>0+$4ur9uIh@6)S~f`P@FgXEhL{H|K~KxozJ_skfysaZo@m$w$ixI
zom?<9=$C{#xS(M8L_Zrf*EeEZ>Lh0bmqd=ooaCToFJM}8lxY&OiaI}mTc`Fub-1ba
zK)TCcx_y?rPMQwA+xK-So<?dAy?USWVIg*{G8Jq_KY9YVRGY`^HUNLut*g1!kpi$a
z_g5=TdO?5=2|J*%aY&^AImDHIO5jD1&d*H|068^>c~d2#=Yqj^4O@U$01F#Tfk`^5
zI7A%VZXuS`xi^B8cII*dKq9)V0URni)Bqq|@fg6w9dipn5c_)pr;s1h5FnHX5E8ir
zKv4j|OEaK$M%y7Aja8RvZh_pS+?ZE#G}pM8S~6Cz-Gx6|ON^pSAfwpdL$w}F^m6EH
zR6F4n<tl6My&0eP#96V%BYL@m+Un^yuj*ez?RffwhlHFBt1QyxmGbj6m=#yP__j%N
zIZ>sQyF9;K>BsFII6|s)9E`udDYR_hj-t-eiDi>($<4CQ7O;Xk26uUSR*&cyr|_mz
zEa_vDI<)1>V>P-jQz%sHFL~Di_(vAz?*PFhg`cPZF-aj=#`E}NP(g_f;Q6g29n}aR
zY47|+5&&Mwr>h~rq+Tg14!-pP2qYB%Iw~}PW}*TB6=MOQlvo2qSiz7FFcAn$gh4$R
za8eZjJ83oWx>T(lfNiB2L4fi`4xpuq1440_#=?LF;TLt&0!ZTO0DxCTDm6xexN^KP
z4+yQy+OGJ?9@Ao#CNsjEG{qw(ck;e@v@MqVVN~7rn2ySWqkr6e@HNRHgJd_`CKYoi
zx{mIEmA$#VV~JP9PaD#{P@k<W)yO}bx0ZFYs<G&TmUQQp!$;QM%~M52cbEgoQPEZe
zp2|&{mo-_Hf8GlWlC~RIH#aPNdB5cvPuWUikgaOacgQ4UXQ5oW?D1~n+JG^oX5&4S
zFXEWjae6n$sJQpaw$6PugqfFQl+DjZRxteV8L$GB->P}|lapp(MPJ`9mhu)hZ_IPI
z`RcHZE3uDi7`HfZkg&0b83*oO@Ok7V0F(Owf=K}|#|i+WmO&MOXbaZN0$@iB5S4kv
zf!Zu(Fo0tO2-X<@aI*k}CuLfIfQ||1;bG*CHIiN3*$mVmiUUjzegIHr0K7_QFeHQ!
z5E0DY!}~e6#yKr>Wpt2?b^S_IeOTyL#M}GU*+4xh+rCHA$-WU|k;v+gt$g=dj3y82
zlJZ)f`(YmMD~_NF#y%`QG9@{lGr83Ib#+ZUXRy$4e##f`X3CPNS;uJUD|7QIXNPR2
z4FSh!biBil)5eYhwAz%y5iOKzUxtRHi~Qw8<IB(7PSbcFZI3QS?otIzDZh}FOwl}<
z-8M}$qC*2pK)c4SE150&^fw5rX15i1;X~P_lI#=#kS`*`ckzP3%pCc|6DdHv3Pqv|
z232g#aWD$-2_|n$GxY$-!~lRcxd!My^}{4s!TdA8MF|F~^RHkR6DME_7eYP-fO!R=
zo_z<40?wcSSeXDq9Hj;r&}!5GPz#0yjlfK1HzvR`mdgo-Bt)+PMDD6y0HLEZgRO(k
z0$S%Qvi$embxLPH`YmS~0yw|>9{rY4U}4}IapD@~?mD(D(<hAzro!73bU`8JtTcD}
z=om~&m}F?9@HdW^(}aAWwg<cHiV`)qi0iW;IuI)`c3*?j<?szoa+kt}Ue79SsOlE3
z5)0r%ACSe2KII5X!HXSc_%N2kpMn<|dS)QUblzVI!2L*aP=MW-#1n972k=G3Y5+lj
zAD~cc0(b=27clk$3JL&+WN!<IBw?nd5JsCktH28g0Ektc03ZYKN)JE;J@ND00SJi#
z@K_W8z-quQhoVZn25?g{0PYuJC4iJuDeqGN7YTcDpzdQU!N){_%mDx)NsE{`cy(K^
z)ehkDT5wXTgMn>g&^G`^N=q)L0AWPJQ!rdNZZSqMpuA5W2N20}7<D6>*oR%P!VA}1
zaR(?KG8>~qfalNm*H{|c;6=rBTU{_j_AG{t0!V0C6#x#gX{HE(f?RqDUNH&+R3!?4
zVCDoM@GaZ`J(U<XdT$%prvC{c&kSH9dc$=6J8?iIKLv{mhY1N}3UIea&an0Z{LEm0
zga$w)Y-x>%VRt<U-~milrU1XZAV8Aqt{ws)_sB#XjF;J40fbNAKLZ5hE17`*fb2yY
zfOFK=Fax;cNOhG<1psy3dCLsEqR~$@g(<27^D>y$^M=XbH*;KoXhshpILS?~0&QSJ
zW}n?TCJF}VMX(8OUf)*(%3M1H2>?!#9lT<r8>0X=Rc!|B0Lu=x4-irh28`~cFvtRc
zE?FuR3?w69vTLsb+Zd?8-b=%ZNnyd6rHtMHZ4MR{ZGQ@DP~ji~V4I@@@F4&IIA@r?
zEy?ej0)j5<ut{ptJ_UFhG1$QnGM<Yd<~4vdu$o)|5chn<0hWXQWhV?yx`o=~0RE-W
z5Wrem+!lp7t-1PEdm(&<-U_DxMe~9^9X9?lLcsNV+o$&N3QGjQiwU>nG7iT)gStP@
GOaBYz-gA-w

literal 0
HcmV?d00001


From 0d51883626e17d866b84493e3781e26efaf842b9 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 4 Oct 2022 12:21:29 -0400
Subject: [PATCH 2/4] Address comments by Tyler

---
 .../common/util/math/MathUtils.java           | 25 +++++++++++--------
 .../vision/apriltag/DetectionResult.java      | 14 +++++------
 .../vision/target/TrackedTarget.java          |  7 +++---
 3 files changed, 24 insertions(+), 22 deletions(-)

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 5ae94c42ea..af70b65f62 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
@@ -168,17 +168,20 @@ public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
                 new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
     }
 
-    // Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
-    // to a camera facing along the +X axis of the field parallel with the ground plane
-    // So we need a 180 flip about X axis
-    private static final Vector<N3> APRILTAG_AXIS = VecBuilder.fill(1, 0, 0);
-    private static final double APRILTAG_ANGLE = Units.degreesToRadians(180);
-    private static final Rotation3d APRILTAG_TRANSFORM =
-            new Rotation3d(APRILTAG_AXIS, APRILTAG_ANGLE);
-
-    public static Pose3d convertApriltagtoOpencv(Pose3d pose) {
-        var ocvRotation = APRILTAG_TRANSFORM.rotateBy(pose.getRotation());
-        return new Pose3d(pose.getTranslation(), ocvRotation);
+    private static final Vector<N3> APRILTAG_BASE_ROTATION_AXIS = VecBuilder.fill(1, 0, 0);
+    private static final double APRILTAG_BASE_ROTATION_ANGLE = Units.degreesToRadians(180);
+    private static final Rotation3d APRILTAG_BASE_ROTATION =
+            new Rotation3d(APRILTAG_BASE_ROTATION_AXIS, APRILTAG_BASE_ROTATION_ANGLE);
+
+    /**
+     * The AprilTag pose rotation outputs are X left, Y down, Z away from the tag with the tag facing
+     * the camera upright but our OpenCV solvePNP code uses X left, Y up, Z towards the camera. So we
+     * need apply an extra rotation to the rotation component of the apriltag pose to make it
+     * consistent with the EDN system that OpenCV uses So we need a 180 flip about X axis
+     */
+    public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
+        var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
+        return new Transform3d(pose.getTranslation(), ocvRotation);
     }
 
     public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
diff --git a/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java b/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
index 6959802442..1ec12d749f 100644
--- a/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
+++ b/photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
@@ -19,8 +19,8 @@
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
-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;
 import java.util.Arrays;
 
@@ -81,11 +81,11 @@ public double getError2() {
         return error2;
     }
 
-    public Pose3d getPoseResult1() {
+    public Transform3d getPoseResult1() {
         return poseResult1;
     }
 
-    public Pose3d getPoseResult2() {
+    public Transform3d getPoseResult2() {
         return poseResult2;
     }
 
@@ -96,9 +96,9 @@ public Pose3d getPoseResult2() {
     double centerX, centerY;
     double[] corners;
 
-    Pose3d poseResult1;
+    Transform3d poseResult1;
     double error1;
-    Pose3d poseResult2;
+    Transform3d poseResult2;
     double error2;
 
     public DetectionResult(
@@ -125,12 +125,12 @@ public DetectionResult(
 
         this.error1 = err1;
         this.poseResult1 =
-                new Pose3d(
+                new Transform3d(
                         new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
                         new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)));
         this.error2 = err2;
         this.poseResult2 =
-                new Pose3d(
+                new Transform3d(
                         new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
                         new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)));
     }
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 baf82ddb0c..d04ff22c91 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
@@ -16,7 +16,6 @@
  */
 package org.photonvision.vision.target;
 
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import java.util.List;
@@ -73,16 +72,16 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params)
         m_yaw =
                 TargetCalculations.calculateYaw(
                         result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
-        Pose3d bestPose = new Pose3d();
+        var bestPose = new Transform3d();
         if (result.getError1() <= result.getError2()) {
             bestPose = result.getPoseResult1();
         } else {
             bestPose = result.getPoseResult2();
         }
 
-        bestPose = MathUtils.convertApriltagtoOpencv(bestPose);
+        bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
 
-        m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
+        m_cameraToTarget3d = bestPose;
 
         double[] corners = result.getCorners();
         Point[] cornerPoints =

From 8b552e058fb8dd81e42429d4fded47ea450aa1d7 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 4 Oct 2022 12:23:23 -0400
Subject: [PATCH 3/4] Make comment more coherant

---
 .../java/org/photonvision/common/util/math/MathUtils.java | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

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 af70b65f62..4cdb135837 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
@@ -175,9 +175,11 @@ public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
 
     /**
      * The AprilTag pose rotation outputs are X left, Y down, Z away from the tag with the tag facing
-     * the camera upright but our OpenCV solvePNP code uses X left, Y up, Z towards the camera. So we
-     * need apply an extra rotation to the rotation component of the apriltag pose to make it
-     * consistent with the EDN system that OpenCV uses So we need a 180 flip about X axis
+     * 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
      */
     public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
         var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());

From 28953feb15609512249885ae9fbe033e61294905 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 4 Oct 2022 12:57:22 -0400
Subject: [PATCH 4/4] Even more comments

---
 .../photonvision/common/util/math/MathUtils.java | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

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 4cdb135837..b2e6973105 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
@@ -20,13 +20,11 @@
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.CoordinateSystem;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.Arrays;
@@ -168,12 +166,7 @@ public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
                 new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
     }
 
-    private static final Vector<N3> APRILTAG_BASE_ROTATION_AXIS = VecBuilder.fill(1, 0, 0);
-    private static final double APRILTAG_BASE_ROTATION_ANGLE = Units.degreesToRadians(180);
-    private static final Rotation3d APRILTAG_BASE_ROTATION =
-            new Rotation3d(APRILTAG_BASE_ROTATION_AXIS, APRILTAG_BASE_ROTATION_ANGLE);
-
-    /**
+    /*
      * 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
@@ -181,6 +174,13 @@ public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
      * apriltag pose to make it consistent with the EDN system that OpenCV uses, internally a 180
      * rotation about the X axis
      */
+    private static final Rotation3d APRILTAG_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(1, 0, 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.
+     */
     public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
         var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
         return new Transform3d(pose.getTranslation(), ocvRotation);