From 2d7a88e23124de823d99141c32a09efe68321aab Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 22 Oct 2022 07:42:45 -0400 Subject: [PATCH] Expose both pose solutions (#521) * Half-add second pose * add c++ * run wpiformat * Fix c++ --- .../networktables/NTDataPublisher.java | 5 +- .../vision/pipe/impl/SolvePNPPipe.java | 3 +- .../vision/pipeline/AprilTagPipeline.java | 10 ++- .../vision/target/TrackedTarget.java | 30 ++++++-- .../vision/pipeline/CirclePNPTest.java | 2 +- .../vision/pipeline/SolvePNPTest.java | 6 +- .../org/photonvision/SimPhotonCamera.java | 2 +- .../org/photonvision/SimVisionSystem.java | 1 + .../cpp/photonlib/PhotonTrackedTarget.cpp | 43 ++++++++--- .../include/photonlib/PhotonTrackedTarget.h | 19 ++++- .../java/org/photonvision/PacketTest.java | 3 + .../vision/pipeline/AprilTagTest.java | 4 +- .../targeting/PhotonTrackedTarget.java | 75 ++++++++++++------- .../robot/DrivetrainPoseEstimator.java | 2 +- 14 files changed, 142 insertions(+), 63 deletions(-) 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 6d0c3cc1c..4da85e01d 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 class NTDataPublisher implements CVPipelineResultConsumer { 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 class NTDataPublisher implements CVPipelineResultConsumer { t.getArea(), t.getSkew(), t.getFiducialId(), - t.getCameraToTarget3d(), + t.getBestCameraToTarget3d(), + t.getAltCameraToTarget3d(), t.getPoseAmbiguity(), cornerList)); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java index c4c5672a0..4b5aa51a6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java @@ -100,8 +100,9 @@ public class SolvePNPPipe Core.norm(rVec)); Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation)); - target.setCameraToTarget3d( + target.setBestCameraToTarget3d( new Transform3d(targetPose.getTranslation(), targetPose.getRotation())); + target.setAltCameraToTarget3d(new Transform3d()); } Mat rotationMatrix = new Mat(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 7b89a6648..af2159b18 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -143,9 +143,13 @@ public class AprilTagPipeline extends CVPipeline>(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 5a5ca27f3..cf287fbad 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 c7d5743fd..44456893a 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 @@ class PacketTest { -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 @@ class PacketTest { 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 @@ class PacketTest { 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 3098924eb..c8bd73f99 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 class AprilTagTest { 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 @@ public class AprilTagTest { 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 b28998dc2..8d22a4f22 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 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); + 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 class PhotonTrackedTarget { double skew, int id, Transform3d pose, + Transform3d altPose, double ambiguity, List corners) { assert corners.size() == 4; @@ -56,7 +58,8 @@ public class 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 class PhotonTrackedTarget { /** * 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 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(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 class PhotonTrackedTarget { 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 class PhotonTrackedTarget { 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 class PhotonTrackedTarget { + ", 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 c9e6480c4..c2f2102c0 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 class DrivetrainPoseEstimator { 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(),