Send corners of min area rectangles (#382)

Adds a new corners entry to targets. Breaks byte-packed backwards compatibility. 

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java

@@ -19,26 +19,38 @@ package org.photonvision.targeting;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import java.util.ArrayList;
+import java.util.List;
 import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 public class PhotonTrackedTarget {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * 7;
+    public static final int PACK_SIZE_BYTES = Double.BYTES * (7 + 2 * 4);
 
     private double yaw;
     private double pitch;
     private double area;
     private double skew;
     private Transform2d cameraToTarget = new Transform2d();
+    private List<TargetCorner> targetCorners;
 
     public PhotonTrackedTarget() {}
 
-    public PhotonTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) {
+    /** Construct a tracked target, given exactly 4 corners */
+    public PhotonTrackedTarget(
+            double yaw,
+            double pitch,
+            double area,
+            double skew,
+            Transform2d pose,
+            List<TargetCorner> corners) {
+        assert corners.size() == 4;
         this.yaw = yaw;
         this.pitch = pitch;
         this.area = area;
         this.skew = skew;
-        cameraToTarget = pose;
+        this.cameraToTarget = pose;
+        this.targetCorners = corners;
     }
 
     public double getYaw() {
@@ -57,6 +69,14 @@ public class PhotonTrackedTarget {
         return skew;
     }
 
+    /**
+     * Return a list of the 4 corners in image space (origin top left, x left, y down), in no
+     * particular order, of the minimum area bounding rectangle of this target
+     */
+    public List<TargetCorner> getCorners() {
+        return targetCorners;
+    }
+
     public Transform2d getCameraToTarget() {
         return cameraToTarget;
     }
@@ -69,7 +89,8 @@ public class PhotonTrackedTarget {
         return Double.compare(that.yaw, yaw) == 0
                 && Double.compare(that.pitch, pitch) == 0
                 && Double.compare(that.area, area) == 0
-                && Objects.equals(cameraToTarget, that.cameraToTarget);
+                && Objects.equals(cameraToTarget, that.cameraToTarget)
+                && Objects.equals(targetCorners, that.targetCorners);
     }
 
     @Override
@@ -84,16 +105,23 @@ public class PhotonTrackedTarget {
      * @return The incoming packet.
      */
     public Packet createFromPacket(Packet packet) {
-        yaw = packet.decodeDouble();
-        pitch = packet.decodeDouble();
-        area = packet.decodeDouble();
-        skew = packet.decodeDouble();
+        this.yaw = packet.decodeDouble();
+        this.pitch = packet.decodeDouble();
+        this.area = packet.decodeDouble();
+        this.skew = packet.decodeDouble();
 
         double x = packet.decodeDouble();
         double y = packet.decodeDouble();
         double r = packet.decodeDouble();
 
-        cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
+        this.targetCorners = new ArrayList<>(4);
+        for (int i = 0; i < 4; i++) {
+            double cx = packet.decodeDouble();
+            double cy = packet.decodeDouble();
+            targetCorners.add(new TargetCorner(cx, cy));
+        }
+
+        this.cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
 
         return packet;
     }
@@ -113,6 +141,11 @@ public class PhotonTrackedTarget {
         packet.encode(cameraToTarget.getTranslation().getY());
         packet.encode(cameraToTarget.getRotation().getDegrees());
 
+        for (int i = 0; i < 4; i++) {
+            packet.encode(targetCorners.get(i).x);
+            packet.encode(targetCorners.get(i).y);
+        }
+
         return packet;
     }
 }

photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+package org.photonvision.targeting;
+
+import java.util.Objects;
+
+/**
+ * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
+ * Origin at the top left, plus-x to the right, plus-y down.
+ */
+public class TargetCorner {
+    public final double x;
+    public final double y;
+
+    public TargetCorner(double cx, double cy) {
+        this.x = cx;
+        this.y = cy;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        TargetCorner that = (TargetCorner) o;
+        return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0;
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(x, y);
+    }
+
+    @Override
+    public String toString() {
+        return "(" + x + "," + y + ')';
+    }
+}