Rotate all solvePNP-ed poses to be 180 about Z facing camera (#500)

* Rotate all solvePNP-ed poses to be 180 about Z facing camera

* Run spotless

* Fix test coordinate systems

photon-client/src/components/pipeline/3D/MiniMap.vue

@@ -181,7 +181,7 @@ export default {
       this.cubes = []
 
       for (const target of this.targets) {
-        const geometry = new BoxGeometry(0.2, 0.2, 0.3 / 5);
+        const geometry = new BoxGeometry(0.3 / 5, 0.2, 0.2);
         const material = new MeshNormalMaterial();
         let quat = (new Quaternion(
             target.pose.qx,

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

@@ -194,7 +194,7 @@ public class TestUtils {
     public static Path getTestMode2020ImagePath() {
         return getResourcesFolderPath(true)
                 .resolve("testimages")
-                .resolve(WPI2020Image.kBlueGoal_108in_Center.path);
+                .resolve(WPI2020Image.kBlueGoal_156in_Left.path);
     }
 
     public static Path getTestMode2022ImagePath() {

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

@@ -159,11 +159,21 @@ public class MathUtils {
                 pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat));
     }
 
-    // TODO: Refactor into new pipe?
+    /**
+     * All our solvepnp code returns a tag with X left, Y up, and Z out of the tag To better match
+     * wpilib, we want to apply another rotation so that we get Z up, X out of the tag, and Y to the
+     * right. We apply the following change of basis: X -> Y Y -> Z Z -> X
+     */
+    private static final Rotation3d WPILIB_BASE_ROTATION =
+            new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
+
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
+        // TODO: Refactor into new pipe?
         // CameraToTarget _should_ be in opencv-land EDN
-        return CoordinateSystem.convert(
-                new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+        var nwu =
+                CoordinateSystem.convert(
+                        new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
+        return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
     }
 
     /*

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -137,12 +137,18 @@ public class Draw3dTargetsPipe
 
                 // Draw X, Y and Z axis
                 MatOfPoint3f pointMat = new MatOfPoint3f();
+                // Those points are in opencv-land, but we are in NWU
+                // NWU | EDN
+                // X: Z
+                // Y: -X
+                // Z: -Y
+                final double AXIS_LEN = 0.2;
                 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));
+                                new Point3(0, 0, AXIS_LEN),
+                                new Point3(AXIS_LEN, 0, 0),
+                                new Point3(0, AXIS_LEN, 0));
                 pointMat.fromList(list);
 
                 Calib3d.projectPoints(

photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java

@@ -115,15 +115,15 @@ public class SolvePNPTest {
         Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
 
-        // We expect the object X axis to be to the right, or negative-Y in world space
+        // We expect the object X to be forward, or -X in world space
         Assertions.assertEquals(
-                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY(), 0.05);
-        // We expect the object Y axis to be up, or +Z in world space
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.05);
+        // We expect the object Y axis to be right, or negative-Y in world space
         Assertions.assertEquals(
-                1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ(), 0.05);
-        // We expect the object Z axis to towards the camera, or negative-X in world space
+                -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.05);
+        // We expect the object Z axis to be up, or +Z in world space
         Assertions.assertEquals(
-                -1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX(), 0.05);
+                1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.05);
 
         TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
     }
@@ -162,7 +162,8 @@ public class SolvePNPTest {
         var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
         Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
         Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
-        Assertions.assertEquals(Units.degreesToRadians(-42), pose.getRotation().getZ(), 1);
+        // Z rotation should be mostly facing us
+        Assertions.assertEquals(Units.degreesToRadians(-140), pose.getRotation().getZ(), 1);
 
         TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
     }

photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -79,15 +79,14 @@ public class AprilTagTest {
         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
+        // We expect the object X to be forward, or -X 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
+                -1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1);
+        // We expect the object Y axis to be right, or negative-Y in world space
         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);
+                -1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1);
+        // We expect the object Z axis to be up, or +Z in world space
+        Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
     private static void printTestResults(CVPipelineResult pipelineResult) {