From 07904589df78b90fd766d4a2013af520536331dd Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 16 Oct 2022 20:48:30 -0400 Subject: [PATCH] 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 --- .../src/components/pipeline/3D/MiniMap.vue | 2 +- .../org/photonvision/common/util/TestUtils.java | 2 +- .../photonvision/common/util/math/MathUtils.java | 16 +++++++++++++--- .../vision/pipe/impl/Draw3dTargetsPipe.java | 12 +++++++++--- .../vision/pipeline/SolvePNPTest.java | 15 ++++++++------- .../vision/pipeline/AprilTagTest.java | 13 ++++++------- 6 files changed, 38 insertions(+), 22 deletions(-) diff --git a/photon-client/src/components/pipeline/3D/MiniMap.vue b/photon-client/src/components/pipeline/3D/MiniMap.vue index 57d5f14ea..5071143ed 100644 --- a/photon-client/src/components/pipeline/3D/MiniMap.vue +++ b/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, 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 57310f0f4..c3dc29a26 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 @@ -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() { 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 b2e697310..df9d711ea 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 @@ -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())); } /* 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 4d1317730..6fca49d19 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 @@ -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( 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 8c56846c3..6656fcb07 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 @@ -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); } 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 index 5d70294af..3098924eb 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java +++ b/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) {