From 3ad476bc28e7a6acbd3f7cf63bbfa85e8461300b Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 10 Jan 2022 20:31:36 -0800 Subject: [PATCH] Send corners of min area rectangles (#382) Adds a new corners entry to targets. Breaks byte-packed backwards compatibility. Co-authored-by: Tyler Veness --- .../networktables/NTDataPublisher.java | 15 +++- .../vision/pipeline/ReflectivePipeline.java | 2 +- .../vision/target/TrackedTarget.java | 2 +- .../org/photonvision/SimVisionSystem.java | 13 +++- .../cpp/photonlib/PhotonTrackedTarget.cpp | 42 +++++++++--- .../native/cpp/photonlib/SimVisionSystem.cpp | 3 +- .../include/photonlib/PhotonTrackedTarget.h | 17 ++++- .../java/org/photonvision/PacketTest.java | 68 +++++++------------ photon-lib/src/test/native/cpp/PacketTest.cpp | 48 ++++++------- .../src/main/resources/web/index.html | 2 +- .../targeting/PhotonTrackedTarget.java | 51 +++++++++++--- .../photonvision/targeting/TargetCorner.java | 51 ++++++++++++++ 12 files changed, 215 insertions(+), 99 deletions(-) create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java 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 f02808962..56cd341c3 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 @@ -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 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)); + 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; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 9e0ca5b20..abc109da5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -108,7 +108,7 @@ public class ReflectivePipeline extends CVPipeline m_subContours; // can be empty + public List m_subContours; // can be empty private MatOfPoint2f m_approximateBoundingPolygon; diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index bc0bd9936..d2d0984c9 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/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)))); } }); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index ef0b52e6a..b5c2936b8 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -18,19 +18,28 @@ #include "photonlib/PhotonTrackedTarget.h" #include +#include #include +#include namespace photonlib { -PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area, - double skew, - const frc::Transform2d& pose) - : yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {} +PhotonTrackedTarget::PhotonTrackedTarget( + double yaw, double pitch, double area, double skew, + const frc::Transform2d& pose, + const wpi::SmallVector, 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 - << target.cameraToTarget.Translation().X().value() - << target.cameraToTarget.Translation().Y().value() - << target.cameraToTarget.Rotation().Degrees().value(); + packet << target.yaw << target.pitch << target.area << target.skew + << target.cameraToTarget.Translation().X().value() + << target.cameraToTarget.Translation().Y().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; } diff --git a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp b/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp index a6e1be3e2..8a7da56b7 100644 --- a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp +++ b/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); } } diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index d4ddbb3b4..7b7f8a34e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -19,9 +19,11 @@ #include #include +#include #include #include +#include #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, - const frc::Transform2d& pose); + PhotonTrackedTarget( + double yaw, double pitch, double area, double skew, + const frc::Transform2d& pose, + const wpi::SmallVector, 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, 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, 4> corners; }; } // 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 55d231257..0b98ee48b 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/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); - } } diff --git a/photon-lib/src/test/native/cpp/PacketTest.cpp b/photon-lib/src/test/native/cpp/PacketTest.cpp index ba31e23de..6d25f7888 100644 --- a/photon-lib/src/test/native/cpp/PacketTest.cpp +++ b/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, - frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)}; + 3.0, + 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 targets{ photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, - frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)}, + 3.0, + -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, - frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad)}}; + 3.0, + -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 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 bytes; - for (auto a : bytePack) bytes.emplace_back(static_cast(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); -} diff --git a/photon-server/src/main/resources/web/index.html b/photon-server/src/main/resources/web/index.html index b20a7cfba..988f55e6a 100644 --- a/photon-server/src/main/resources/web/index.html +++ b/photon-server/src/main/resources/web/index.html @@ -1 +1 @@ -

UI has not been copied!

\ No newline at end of file +

UI has not been copied!

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 a6b1495b5..8fb26f695 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/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 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 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 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; } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java new file mode 100644 index 000000000..d98369388 --- /dev/null +++ b/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 . + */ +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 + ')'; + } +}