diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index f7a0aaf8a..7a3426114 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -189,11 +189,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer { public static List simpleFromTrackedTargets(List targets) { var ret = new ArrayList(); for (var t : targets) { - var points = new Point[4]; - t.getMinAreaRect().points(points); - var cornerList = new ArrayList(); - - for (int i = 0; i < 4; i++) cornerList.add(new TargetCorner(points[i].x, points[i].y)); + var minAreaRectCorners = new ArrayList(); + var detectedCorners = new ArrayList(); + { + var points = new Point[4]; + t.getMinAreaRect().points(points); + for (int i = 0; i < 4; i++) { + minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y)); + } + } + { + var points = t.getTargetCorners(); + for (int i = 0; i < points.size(); i++) { + detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y)); + } + } ret.add( new PhotonTrackedTarget( @@ -205,7 +215,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer { t.getBestCameraToTarget3d(), t.getAltCameraToTarget3d(), t.getPoseAmbiguity(), - cornerList)); + minAreaRectCorners, + detectedCorners)); } return ret; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java index 74d90da8f..ed3ae46e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java @@ -47,7 +47,7 @@ public class CornerDetectionPipe { var targetCorners = detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls); - target.setCorners(targetCorners); + target.setTargetCorners(targetCorners); break; } default: diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index e02d47809..dd4227824 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -223,7 +223,7 @@ public class ColoredShapePipeline collect2dTargetsResult.output.forEach( shape -> { shape.getMinAreaRect().points(rectPoints); - shape.setCorners(Arrays.asList(rectPoints)); + shape.setTargetCorners(Arrays.asList(rectPoints)); }); sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 925244337..4bb799704 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -41,7 +41,7 @@ public class TrackedTarget implements Releasable { private MatOfPoint2f m_approximateBoundingPolygon; - private List m_targetCorners; + private List m_targetCorners = List.of(); private Point m_targetOffsetPoint; private Point m_robotOffsetPoint; @@ -289,7 +289,7 @@ public class TrackedTarget implements Releasable { if (m_cameraRelativeRvec != null) m_cameraRelativeRvec.release(); } - public void setCorners(List targetCorners) { + public void setTargetCorners(List targetCorners) { this.m_targetCorners = targetCorners; } diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index e52c58f7f..b7fc2ebf5 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -231,6 +231,9 @@ public class SimVisionSystem { camToTargetTrans, camToTargetTransAlt, 0.0, // TODO - simulate ambiguity when straight on? + List.of( + new TargetCorner(0, 0), new TargetCorner(0, 0), + new TargetCorner(0, 0), new TargetCorner(0, 0)), List.of( new TargetCorner(0, 0), new TargetCorner(0, 0), new TargetCorner(0, 0), new TargetCorner(0, 0)))); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index 65c78b52c..42a75f748 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -30,13 +30,15 @@ #include #include +static constexpr const uint8_t MAX_CORNERS = 8; + namespace photonlib { PhotonTrackedTarget::PhotonTrackedTarget( double yaw, double pitch, double area, double skew, int id, const frc::Transform3d& pose, const frc::Transform3d& alternatePose, double ambiguity, - const wpi::SmallVector, 4> corners) + const wpi::SmallVector, 4> minAreaRectCorners) : yaw(yaw), pitch(pitch), area(area), @@ -45,12 +47,12 @@ PhotonTrackedTarget::PhotonTrackedTarget( bestCameraToTarget(pose), altCameraToTarget(alternatePose), poseAmbiguity(ambiguity), - corners(corners) {} + minAreaRectCorners(minAreaRectCorners) {} bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { return other.yaw == yaw && other.pitch == pitch && other.area == area && other.skew == skew && other.bestCameraToTarget == bestCameraToTarget && - other.corners == corners; + other.minAreaRectCorners == minAreaRectCorners; } bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { @@ -77,7 +79,16 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { << target.poseAmbiguity; for (int i = 0; i < 4; i++) { - packet << target.corners[i].first << target.corners[i].second; + packet << target.minAreaRectCorners[i].first + << target.minAreaRectCorners[i].second; + } + + uint8_t num_corners = + std::min(target.detectedCorners.size(), MAX_CORNERS); + packet << num_corners; + for (size_t i = 0; i < target.detectedCorners.size(); i++) { + packet << target.detectedCorners[i].first + << target.detectedCorners[i].second; } return packet; @@ -111,12 +122,21 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { packet >> target.poseAmbiguity; - target.corners.clear(); + target.minAreaRectCorners.clear(); + double first = 0; + double second = 0; for (int i = 0; i < 4; i++) { - double first = 0; - double second = 0; packet >> first >> second; - target.corners.emplace_back(first, second); + target.minAreaRectCorners.emplace_back(first, second); + } + + uint8_t numCorners = 0; + packet >> numCorners; + target.detectedCorners.clear(); + target.detectedCorners.reserve(numCorners); + for (size_t i = 0; i < numCorners; i++) { + packet >> first >> second; + target.detectedCorners.emplace_back(first, second); } return packet; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 80e0b8dca..0dd116268 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -92,10 +92,27 @@ class PhotonTrackedTarget { int GetFiducialId() const { return fiducialId; } /** - * Returns the corners of the minimum area rectangle bounding this target. + * Return a list of the 4 corners in image space (origin top left, x right, y + * down), in no particular order, of the minimum area bounding rectangle of + * this target */ - wpi::SmallVector, 4> GetCorners() const { - return corners; + wpi::SmallVector, 4> GetMinAreaRectCorners() const { + return minAreaRectCorners; + } + + /** + * Return a list of the n corners in image space (origin top left, x right, y + * down), in no particular order, detected for this target. + * For fiducials, the order is known and is always counter-clock wise around + * the tag, like so + * + * -> +X 3 ----- 2 + * | | | + * V + Y | | + * 0 ----- 1 + */ + std::vector> GetDetectedCorners() { + return detectedCorners; } /** @@ -137,6 +154,7 @@ class PhotonTrackedTarget { frc::Transform3d bestCameraToTarget; frc::Transform3d altCameraToTarget; double poseAmbiguity; - wpi::SmallVector, 4> corners; + wpi::SmallVector, 4> minAreaRectCorners; + std::vector> detectedCorners; }; } // namespace photonlib diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index 44456893a..6daa613f3 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -47,6 +47,11 @@ class PacketTest { new Transform3d(new Translation3d(), new Rotation3d()), new Transform3d(new Translation3d(), new Rotation3d()), 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -85,6 +90,11 @@ class PacketTest { new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -99,6 +109,11 @@ class PacketTest { new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), diff --git a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java index 9f04cbd7a..ed5bead86 100644 --- a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java @@ -94,6 +94,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -108,6 +113,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)), new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -127,6 +137,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -166,6 +181,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -180,6 +200,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -199,6 +224,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()), new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -238,6 +268,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -252,6 +287,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -271,6 +311,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()), new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -311,6 +356,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -325,6 +375,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -344,6 +399,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()), new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -374,6 +434,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -388,6 +453,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -406,6 +476,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()), new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -439,6 +514,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -453,6 +533,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -472,6 +557,11 @@ class RobotPoseEstimatorTest { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), 0.4, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 8d22a4f22..908ca24e6 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -23,11 +23,12 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; 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 * (5 + 7 + 2 * 4 + 1 + 7); + private static final int MAX_CORNERS = 8; + public static final int PACK_SIZE_BYTES = + Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); private double yaw; private double pitch; @@ -37,7 +38,12 @@ public class PhotonTrackedTarget { private Transform3d bestCameraToTarget = new Transform3d(); private Transform3d altCameraToTarget = new Transform3d(); private double poseAmbiguity; - private List targetCorners; + + // Corners from the min-area rectangle bounding the target + private List minAreaRectCorners; + + // Corners from whatever corner detection method was used + private List detectedCorners; public PhotonTrackedTarget() {} @@ -51,8 +57,14 @@ public class PhotonTrackedTarget { Transform3d pose, Transform3d altPose, double ambiguity, - List corners) { - assert corners.size() == 4; + List minAreaRectCorners, + List detectedCorners) { + assert minAreaRectCorners.size() == 4; + + if (detectedCorners.size() > MAX_CORNERS) { + detectedCorners = detectedCorners.subList(0, MAX_CORNERS); + } + this.yaw = yaw; this.pitch = pitch; this.area = area; @@ -60,7 +72,8 @@ public class PhotonTrackedTarget { this.fiducialId = id; this.bestCameraToTarget = pose; this.altCameraToTarget = altPose; - this.targetCorners = corners; + this.minAreaRectCorners = minAreaRectCorners; + this.detectedCorners = detectedCorners; this.poseAmbiguity = ambiguity; } @@ -94,11 +107,28 @@ public class PhotonTrackedTarget { } /** - * Return a list of the 4 corners in image space (origin top left, x left, y down), in no + * Return a list of the 4 corners in image space (origin top left, x right, y down), in no * particular order, of the minimum area bounding rectangle of this target */ - public List getCorners() { - return targetCorners; + public List getMinAreaRectCorners() { + return minAreaRectCorners; + } + + /** + * Return a list of the n corners in image space (origin top left, x right, y down), in no + * particular order, detected for this target. + * + *

For fiducials, the order is known and is always counter-clock wise around the tag, like so + * + *

spotless:off + * -> +X 3 ----- 2 + * | | | + * V | | + * +Y 0 ----- 1 + * spotless:on + */ + public List getDetectedCorners() { + return detectedCorners; } /** @@ -118,21 +148,54 @@ public class PhotonTrackedTarget { } @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - PhotonTrackedTarget that = (PhotonTrackedTarget) o; - return Double.compare(that.yaw, yaw) == 0 - && Double.compare(that.pitch, pitch) == 0 - && Double.compare(that.area, area) == 0 - && Objects.equals(bestCameraToTarget, that.bestCameraToTarget) - && Objects.equals(altCameraToTarget, that.altCameraToTarget) - && Objects.equals(targetCorners, that.targetCorners); + public int hashCode() { + final int prime = 31; + int result = 1; + long temp; + temp = Double.doubleToLongBits(yaw); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(pitch); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(area); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(skew); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + fiducialId; + result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); + result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); + temp = Double.doubleToLongBits(poseAmbiguity); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); + result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode()); + return result; } @Override - public int hashCode() { - return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget); + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + PhotonTrackedTarget other = (PhotonTrackedTarget) obj; + if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; + if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; + if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; + if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; + if (fiducialId != other.fiducialId) return false; + if (bestCameraToTarget == null) { + if (other.bestCameraToTarget != null) return false; + } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; + if (altCameraToTarget == null) { + if (other.altCameraToTarget != null) return false; + } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; + if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) + return false; + if (minAreaRectCorners == null) { + if (other.minAreaRectCorners != null) return false; + } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; + if (detectedCorners == null) { + if (other.detectedCorners != null) return false; + } else if (!detectedCorners.equals(other.detectedCorners)) return false; + return true; } private static Transform3d decodeTransform(Packet packet) { @@ -158,6 +221,25 @@ public class PhotonTrackedTarget { packet.encode(transform.getRotation().getQuaternion().getZ()); } + private static void encodeList(Packet packet, List list) { + packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); + for (int i = 0; i < list.size(); i++) { + packet.encode(list.get(i).x); + packet.encode(list.get(i).y); + } + } + + private static List decodeList(Packet p) { + byte len = p.decodeByte(); + var ret = new ArrayList(); + for (int i = 0; i < len; i++) { + double cx = p.decodeDouble(); + double cy = p.decodeDouble(); + ret.add(new TargetCorner(cx, cy)); + } + return ret; + } + /** * Populates the fields of this class with information from the incoming packet. * @@ -176,13 +258,15 @@ public class PhotonTrackedTarget { this.poseAmbiguity = packet.decodeDouble(); - this.targetCorners = new ArrayList<>(4); + this.minAreaRectCorners = new ArrayList<>(4); for (int i = 0; i < 4; i++) { double cx = packet.decodeDouble(); double cy = packet.decodeDouble(); - targetCorners.add(new TargetCorner(cx, cy)); + minAreaRectCorners.add(new TargetCorner(cx, cy)); } + detectedCorners = decodeList(packet); + return packet; } @@ -203,10 +287,12 @@ public class PhotonTrackedTarget { packet.encode(poseAmbiguity); for (int i = 0; i < 4; i++) { - packet.encode(targetCorners.get(i).x); - packet.encode(targetCorners.get(i).y); + packet.encode(minAreaRectCorners.get(i).x); + packet.encode(minAreaRectCorners.get(i).y); } + encodeList(packet, detectedCorners); + return packet; } @@ -226,7 +312,7 @@ public class PhotonTrackedTarget { + ", cameraToTarget=" + bestCameraToTarget + ", targetCorners=" - + targetCorners + + minAreaRectCorners + '}'; } }