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-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -24,10 +24,12 @@ import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
+import org.opencv.core.Point;
 import org.photonvision.common.dataflow.CVPipelineResultConsumer;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 
@@ -209,9 +211,20 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
         var ret = new ArrayList<PhotonTrackedTarget>();
         for (var t : targets) {
+            var points = new Point[4];
+            t.getMinAreaRect().points(points);
+            var cornerList = new ArrayList<TargetCorner>();
+
+            for (int i = 0; i < 4; i++) cornerList.add(new TargetCorner(points[i].x, points[i].y));
+
             ret.add(
                     new PhotonTrackedTarget(
-                            t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget()));
+                            t.getYaw(),
+                            t.getPitch(),
+                            t.getArea(),
+                            t.getSkew(),
+                            t.getCameraToTarget(),
+                            cornerList));
         }
         return ret;
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -108,7 +108,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
         var sortContoursParams =
                 new SortContoursPipe.SortContoursParams(
                         settings.contourSortMode,
-                        settings.outputShowMultipleTargets ? 5 : 1, // TODO don't hardcode?
+                        settings.outputShowMultipleTargets ? 8 : 1, // TODO don't hardcode?
                         frameStaticProperties);
         sortContoursPipe.setParams(sortContoursParams);
 

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -28,7 +28,7 @@ import org.photonvision.vision.opencv.*;
 
 public class TrackedTarget implements Releasable {
     public final Contour m_mainContour;
-    List<Contour> m_subContours; // can be empty
+    public List<Contour> m_subContours; // can be empty
 
     private MatOfPoint2f m_approximateBoundingPolygon;
 

photon-lib/src/main/java/org/photonvision/SimVisionSystem.java

@@ -20,7 +20,9 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
+import java.util.List;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 public class SimVisionSystem {
     SimPhotonCamera cam;
@@ -151,8 +153,17 @@ public class SimVisionSystem {
                                     - this.camPitchDegrees;
 
                     if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) {
+                        // TODO simulate target corners
                         visibleTgtList.add(
-                                new PhotonTrackedTarget(yawDegrees, pitchDegrees, area, 0.0, camToTargetTrans));
+                                new PhotonTrackedTarget(
+                                        yawDegrees,
+                                        pitchDegrees,
+                                        area,
+                                        0.0,
+                                        camToTargetTrans,
+                                        List.of(
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0),
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0))));
                     }
                 });
 

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -18,19 +18,28 @@
 #include "photonlib/PhotonTrackedTarget.h"
 
 #include <iostream>
+#include <utility>
 
 #include <frc/geometry/Translation2d.h>
+#include <wpi/SmallVector.h>
 
 namespace photonlib {
 
-PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area,
+PhotonTrackedTarget::PhotonTrackedTarget(
-                                         double skew,
+    double yaw, double pitch, double area, double skew,
-                                         const frc::Transform2d& pose)
+    const frc::Transform2d& pose,
-    : yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {}
+    const wpi::SmallVector<std::pair<double, double>, 4> corners)
+    : yaw(yaw),
+      pitch(pitch),
+      area(area),
+      skew(skew),
+      cameraToTarget(pose),
+      corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
-         other.skew == skew && other.cameraToTarget == cameraToTarget;
+         other.skew == skew && other.cameraToTarget == cameraToTarget &&
+         other.corners == corners;
 }
 
 bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -38,10 +47,16 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
 }
 
 Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
-  return packet << target.yaw << target.pitch << target.area << target.skew
+  packet << target.yaw << target.pitch << target.area << target.skew
-                << target.cameraToTarget.Translation().X().value()
+         << target.cameraToTarget.Translation().X().value()
-                << target.cameraToTarget.Translation().Y().value()
+         << target.cameraToTarget.Translation().Y().value()
-                << target.cameraToTarget.Rotation().Degrees().value();
+         << target.cameraToTarget.Rotation().Degrees().value();
+
+  for (int i = 0; i < 4; i++) {
+    packet << target.corners[i].first << target.corners[i].second;
+  }
+
+  return packet;
 }
 
 Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
@@ -54,6 +69,15 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
   target.cameraToTarget =
       frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
                        units::degree_t(rot));
+
+  target.corners.clear();
+  for (int i = 0; i < 4; i++) {
+    double first = 0;
+    double second = 0;
+    packet >> first >> second;
+    target.corners.emplace_back(first, second);
+  }
+
   return packet;
 }
 

photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp

@@ -86,7 +86,8 @@ void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
 
     if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
       PhotonTrackedTarget newTgt = PhotonTrackedTarget(
-          yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans);
+          yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
       visibleTgtList.push_back(newTgt);
     }
   }

photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h

@@ -19,9 +19,11 @@
 
 #include <cstddef>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <frc/geometry/Transform2d.h>
+#include <wpi/SmallVector.h>
 
 #include "photonlib/Packet.h"
 
@@ -43,9 +45,12 @@ class PhotonTrackedTarget {
    * @param area The area of the target.
    * @param skew The skew of the target.
    * @param pose The camera-relative pose of the target.
+   * @Param corners The corners of the bounding rectangle.
    */
-  PhotonTrackedTarget(double yaw, double pitch, double area, double skew,
+  PhotonTrackedTarget(
-                      const frc::Transform2d& pose);
+      double yaw, double pitch, double area, double skew,
+      const frc::Transform2d& pose,
+      const wpi::SmallVector<std::pair<double, double>, 4> corners);
 
   /**
    * Returns the target yaw (positive-left).
@@ -71,6 +76,13 @@ class PhotonTrackedTarget {
    */
   double GetSkew() const { return skew; }
 
+  /**
+   * Returns the corners of the minimum area rectangle bounding this target.
+   */
+  wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
+    return corners;
+  }
+
   /**
    * Returns the pose of the target relative to the robot.
    * @return The pose of the target relative to the robot.
@@ -89,5 +101,6 @@ class PhotonTrackedTarget {
   double area = 0;
   double skew = 0;
   frc::Transform2d cameraToTarget;
+  wpi::SmallVector<std::pair<double, double>, 4> corners;
 };
 }  // namespace photonlib

photon-lib/src/test/java/org/photonvision/PacketTest.java

@@ -26,13 +26,23 @@ import org.junit.jupiter.api.Test;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 class PacketTest {
     @Test
     void testSimpleTrackedTarget() {
         var target =
                 new PhotonTrackedTarget(
-                        3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
+                        3.0,
+                        4.0,
+                        9.0,
+                        -5.0,
+                        new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)),
+                        List.of(
+                                new TargetCorner(1, 2),
+                                new TargetCorner(3, 4),
+                                new TargetCorner(5, 6),
+                                new TargetCorner(7, 8)));
         var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES);
         target.populatePacket(p);
 
@@ -62,13 +72,23 @@ class PacketTest {
                                         -4.0,
                                         9.0,
                                         4.0,
-                                        new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))),
+                                        new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         3.0,
                                         -4.0,
                                         9.1,
                                         6.7,
-                                        new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5)))));
+                                        new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5)),
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
         var p2 = new Packet(result2.getPacketSize());
         result2.populatePacket(p2);
 
@@ -77,46 +97,4 @@ class PacketTest {
 
         Assertions.assertEquals(result2, b2);
     }
-
-    @Test
-    void testBytePackFromCpp() {
-        byte[] bytePack = {
-            64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0, 0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0, 0,
-            0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47,
-            122
-        };
-        var t = new PhotonTrackedTarget();
-        t.createFromPacket(new Packet(bytePack));
-
-        var target =
-                new PhotonTrackedTarget(
-                        3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
-
-        Assertions.assertEquals(t, target);
-    }
-
-    @Test
-    void testPacketv2021_1_6() {
-        // From v2021.1.6
-        var simplified =
-                new PhotonPipelineResult(
-                        12.34,
-                        List.of(
-                                new PhotonTrackedTarget(
-                                        -23, -10, 6, 1, new Transform2d(new Translation2d(1, 2), new Rotation2d(3)))));
-        byte[] bytes = {
-            64, 40, -82, 20, 122, -31, 71, -82, 1, -64, 55, 0, 0, 0, 0, 0, 0, -64, 36, 0, 0, 0, 0, 0, 0,
-            64, 24, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0,
-            0, 0, 0, 0, 64, 101, 124, 101, 19, -54, -47, 122, 0
-        };
-
-        // Let's check that those bytes still mean the same thing
-        Packet packet = new Packet(1);
-        packet.clear();
-        packet.setData(bytes);
-        var ret = new PhotonPipelineResult();
-        ret.createFromPacket(packet);
-        System.out.println(ret);
-        Assertions.assertEquals(simplified, ret);
-    }
 }

photon-lib/src/test/native/cpp/PacketTest.cpp

@@ -25,8 +25,13 @@
 
 TEST(PacketTest, PhotonTrackedTarget) {
   photonlib::PhotonTrackedTarget target{
-      3.0, 4.0, 9.0, -5.0,
+      3.0,
-      frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
+      4.0,
+      9.0,
+      -5.0,
+      frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad),
+      {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
+
   photonlib::Packet p;
   p << target;
 
@@ -52,11 +57,20 @@ TEST(PacketTest, PhotonPipelineResult) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.0, 4.0,
+          3.0,
-          frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)},
+          -4.0,
+          9.0,
+          4.0,
+          frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad),
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
       photonlib::PhotonTrackedTarget{
-          3.0, -4.0, 9.1, 6.7,
+          3.0,
-          frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad)}};
+          -4.0,
+          9.1,
+          6.7,
+          frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad),
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
 
   photonlib::PhotonPipelineResult result2{2_s, targets};
   photonlib::Packet p2;
@@ -67,25 +81,3 @@ TEST(PacketTest, PhotonPipelineResult) {
 
   EXPECT_EQ(result2, b2);
 }
-
-TEST(PacketTest, BytePackFromJava) {
-  std::vector<signed char> bytePack{
-      64, 8, 0,  0,  0,  0,   0,  0,  64,  16,  0,   0,   0,   0,
-      0,  0, 64, 34, 0,  0,   0,  0,  0,   0,   -64, 20,  0,   0,
-      0,  0, 0,  0,  63, -16, 0,  0,  0,   0,   0,   0,   64,  0,
-      0,  0, 0,  0,  0,  0,   64, 85, 124, 101, 19,  -54, -47, 122};
-
-  std::vector<char> bytes;
-  for (auto a : bytePack) bytes.emplace_back(static_cast<char>(a));
-
-  photonlib::Packet packet{bytes};
-
-  photonlib::PhotonTrackedTarget res;
-  packet >> res;
-
-  photonlib::PhotonTrackedTarget target{
-      3.0, 4.0, 9.0, -5.0,
-      frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
-
-  EXPECT_EQ(res, target);
-}

photon-server/src/main/resources/web/index.html

@@ -1 +1 @@
-<p>UI has not been copied!</p> 
+<p>UI has not been copied!</p>

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();
+        this.yaw = packet.decodeDouble();
-        pitch = packet.decodeDouble();
+        this.pitch = packet.decodeDouble();
-        area = packet.decodeDouble();
+        this.area = packet.decodeDouble();
-        skew = 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 + ')';
+    }
+}