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 6d0c3cc1ce..4da85e01df 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,7 +189,7 @@ public void accept(CVPipelineResult result) { targetAreaEntry.forceSetDouble(bestTarget.getArea()); targetSkewEntry.forceSetDouble(bestTarget.getSkew()); - var pose = bestTarget.getCameraToTarget3d(); + var pose = bestTarget.getBestCameraToTarget3d(); targetPoseEntry.forceSetDoubleArray( new double[] { pose.getTranslation().getX(), @@ -232,7 +232,8 @@ public static List simpleFromTrackedTargets(List toHashMap() { ret.put("skew", getSkew()); ret.put("area", getArea()); ret.put("ambiguity", getPoseAmbiguity()); - if (getCameraToTarget3d() != null) { - ret.put("pose", transformToMap(getCameraToTarget3d())); + if (getBestCameraToTarget3d() != null) { + ret.put("pose", transformToMap(getBestCameraToTarget3d())); } ret.put("fiducialId", getFiducialId()); return ret; diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index b67345c045..5b13aed581 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -161,7 +161,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) { System.out.println( "Found targets at " + pipelineResult.targets.stream() - .map(TrackedTarget::getCameraToTarget3d) + .map(TrackedTarget::getBestCameraToTarget3d) .collect(Collectors.toList())); } } diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java index 6656fcb07f..9341e2c0d1 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java @@ -111,7 +111,7 @@ public void test2019() { printTestResults(pipelineResult); // these numbers are not *accurate*, but they are known and expected - var pose = pipelineResult.targets.get(0).getCameraToTarget3d(); + var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05); Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05); @@ -159,7 +159,7 @@ public void test2020() { pipelineResult.targets); // these numbers are not *accurate*, but they are known and expected - var pose = pipelineResult.targets.get(0).getCameraToTarget3d(); + var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05); Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05); // Z rotation should be mostly facing us @@ -211,7 +211,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) { System.out.println( "Found targets at " + pipelineResult.targets.stream() - .map(TrackedTarget::getCameraToTarget3d) + .map(TrackedTarget::getBestCameraToTarget3d) .collect(Collectors.toList())); } } diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java index 2807bd9b09..b277387897 100644 --- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java @@ -144,7 +144,7 @@ public void submitProcessedFrame( targetAreaEntry.setDouble(bestTarget.getArea()); targetSkewEntry.setDouble(bestTarget.getSkew()); - var transform = bestTarget.getCameraToTarget(); + var transform = bestTarget.getBestCameraToTarget(); double[] poseData = { transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees() }; diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index 1e64bc64dc..dda23e7ce6 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -171,6 +171,7 @@ public void processFrame(Pose2d robotPoseMeters) { 0.0, -1, // TODO fiducial ID new Transform3d(), + new Transform3d(), 0.25, List.of( 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 feb6c9fec9..9f37443b7d 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -41,12 +41,12 @@ PhotonTrackedTarget::PhotonTrackedTarget( area(area), skew(skew), fiducialId(id), - cameraToTarget(pose), + bestCameraToTarget(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.bestCameraToTarget == bestCameraToTarget && other.corners == corners; } @@ -56,13 +56,21 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { packet << target.yaw << target.pitch << target.area << target.skew - << target.fiducialId << target.cameraToTarget.Translation().X().value() - << target.cameraToTarget.Translation().Y().value() - << target.cameraToTarget.Translation().Z().value() - << target.cameraToTarget.Rotation().GetQuaternion().W() - << target.cameraToTarget.Rotation().GetQuaternion().X() - << target.cameraToTarget.Rotation().GetQuaternion().Y() - << target.cameraToTarget.Rotation().GetQuaternion().Z() + << target.fiducialId + << target.bestCameraToTarget.Translation().X().value() + << target.bestCameraToTarget.Translation().Y().value() + << target.bestCameraToTarget.Translation().Z().value() + << target.bestCameraToTarget.Rotation().GetQuaternion().W() + << target.bestCameraToTarget.Rotation().GetQuaternion().X() + << target.bestCameraToTarget.Rotation().GetQuaternion().Y() + << target.bestCameraToTarget.Rotation().GetQuaternion().Z() + << target.altCameraToTarget.Translation().X().value() + << target.altCameraToTarget.Translation().Y().value() + << target.altCameraToTarget.Translation().Z().value() + << target.altCameraToTarget.Rotation().GetQuaternion().W() + << target.altCameraToTarget.Rotation().GetQuaternion().X() + << target.altCameraToTarget.Rotation().GetQuaternion().Y() + << target.altCameraToTarget.Rotation().GetQuaternion().Z() << target.poseAmbiguity; for (int i = 0; i < 4; i++) { @@ -75,17 +83,28 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { packet >> target.yaw >> target.pitch >> target.area >> target.skew >> target.fiducialId; + + // We use these for best and alt transforms below double x = 0; double y = 0; double z = 0; double w = 0; + + // First transform is the "best" pose packet >> x >> y >> z; - const auto translation = frc::Translation3d( + const auto bestTranslation = frc::Translation3d( units::meter_t(x), units::meter_t(y), units::meter_t(z)); packet >> w >> x >> y >> z; - const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation); - target.cameraToTarget = frc::Transform3d(translation, rotation); + // Second transform is the "alternate" pose + packet >> x >> y >> z; + const auto altTranslation = frc::Translation3d( + units::meter_t(x), units::meter_t(y), units::meter_t(z)); + packet >> w >> x >> y >> z; + const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation); packet >> target.poseAmbiguity; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 5a5ca27f3f..cf287fbad7 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -97,10 +97,22 @@ class PhotonTrackedTarget { double GetPoseAmbiguity() const { return poseAmbiguity; } /** - * Returns the pose of the target relative to the robot. + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to + * object/fiducial tag space (X forward, Y left, Z up) with the lowest + * reprojection error. The ratio between this and the alternate target's + * reprojection error is the ambiguity, which is between 0 and 1. * @return The pose of the target relative to the robot. */ - frc::Transform3d GetCameraToTarget() const { return cameraToTarget; } + frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; } + + /** + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to + * object/fiducial tag space (X forward, Y left, Z up) with the highest + * reprojection error + */ + frc::Transform3d GetAlternateCameraToTarget() const { + return altCameraToTarget; + } bool operator==(const PhotonTrackedTarget& other) const; bool operator!=(const PhotonTrackedTarget& other) const; @@ -114,7 +126,8 @@ class PhotonTrackedTarget { double area = 0; double skew = 0; int fiducialId; - frc::Transform3d cameraToTarget; + frc::Transform3d bestCameraToTarget; + frc::Transform3d altCameraToTarget; double poseAmbiguity; wpi::SmallVector, 4> corners; }; diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index c7d5743fda..44456893a4 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -45,6 +45,7 @@ void testSimpleTrackedTarget() { -5.0, -1, new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), 0.25, List.of( new TargetCorner(1, 2), @@ -82,6 +83,7 @@ void testSimplePipelineResult() { 4.0, 2, 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), @@ -95,6 +97,7 @@ void testSimplePipelineResult() { 6.7, 3, 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), diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java index 3098924eb5..c8bd73f991 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java @@ -69,7 +69,7 @@ public void testApriltagFacingCamera() { TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999); // these numbers are not *accurate*, but they are known and expected - var pose = pipelineResult.targets.get(0).getCameraToTarget3d(); + var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d(); Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2); Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2); Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2); @@ -97,7 +97,7 @@ private static void printTestResults(CVPipelineResult pipelineResult) { System.out.println( "Found targets at " + pipelineResult.targets.stream() - .map(TrackedTarget::getCameraToTarget3d) + .map(TrackedTarget::getBestCameraToTarget3d) .collect(Collectors.toList())); } } 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 b28998dc2c..8d22a4f223 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -27,14 +27,15 @@ import org.photonvision.common.dataflow.structures.Packet; public class PhotonTrackedTarget { - public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1); + public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7); private double yaw; private double pitch; private double area; private double skew; private int fiducialId; - private Transform3d cameraToTarget = new Transform3d(); + private Transform3d bestCameraToTarget = new Transform3d(); + private Transform3d altCameraToTarget = new Transform3d(); private double poseAmbiguity; private List targetCorners; @@ -48,6 +49,7 @@ public PhotonTrackedTarget( double skew, int id, Transform3d pose, + Transform3d altPose, double ambiguity, List corners) { assert corners.size() == 4; @@ -56,7 +58,8 @@ public PhotonTrackedTarget( this.area = area; this.skew = skew; this.fiducialId = id; - this.cameraToTarget = pose; + this.bestCameraToTarget = pose; + this.altCameraToTarget = altPose; this.targetCorners = corners; this.poseAmbiguity = ambiguity; } @@ -100,10 +103,18 @@ public List getCorners() { /** * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag - * space (X right, Y up, Z towards the camera/out of the wall) + * space (X forward, Y left, Z up) with the lowest reprojection error */ - public Transform3d getCameraToTarget() { - return cameraToTarget; + public Transform3d getBestCameraToTarget() { + return bestCameraToTarget; + } + + /** + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag + * space (X forward, Y left, Z up) with the highest reprojection error + */ + public Transform3d getAlternateCameraToTarget() { + return altCameraToTarget; } @Override @@ -114,13 +125,37 @@ public boolean equals(Object o) { 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(bestCameraToTarget, that.bestCameraToTarget) + && Objects.equals(altCameraToTarget, that.altCameraToTarget) && Objects.equals(targetCorners, that.targetCorners); } @Override public int hashCode() { - return Objects.hash(yaw, pitch, area, cameraToTarget); + return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget); + } + + private static Transform3d decodeTransform(Packet packet) { + double x = packet.decodeDouble(); + double y = packet.decodeDouble(); + double z = packet.decodeDouble(); + var translation = new Translation3d(x, y, z); + double w = packet.decodeDouble(); + x = packet.decodeDouble(); + y = packet.decodeDouble(); + z = packet.decodeDouble(); + var rotation = new Rotation3d(new Quaternion(w, x, y, z)); + return new Transform3d(translation, rotation); + } + + private static void encodeTransform(Packet packet, Transform3d transform) { + packet.encode(transform.getTranslation().getX()); + packet.encode(transform.getTranslation().getY()); + packet.encode(transform.getTranslation().getZ()); + packet.encode(transform.getRotation().getQuaternion().getW()); + packet.encode(transform.getRotation().getQuaternion().getX()); + packet.encode(transform.getRotation().getQuaternion().getY()); + packet.encode(transform.getRotation().getQuaternion().getZ()); } /** @@ -136,16 +171,9 @@ public Packet createFromPacket(Packet packet) { this.skew = packet.decodeDouble(); this.fiducialId = packet.decodeInt(); - double x = packet.decodeDouble(); - double y = packet.decodeDouble(); - double z = packet.decodeDouble(); - var translation = new Translation3d(x, y, z); - double w = packet.decodeDouble(); - x = packet.decodeDouble(); - y = packet.decodeDouble(); - z = packet.decodeDouble(); - var rotation = new Rotation3d(new Quaternion(w, x, y, z)); - this.cameraToTarget = new Transform3d(translation, rotation); + this.bestCameraToTarget = decodeTransform(packet); + this.altCameraToTarget = decodeTransform(packet); + this.poseAmbiguity = packet.decodeDouble(); this.targetCorners = new ArrayList<>(4); @@ -170,13 +198,8 @@ public Packet populatePacket(Packet packet) { packet.encode(area); packet.encode(skew); packet.encode(fiducialId); - packet.encode(cameraToTarget.getTranslation().getX()); - packet.encode(cameraToTarget.getTranslation().getY()); - packet.encode(cameraToTarget.getTranslation().getZ()); - packet.encode(cameraToTarget.getRotation().getQuaternion().getW()); - packet.encode(cameraToTarget.getRotation().getQuaternion().getX()); - packet.encode(cameraToTarget.getRotation().getQuaternion().getY()); - packet.encode(cameraToTarget.getRotation().getQuaternion().getZ()); + encodeTransform(packet, bestCameraToTarget); + encodeTransform(packet, altCameraToTarget); packet.encode(poseAmbiguity); for (int i = 0; i < 4; i++) { @@ -201,7 +224,7 @@ public String toString() { + ", fiducialId=" + fiducialId + ", cameraToTarget=" - + cameraToTarget + + bestCameraToTarget + ", targetCorners=" + targetCorners + '}'; diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java index c9e6480c49..c2f2102c05 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -87,7 +87,7 @@ public void update( var res = cam.getLatestResult(); if (res.hasTargets()) { double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis() / 1000.0; - Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget(); + Transform3d camToTargetTrans = res.getBestTarget().getBestCameraToTarget(); var transform = new Transform2d( camToTargetTrans.getTranslation().toTranslation2d(),