Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-05 03:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Expose object detection class id/conf in photonlib (#1266)

Browse Source 
* Implement class id/conf in photonlib

* Maybe fix things

* run lint

* Update Packet.java comments

* Update Packet.java comments again

* Update comments

* oops

* Update packet.py

---------

Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
This commit is contained in:
Matt
2024-05-10 14:52:16 -04:00
committed by GitHub
parent 113951100e
commit 1708376df8
15 changed files with 172 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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(),

8
photon-lib/py/photonlibpy/packet.py
View File

@@ -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.

3
photon-lib/py/photonlibpy/photonTrackedTarget.py
View File

@@ -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()

2
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -437,6 +437,8 @@ public class PhotonCameraSim implements AutoCloseable {
areaPercent,
Math.toDegrees(centerRot.getX()),
tgt.fiducialID,
-1,
-1,
pnpSim.best,
pnpSim.alt,
pnpSim.ambiguity,

3
photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View File

@@ -264,7 +264,8 @@ class PhotonCameraSim {
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble});
-1, -1, pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec,
cornersDouble});
}
if (videoSimRawEnabled) {

2
photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h
View File

@@ -193,6 +193,8 @@ class SimVisionSystem {
areaPixels,
0.0,
target.targetId,
-1,
-1,
camToTargetTransform,
// TODO sim alternate pose
camToTargetTransform,

38
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -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,

42
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -61,21 +61,21 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
wpi::SmallVector<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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<photon::PhotonTrackedTarget, 3> 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),

30
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -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.
*

33
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -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<PhotonTrackedTarget> {
@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,

2
photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java
View File

@@ -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(),

14
photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp
View File

@@ -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<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> 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;

4
photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp
View File

@@ -47,6 +47,8 @@ photon::PhotonTrackedTarget wpi::Protobuf<photon::PhotonTrackedTarget>::Unpack(
m->area(),
m->skew(),
m->fiducial_id(),
m->obj_detection_id(),
m->obj_detection_conf(),
wpi::UnpackProtobuf<frc::Transform3d>(m->best_camera_to_target()),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt_camera_to_target()),
m->pose_ambiguity(),
@@ -63,6 +65,8 @@ void wpi::Protobuf<photon::PhotonTrackedTarget>::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);

18
photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h
View File

@@ -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<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> 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;

3
photon-targeting/src/main/proto/photon.proto
View File

@@ -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 {