[photonlib] Simulation Visualization Update (#895)

photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java

@@ -25,6 +25,7 @@
 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;
@@ -37,35 +38,90 @@
 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;
 
-    public RotTrlTransform3d() {
-        this(new Rotation3d(), new Translation3d());
+    /**
+     * A rotation-translation transformation.
+     *
+     * <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
+     * transform as if the origin was transformed by these components instead.
+     *
+     * @param rot The rotation component
+     * @param trl The translation component
+     */
+    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()));
     }
 
     /**
      * Creates a rotation-translation transformation from a Transform3d.
      *
-     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
-     * as if the origin was transformed by these components.
+     * <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
+     * transform as if the origin was transformed by trf instead.
      *
      * @param trf The origin transformation
      */
     public RotTrlTransform3d(Transform3d trf) {
         this(trf.getRotation(), trf.getTranslation());
     }
 
-    /**
-     * A rotation-translation transformation.
-     *
-     * <p>Applying this transformation to poses will preserve their current origin-to-pose transform
-     * as if the origin was transformed by these components.
-     *
-     * @param rot The rotation component
-     * @param trl The translation component
-     */
-    public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
-        this.rot = rot;
-        this.trl = trl;
+    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;
     }
 
     /**
@@ -80,9 +136,12 @@ 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 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);
     }
 
     /** This transformation as a Transform3d (as if of the origin) */
@@ -101,19 +160,74 @@ public Rotation3d getRotation() {
     }
 
     public Translation3d apply(Translation3d trl) {
-        return apply(new Pose3d(trl, new Rotation3d())).getTranslation();
+        // return trl.rotateBy(rot).plus(this.trl);
+        return rotate(trl).plus(this.trl);
     }
-    ;
 
     public List<Translation3d> applyTrls(List<Translation3d> trls) {
         return trls.stream().map(this::apply).collect(Collectors.toList());
     }
 
+    public Rotation3d apply(Rotation3d rot) {
+        return rotate(rot);
+    }
+
+    public List<Rotation3d> applyRots(List<Rotation3d> rots) {
+        return rots.stream().map(this::apply).collect(Collectors.toList());
+    }
+
     public Pose3d apply(Pose3d pose) {
-        return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot));
+        // return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl),
+        // pose.getRotation().plus(rot));
+        return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation()));
     }
 
     public List<Pose3d> applyPoses(List<Pose3d> 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);
+    }
 }

photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java

@@ -25,8 +25,8 @@
 package org.photonvision.estimation;
 
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
@@ -36,8 +36,8 @@
 /** Describes the 3d model of a target. */
 public class TargetModel {
     /**
-     * Translations of this target's vertices relative to its pose. If this target is spherical, this
-     * list has one translation with x == radius.
+     * Translations of this target's vertices relative to its pose. Rectangular and spherical targets
+     * will have four vertices. See their respective constructors for more info.
      */
     public final List<Translation3d> vertices;
 
@@ -47,7 +47,17 @@ public class TargetModel {
     public static final TargetModel kTag16h5 =
             new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
 
-    /** Creates a rectangular, planar target model given the width and height. */
+    /**
+     * Creates a rectangular, planar target model given the width and height. The model has four
+     * vertices:
+     *
+     * <ul>
+     *   <li>Point 0: [0, -width/2, -height/2]
+     *   <li>Point 1: [0, width/2, -height/2]
+     *   <li>Point 2: [0, width/2, height/2]
+     *   <li>Point 3: [0, -width/2, height/2]
+     * </ul>
+     */
     public TargetModel(double widthMeters, double heightMeters) {
         this.vertices =
                 List.of(
@@ -61,18 +71,64 @@ public TargetModel(double widthMeters, double heightMeters) {
     }
 
     /**
-     * Creates a spherical target model which has similar dimensions when viewed from any angle. This
-     * model will only have one vertex which has x == radius.
+     * Creates a cuboid target model given the length, width, height. The model has eight vertices:
+     *
+     * <ul>
+     *   <li>Point 0: [length/2, -width/2, -height/2]
+     *   <li>Point 1: [length/2, width/2, -height/2]
+     *   <li>Point 2: [length/2, width/2, height/2]
+     *   <li>Point 3: [length/2, -width/2, height/2]
+     *   <li>Point 4: [-length/2, -width/2, height/2]
+     *   <li>Point 5: [-length/2, width/2, height/2]
+     *   <li>Point 6: [-length/2, width/2, -height/2]
+     *   <li>Point 7: [-length/2, -width/2, -height/2]
+     * </ul>
+     */
+    public TargetModel(double lengthMeters, double widthMeters, double heightMeters) {
+        this(
+                List.of(
+                        new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
+                        new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
+                        new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0)));
+    }
+
+    /**
+     * Creates a spherical target model which has similar dimensions regardless of its rotation. This
+     * model has four vertices:
+     *
+     * <ul>
+     *   <li>Point 0: [0, -radius, 0]
+     *   <li>Point 1: [0, 0, -radius]
+     *   <li>Point 2: [0, radius, 0]
+     *   <li>Point 3: [0, 0, radius]
+     * </ul>
+     *
+     * <i>Q: Why these vertices?</i> A: This target should be oriented to the camera every frame, much
+     * like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
+     * are used for drawing the image of this sphere, but do not match the corners that will be
+     * published by photonvision.
      */
     public TargetModel(double diameterMeters) {
-        this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0));
+        double radius = diameterMeters / 2.0;
+        this.vertices =
+                List.of(
+                        new Translation3d(0, -radius, 0),
+                        new Translation3d(0, 0, -radius),
+                        new Translation3d(0, radius, 0),
+                        new Translation3d(0, 0, radius));
         this.isPlanar = false;
         this.isSpherical = true;
     }
 
     /**
      * Creates a target model from arbitrary 3d vertices. Automatically determines if the given
-     * vertices are planar(x == 0). More than 2 vertices must be given.
+     * vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
+     * vertices should define a non-intersecting contour.
      *
      * @param vertices Translations representing the vertices of this target model relative to its
      *     pose.
@@ -95,13 +151,31 @@ public TargetModel(List<Translation3d> vertices) {
     /**
      * This target's vertices offset from its field pose.
      *
-     * <p>Note: If this target is spherical, only one vertex radius meters in front of the pose is
-     * returned.
+     * <p>Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
+     * Translation3d)} with this method.
      */
     public List<Translation3d> getFieldVertices(Pose3d targetPose) {
-        return vertices.stream()
-                .map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation())
-                .collect(Collectors.toList());
+        var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation());
+        return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList());
+    }
+
+    /**
+     * Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
+     * to the camera translation. This is used for spherical targets which should not have their
+     * projection change regardless of their own rotation.
+     *
+     * @param tgtTrl This target's translation
+     * @param cameraTrl Camera's translation
+     * @return This target's pose oriented to the camera
+     */
+    public static Pose3d getOrientedPose(Translation3d tgtTrl, Translation3d cameraTrl) {
+        var relCam = cameraTrl.minus(tgtTrl);
+        var orientToCam =
+                new Rotation3d(
+                        0,
+                        new Rotation2d(Math.hypot(relCam.getX(), relCam.getY()), -relCam.getZ()).getRadians(),
+                        new Rotation2d(relCam.getX(), relCam.getY()).getRadians());
+        return new Pose3d(tgtTrl, orientToCam);
     }
 
     @Override