From 5b2be119e7c892a53391fa383428af27677b5af2 Mon Sep 17 00:00:00 2001 From: amquake Date: Wed, 25 Oct 2023 18:19:48 -0700 Subject: [PATCH] [photon-lib] Cleanup simulation Rotation3d usage (#982) --- .../vision/pipeline/AprilTagPipeline.java | 21 ++-- .../simulation/PhotonCameraSim.java | 19 +-- .../photonvision/estimation/OpenCVHelp.java | 27 ++-- .../estimation/RotTrlTransform3d.java | 116 ++---------------- 4 files changed, 29 insertions(+), 154 deletions(-) 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 f49aaab5d2..4a17673c94 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 @@ -181,20 +181,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting new Transform3d( new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get()); // match expected AprilTag coordinate system - // TODO cleanup coordinate systems in wpilib 2024 - var apriltagTrl = - CoordinateSystem.convert( - camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN()); - var apriltagRot = - CoordinateSystem.convert( - new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU()) - .plus( - CoordinateSystem.convert( - camToTag.getRotation(), - CoordinateSystem.NWU(), - CoordinateSystem.EDN())); - apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot); - camToTag = new Transform3d(apriltagTrl, apriltagRot); + camToTag = + CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN()); + // (AprilTag expects Z axis going into tag) + camToTag = + new Transform3d( + camToTag.getTranslation(), + new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation())); tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0); } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 7ee78a19be..1ad02b5b4d 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -31,7 +31,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; import java.util.ArrayList; @@ -49,6 +48,7 @@ import org.photonvision.PhotonTargetSortMode; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; +import org.photonvision.estimation.CameraTargetRelation; import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.RotTrlTransform3d; import org.photonvision.estimation.TargetModel; @@ -202,23 +202,16 @@ public Mat getVideoSimFrameRaw() { * @return If this vision target can be seen before image projection. */ public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) { - // var rel = new CameraTargetRelation(camPose, target.getPose()); - // TODO: removal awaiting wpilib Rotation3d performance improvements - var relTarget = RotTrlTransform3d.makeRelativeTo(camPose).apply(target.getPose()); - var camToTargYaw = new Rotation2d(relTarget.getX(), relTarget.getY()); - var camToTargPitch = - new Rotation2d(Math.hypot(relTarget.getX(), relTarget.getY()), -relTarget.getZ()); - var relCam = RotTrlTransform3d.makeRelativeTo(target.getPose()).apply(camPose); - var targToCamAngle = new Rotation2d(relCam.getX(), Math.hypot(relCam.getY(), relCam.getZ())); + var rel = new CameraTargetRelation(camPose, target.getPose()); return ( // target translation is outside of camera's FOV - (Math.abs(camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2) - && (Math.abs(camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2) + (Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2) + && (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2) && (!target.getModel().isPlanar - || Math.abs(targToCamAngle.getDegrees()) + || Math.abs(rel.targToCamAngle.getDegrees()) < 90) // camera is behind planar target and it should be occluded - && (relTarget.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far + && (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far } /** diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index e7028d58cc..1f3b8b9761 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -49,8 +49,8 @@ import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { - private static RotTrlTransform3d NWU_TO_EDN; - private static RotTrlTransform3d EDN_TO_NWU; + private static Rotation3d NWU_TO_EDN; + private static Rotation3d EDN_TO_NWU; static { try { @@ -59,18 +59,12 @@ public final class OpenCVHelp { throw new RuntimeException("Failed to load native libraries!", e); } - NWU_TO_EDN = - new RotTrlTransform3d( - new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)), - new Translation3d()); - EDN_TO_NWU = - new RotTrlTransform3d( - new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)), - new Translation3d()); + NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { - var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F); + var mat = new Mat(matrix.getNumRows(), matrix.getNumCols(), CvType.CV_64F); mat.put(0, 0, matrix.getDDRM().getData()); return mat; } @@ -219,14 +213,12 @@ public static List reorderCircular(List elements, boolean backwards, i return reordered; } - // TODO: RotTrlTransform3d removal awaiting Rotation3d performance improvements /** * Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} * in EDN, this would be {0, -1, 0} in NWU. */ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { - return new RotTrlTransform3d(EDN_TO_NWU.apply(rot), new Translation3d()) - .apply(EDN_TO_NWU.inverse().getRotation()); + return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU)); } /** @@ -234,8 +226,7 @@ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { * in NWU, this would be {0, 0, 1} in EDN. */ private static Rotation3d rotationNWUtoEDN(Rotation3d rot) { - return new RotTrlTransform3d(NWU_TO_EDN.apply(rot), new Translation3d()) - .apply(NWU_TO_EDN.inverse().getRotation()); + return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN)); } /** @@ -243,7 +234,7 @@ private static Rotation3d rotationNWUtoEDN(Rotation3d rot) { * in EDN, this would be {0, -1, 0} in NWU. */ private static Translation3d translationEDNtoNWU(Translation3d trl) { - return EDN_TO_NWU.apply(trl); + return trl.rotateBy(EDN_TO_NWU); } /** @@ -251,7 +242,7 @@ private static Translation3d translationEDNtoNWU(Translation3d trl) { * in NWU, this would be {0, 0, 1} in EDN. */ private static Translation3d translationNWUtoEDN(Translation3d trl) { - return NWU_TO_EDN.apply(trl); + return trl.rotateBy(NWU_TO_EDN); } /** diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index 2b79e920d6..11bd52ff9c 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -18,7 +18,6 @@ package org.photonvision.estimation; 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.geometry.Translation3d; @@ -31,11 +30,6 @@ public class RotTrlTransform3d { private final Translation3d trl; private final Rotation3d rot; - // TODO: removal awaiting wpilib Rotation3d performance improvements - private double m_w; - private double m_x; - private double m_y; - private double m_z; /** * A rotation-translation transformation. @@ -48,25 +42,12 @@ public class RotTrlTransform3d { */ public RotTrlTransform3d(Rotation3d rot, Translation3d trl) { this.rot = rot; - var quat = rot.getQuaternion(); - m_w = quat.getW(); - m_x = quat.getX(); - m_y = quat.getY(); - m_z = quat.getZ(); this.trl = trl; } public RotTrlTransform3d(Pose3d initial, Pose3d last) { - // this.rot = last.getRotation().minus(initial.getRotation()); - // this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot)); - - var quat = initial.getRotation().getQuaternion(); - m_w = quat.getW(); - m_x = quat.getX(); - m_y = quat.getY(); - m_z = quat.getZ(); - this.rot = invrotate(last.getRotation()); - this.trl = last.getTranslation().minus(rotate(initial.getTranslation())); + this.rot = last.getRotation().minus(initial.getRotation()); + this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot)); } /** @@ -85,38 +66,6 @@ public RotTrlTransform3d() { this(new Rotation3d(), new Translation3d()); } - private Translation3d rotate(Translation3d otrl) { - final var p = new Quaternion(0.0, otrl.getX(), otrl.getY(), otrl.getZ()); - final var qprime = times(times(p), new Quaternion(m_w, -m_x, -m_y, -m_z)); - return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ()); - } - - private Translation3d invrotate(Translation3d otrl) { - m_x = -m_x; - m_y = -m_y; - m_z = -m_z; - var result = rotate(otrl); - m_x = -m_x; - m_y = -m_y; - m_z = -m_z; - return result; - } - - private Rotation3d rotate(Rotation3d orot) { - return new Rotation3d(times(orot.getQuaternion())); - } - - private Rotation3d invrotate(Rotation3d orot) { - m_x = -m_x; - m_y = -m_y; - m_z = -m_z; - var result = rotate(orot); - m_x = -m_x; - m_y = -m_y; - m_z = -m_z; - return result; - } - /** * The rotation-translation transformation that makes poses in the world consider this pose as the * new origin, or change the basis to this pose. @@ -129,12 +78,9 @@ public static RotTrlTransform3d makeRelativeTo(Pose3d pose) { /** The inverse of this transformation. Applying the inverse will "undo" this transformation. */ public RotTrlTransform3d inverse() { - // var inverseRot = rot.unaryMinus(); - // var inverseTrl = trl.rotateBy(inverseRot).unaryMinus(); - // return new RotTrlTransform3d(inverseRot, inverseTrl); - - var inverseTrl = invrotate(trl).unaryMinus(); - return new RotTrlTransform3d(new Rotation3d(new Quaternion(m_w, -m_x, -m_y, -m_z)), inverseTrl); + var inverseRot = rot.unaryMinus(); + var inverseTrl = trl.rotateBy(inverseRot).unaryMinus(); + return new RotTrlTransform3d(inverseRot, inverseTrl); } /** This transformation as a Transform3d (as if of the origin) */ @@ -153,8 +99,7 @@ public Rotation3d getRotation() { } public Translation3d apply(Translation3d trl) { - // return trl.rotateBy(rot).plus(this.trl); - return rotate(trl).plus(this.trl); + return trl.rotateBy(rot).plus(this.trl); } public List applyTrls(List trls) { @@ -162,7 +107,7 @@ public List applyTrls(List trls) { } public Rotation3d apply(Rotation3d rot) { - return rotate(rot); + return rot.plus(this.rot); } public List applyRots(List rots) { @@ -170,57 +115,10 @@ public List applyRots(List rots) { } public Pose3d apply(Pose3d pose) { - // return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), - // pose.getRotation().plus(rot)); return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation())); } public List applyPoses(List poses) { return poses.stream().map(this::apply).collect(Collectors.toList()); } - - // TODO: removal awaiting wpilib Rotation3d performance improvements - private Quaternion times(Quaternion other) { - final double o_w = other.getW(); - final double o_x = other.getX(); - final double o_y = other.getY(); - final double o_z = other.getZ(); - return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z); - } - - private static Quaternion times(Quaternion a, Quaternion b) { - final double m_w = a.getW(); - final double m_x = a.getX(); - final double m_y = a.getY(); - final double m_z = a.getZ(); - final double o_w = b.getW(); - final double o_x = b.getX(); - final double o_y = b.getY(); - final double o_z = b.getZ(); - return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z); - } - - private static Quaternion times( - double m_w, - double m_x, - double m_y, - double m_z, - double o_w, - double o_x, - double o_y, - double o_z) { - // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts - - // v₁ x v₂ - final double cross_x = m_y * o_z - o_y * m_z; - final double cross_y = o_x * m_z - m_x * o_z; - final double cross_z = m_x * o_y - o_x * m_y; - - // v = w₁v₂ + w₂v₁ + v₁ x v₂ - final double new_x = o_x * m_w + (m_x * o_w) + cross_x; - final double new_y = o_y * m_w + (m_y * o_w) + cross_y; - final double new_z = o_z * m_w + (m_z * o_w) + cross_z; - - return new Quaternion(m_w * o_w - (m_x * o_x + m_y * o_y + m_z * o_z), new_x, new_y, new_z); - } }