Fix AprilTag rotation reversal bug

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

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -19,14 +19,17 @@
 
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
 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.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 +162,33 @@ 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());
+    /*
+     * 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
+     */
+    private static final Rotation3d APRILTAG_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180));
 
-        // 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));
+    /**
+     * 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);
+    }
 
-        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());
     }
 }

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

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

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

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

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

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;
@@ -26,6 +25,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.*;
@@ -72,14 +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();
         }
 
-        m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
+        bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
+
+        m_cameraToTarget3d = bestPose;
 
         double[] corners = result.getCorners();
         Point[] cornerPoints =
@@ -113,9 +115,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);
     }
 

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,15 +53,20 @@ 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;
 
         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,16 +97,22 @@ 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,
                 "Conig loaded pipeline settings for index 0 not of expected type ReflectivePipelineSettings!");
         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

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

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"