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 ea51d2a47..9f2079976 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 @@ -172,7 +172,7 @@ public class TrackedTarget implements Releasable { /** * @return O-indexed class index for the detected object. */ - public double getClassID() { + public int getClassID() { return m_classId; } @@ -453,6 +453,8 @@ public class TrackedTarget implements Releasable { t.getArea(), t.getSkew(), t.getFiducialId(), + t.getClassID(), + (float) t.getConfidence(), t.getBestCameraToTarget3d(), t.getAltCameraToTarget3d(), t.getPoseAmbiguity(), diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 22d407479..4e5c3804d 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -96,6 +96,14 @@ class Packet: """ return self._decodeGeneric(">l", 4) + def decodeFloat(self) -> float: + """ + * Returns a decoded float from the packet. + * + * @return A decoded float from the packet. + """ + return self._decodeGeneric(">f", 4) + def decodei64(self) -> int: """ * Returns a decoded int64 from the packet. diff --git a/photon-lib/py/photonlibpy/photonTrackedTarget.py b/photon-lib/py/photonlibpy/photonTrackedTarget.py index 56c225d99..5bfbc73f6 100644 --- a/photon-lib/py/photonlibpy/photonTrackedTarget.py +++ b/photon-lib/py/photonlibpy/photonTrackedTarget.py @@ -71,6 +71,9 @@ class PhotonTrackedTarget: self.skew = packet.decodeDouble() self.fiducialId = packet.decode32() + self.classId = packet.decode32() + self.objDetectConf = packet.decodeFloat() + self.bestCameraToTarget = packet.decodeTransform() self.altCameraToTarget = packet.decodeTransform() diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index d7df0a44e..b2dd11403 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -437,6 +437,8 @@ public class PhotonCameraSim implements AutoCloseable { areaPercent, Math.toDegrees(centerRot.getX()), tgt.fiducialID, + -1, + -1, pnpSim.best, pnpSim.alt, pnpSim.ambiguity, diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index 720cc48d9..9bc945f46 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -264,7 +264,8 @@ class PhotonCameraSim { -centerRot.Z().convert().to(), -centerRot.Y().convert().to(), areaPercent, centerRot.X().convert().to(), tgt.fiducialId, - pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble}); + -1, -1, pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, + cornersDouble}); } if (videoSimRawEnabled) { diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h index 759efb3ad..3cd55b707 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h @@ -193,6 +193,8 @@ class SimVisionSystem { areaPixels, 0.0, target.targetId, + -1, + -1, camToTargetTransform, // TODO sim alternate pose camToTargetTransform, diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 734c57077..d80595fc3 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -73,6 +73,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 0, + -1, + -1, 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, @@ -92,6 +94,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 1, + -1, + -1, 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, @@ -111,6 +115,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, 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, @@ -154,6 +160,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 1, + -1, + -1, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -173,6 +181,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 1, + -1, + -1, new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, @@ -192,6 +202,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()), new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()), 0.4, @@ -239,6 +251,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 1, + -1, + -1, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -258,6 +272,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 1, + -1, + -1, new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, @@ -277,6 +293,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, 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, @@ -324,6 +342,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 1, + -1, + -1, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -343,6 +363,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 1, + -1, + -1, new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, @@ -362,6 +384,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, 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, @@ -401,6 +425,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 1, + -1, + -1, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -420,6 +446,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 0, + -1, + -1, new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, @@ -439,6 +467,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()), new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()), 0.4, @@ -478,6 +508,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 0, + -1, + -1, new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -545,6 +577,8 @@ class PhotonPoseEstimatorTest { 9.0, 4.0, 0, + -1, + -1, new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), 0.7, @@ -564,6 +598,8 @@ class PhotonPoseEstimatorTest { 9.1, 6.7, 1, + -1, + -1, new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), 0.3, @@ -583,6 +619,8 @@ class PhotonPoseEstimatorTest { 19.0, 3.0, 0, + -1, + -1, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), 0.4, diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 9835d0d72..700287ac3 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -61,21 +61,21 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, + 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), @@ -116,21 +116,21 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, + 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m), @@ -159,21 +159,21 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, + 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), @@ -203,21 +203,21 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, + 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), @@ -238,21 +238,21 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { wpi::SmallVector targetsThree{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, + 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 0, + 3.0, -4.0, 9.1, 6.7, 0, -1, -1, frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m), @@ -278,21 +278,21 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, + 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), @@ -320,21 +320,21 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { wpi::SmallVector targets{ photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, + 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, + 3.0, -4.0, 9.1, 6.7, 1, -1, -1, frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, + 9.0, -2.0, 19.0, 3.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index 6a6e6419e..ad5c8665f 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -107,6 +107,19 @@ public class Packet { packetData[writePos++] = (byte) src; } + /** + * Encodes the float into the packet. + * + * @param src The float to encode. + */ + public void encode(float src) { + int data = Float.floatToIntBits(src); + packetData[writePos++] = (byte) ((data >> 24) & 0xff); + packetData[writePos++] = (byte) ((data >> 16) & 0xff); + packetData[writePos++] = (byte) ((data >> 8) & 0xff); + packetData[writePos++] = (byte) (data & 0xff); + } + /** * Encodes the double into the packet. * @@ -213,6 +226,23 @@ public class Packet { return Double.longBitsToDouble(data); } + /** + * Returns a decoded float from the packet. + * + * @return A decoded float from the packet. + */ + public float decodeFloat() { + if (packetData.length < (readPos + 3)) { + return 0; + } + int data = + (int) (0xff & packetData[readPos++]) << 24 + | (int) (0xff & packetData[readPos++]) << 16 + | (int) (0xff & packetData[readPos++]) << 8 + | (int) (0xff & packetData[readPos++]); + return Float.intBitsToFloat(data); + } + /** * Returns a decoded boolean from the packet. * 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 bd59cb4b3..1c4d85ca0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -34,6 +34,8 @@ public class PhotonTrackedTarget implements ProtobufSerializable { private final double area; private final double skew; private final int fiducialId; + private final int classId; + private final float objDetectConf; private final Transform3d bestCameraToTarget; private final Transform3d altCameraToTarget; private final double poseAmbiguity; @@ -50,7 +52,9 @@ public class PhotonTrackedTarget implements ProtobufSerializable { double pitch, double area, double skew, - int id, + int fiducialId, + int classId, + float objDetectConf, Transform3d pose, Transform3d altPose, double ambiguity, @@ -66,7 +70,9 @@ public class PhotonTrackedTarget implements ProtobufSerializable { this.pitch = pitch; this.area = area; this.skew = skew; - this.fiducialId = id; + this.fiducialId = fiducialId; + this.classId = classId; + this.objDetectConf = objDetectConf; this.bestCameraToTarget = pose; this.altCameraToTarget = altPose; this.minAreaRectCorners = minAreaRectCorners; @@ -90,11 +96,24 @@ public class PhotonTrackedTarget implements ProtobufSerializable { return skew; } - /** Get the Fiducial ID, or -1 if not set. */ + /** Get the fiducial ID, or -1 if not set. */ public int getFiducialId() { return fiducialId; } + /** Get the object detection class ID number, or -1 if not set. */ + public int getDetectedObjectClassID() { + return classId; + } + + /** + * Get the object detection confidence, or -1 if not set. This will be between 0 and 1, with 1 + * indicating most confidence, and 0 least. + */ + public float getDetectedObjectConfidence() { + return objDetectConf; + } + /** * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is betweeen 0 * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers @@ -219,7 +238,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable { public static final class APacketSerde implements PacketSerde { @Override public int getMaxByteSize() { - return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); + return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 1 + 4 + 7 + 2 * MAX_CORNERS); } @Override @@ -229,6 +248,8 @@ public class PhotonTrackedTarget implements ProtobufSerializable { packet.encode(value.area); packet.encode(value.skew); packet.encode(value.fiducialId); + packet.encode(value.classId); + packet.encode(value.objDetectConf); PacketUtils.packTransform3d(packet, value.bestCameraToTarget); PacketUtils.packTransform3d(packet, value.altCameraToTarget); packet.encode(value.poseAmbiguity); @@ -250,6 +271,8 @@ public class PhotonTrackedTarget implements ProtobufSerializable { var area = packet.decodeDouble(); var skew = packet.decodeDouble(); var fiducialId = packet.decodeInt(); + var classId = packet.decodeInt(); + var objDetectConf = packet.decodeFloat(); Transform3d best = PacketUtils.unpackTransform3d(packet); Transform3d alt = PacketUtils.unpackTransform3d(packet); double ambiguity = packet.decodeDouble(); @@ -271,6 +294,8 @@ public class PhotonTrackedTarget implements ProtobufSerializable { area, skew, fiducialId, + classId, + objDetectConf, best, alt, ambiguity, diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java index 1bbceb905..cdd4f6cf2 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java @@ -57,6 +57,8 @@ public class PhotonTrackedTargetProto msg.getArea(), msg.getSkew(), msg.getFiducialId(), + msg.getObjDetectionId(), + msg.getObjDetectionConf(), Transform3d.proto.unpack(msg.getBestCameraToTarget()), Transform3d.proto.unpack(msg.getAltCameraToTarget()), msg.getPoseAmbiguity(), diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index 3252528ef..c0200615e 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -28,9 +28,9 @@ static constexpr const uint8_t MAX_CORNERS = 8; namespace photon { PhotonTrackedTarget::PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int id, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, - double ambiguity, + double yaw, double pitch, double area, double skew, int id, int objdetid, + float objdetconf, const frc::Transform3d& pose, + const frc::Transform3d& alternatePose, double ambiguity, const wpi::SmallVector, 4> minAreaRectCorners, const std::vector> detectedCorners) : yaw(yaw), @@ -38,6 +38,8 @@ PhotonTrackedTarget::PhotonTrackedTarget( area(area), skew(skew), fiducialId(id), + objDetectId(objdetid), + objDetectConf(objdetconf), bestCameraToTarget(pose), altCameraToTarget(alternatePose), poseAmbiguity(ambiguity), @@ -47,12 +49,14 @@ PhotonTrackedTarget::PhotonTrackedTarget( bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { return other.yaw == yaw && other.pitch == pitch && other.area == area && other.skew == skew && other.bestCameraToTarget == bestCameraToTarget && + other.objDetectConf == objDetectConf && + other.objDetectId == objDetectId && other.minAreaRectCorners == minAreaRectCorners; } Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { packet << target.yaw << target.pitch << target.area << target.skew - << target.fiducialId + << target.fiducialId << target.objDetectId << target.objDetectConf << target.bestCameraToTarget.Translation().X().value() << target.bestCameraToTarget.Translation().Y().value() << target.bestCameraToTarget.Translation().Z().value() @@ -87,7 +91,7 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { packet >> target.yaw >> target.pitch >> target.area >> target.skew >> - target.fiducialId; + target.fiducialId >> target.objDetectId >> target.objDetectConf; // We use these for best and alt transforms below double x = 0; diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp index 352a93383..9e13181ab 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp @@ -47,6 +47,8 @@ photon::PhotonTrackedTarget wpi::Protobuf::Unpack( m->area(), m->skew(), m->fiducial_id(), + m->obj_detection_id(), + m->obj_detection_conf(), wpi::UnpackProtobuf(m->best_camera_to_target()), wpi::UnpackProtobuf(m->alt_camera_to_target()), m->pose_ambiguity(), @@ -63,6 +65,8 @@ void wpi::Protobuf::Pack( m->set_area(value.area); m->set_skew(value.skew); m->set_fiducial_id(value.fiducialId); + m->set_obj_detection_id(value.objDetectId); + m->set_obj_detection_conf(value.objDetectConf); wpi::PackProtobuf(m->mutable_best_camera_to_target(), value.bestCameraToTarget); wpi::PackProtobuf(m->mutable_alt_camera_to_target(), value.altCameraToTarget); diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index feba56591..bbd6721bf 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -51,8 +51,8 @@ class PhotonTrackedTarget { */ PhotonTrackedTarget( double yaw, double pitch, double area, double skew, int fiducialID, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, - double ambiguity, + int objDetectCassId, float objDetectConf, const frc::Transform3d& pose, + const frc::Transform3d& alternatePose, double ambiguity, const wpi::SmallVector, 4> minAreaRectCorners, const std::vector> detectedCorners); @@ -86,6 +86,18 @@ class PhotonTrackedTarget { */ int GetFiducialId() const { return fiducialId; } + /** + * Get the Fiducial ID of the target currently being tracked, + * or -1 if not set. + */ + int GetDetectedObjectClassID() const { return objDetectId; } + + /** + * Get the object detection confidence, or -1 if not set. This will be between + * 0 and 1, with 1 indicating most confidence, and 0 least. + */ + float GetDetectedObjectConfidence() const { return objDetectConf; } + /** * 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 @@ -147,6 +159,8 @@ class PhotonTrackedTarget { double area = 0; double skew = 0; int fiducialId; + int objDetectId; + float objDetectConf; frc::Transform3d bestCameraToTarget; frc::Transform3d altCameraToTarget; double poseAmbiguity; diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index 23daca25d..984b26e2e 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -53,6 +53,9 @@ message ProtobufPhotonTrackedTarget { double pose_ambiguity = 8; repeated ProtobufTargetCorner min_area_rect_corners = 9; repeated ProtobufTargetCorner detected_corners = 10; + + int32 obj_detection_id = 11; + float obj_detection_conf = 12; } message ProtobufPhotonPipelineResult {