diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js index 277aa4fdd6..fcf70bbf53 100644 --- a/photon-client/src/store/index.js +++ b/photon-client/src/store/index.js @@ -94,7 +94,8 @@ export default new Vuex.Store({ blur: 0.0, threads: 1, debug: false, - refineEdges: true + refineEdges: true, + numIterations: 1, } } ], diff --git a/photon-client/src/views/PipelineViews/AprilTagTab.vue b/photon-client/src/views/PipelineViews/AprilTagTab.vue index 48ce0fd036..84d54e2a66 100644 --- a/photon-client/src/views/PipelineViews/AprilTagTab.vue +++ b/photon-client/src/views/PipelineViews/AprilTagTab.vue @@ -12,40 +12,55 @@ + @@ -82,6 +97,14 @@ this.$store.commit("mutatePipeline", {"decimate": val}); } }, + numIterations: { + get() { + return this.$store.getters.currentPipelineSettings.numIterations + }, + set(val) { + this.$store.commit("mutatePipeline", {"numIterations": val}); + } + }, blur: { get() { return this.$store.getters.currentPipelineSettings.blur diff --git a/photon-client/src/views/PipelineViews/TargetsTab.vue b/photon-client/src/views/PipelineViews/TargetsTab.vue index acc16666aa..94cd97f8c4 100644 --- a/photon-client/src/views/PipelineViews/TargetsTab.vue +++ b/photon-client/src/views/PipelineViews/TargetsTab.vue @@ -46,6 +46,11 @@ Z Angle, ° + @@ -73,6 +78,11 @@ {{ parseFloat(value.pose.y).toFixed(2) }} m {{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}° + 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 b4ed58a533..6d0c3cc1ce 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 @@ -233,6 +233,7 @@ public static List simpleFromTrackedTargets(List(Nat.N3(), Nat.N3()).fill(pose2RotArr))); } + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be + * ambiguous. + */ + public double getPoseAmbiguity() { + var min = Math.min(error1, error2); + var max = Math.max(error1, error2); + + if (max > 0) { + return min / max; + } else { + return -1; + } + } + @Override public String toString() { return "DetectionResult [centerX=" 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 87934b682d..c6dfced62c 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 @@ -51,6 +51,7 @@ public class TrackedTarget implements Releasable { private CVShape m_shape; private int m_fiducialId = -1; + private double m_poseAmbiguity = -1; private Mat m_cameraRelativeTvec, m_cameraRelativeRvec; @@ -117,6 +118,8 @@ public TrackedTarget(DetectionResult result, TargetCalculationParameters params) var axis = bestPose.getRotation().getAxis().times(angle); rvec.put(0, 0, axis.getData()); setCameraRelativeRvec(rvec); + + m_poseAmbiguity = result.getPoseAmbiguity(); } public void setFiducialId(int id) { @@ -127,6 +130,14 @@ public int getFiducialId() { return m_fiducialId; } + public void setPoseAmbiguity(double ambiguity) { + m_poseAmbiguity = ambiguity; + } + + public double getPoseAmbiguity() { + return m_poseAmbiguity; + } + /** * Set the approximate bouding polygon. * @@ -260,6 +271,7 @@ public HashMap toHashMap() { ret.put("yaw", getYaw()); ret.put("skew", getSkew()); ret.put("area", getArea()); + ret.put("ambiguity", getPoseAmbiguity()); if (getCameraToTarget3d() != null) { ret.put("pose", transformToMap(getCameraToTarget3d())); } diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index f4b2eed262..1e64bc64dc 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(), + 0.25, 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 401fbe8dbe..feb6c9fec9 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -62,7 +62,8 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { << target.cameraToTarget.Rotation().GetQuaternion().W() << target.cameraToTarget.Rotation().GetQuaternion().X() << target.cameraToTarget.Rotation().GetQuaternion().Y() - << target.cameraToTarget.Rotation().GetQuaternion().Z(); + << target.cameraToTarget.Rotation().GetQuaternion().Z() + << target.poseAmbiguity; for (int i = 0; i < 4; i++) { packet << target.corners[i].first << target.corners[i].second; @@ -86,6 +87,8 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { target.cameraToTarget = frc::Transform3d(translation, rotation); + packet >> target.poseAmbiguity; + target.corners.clear(); for (int i = 0; i < 4; i++) { double first = 0; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 2be3c47c7d..5a5ca27f3f 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -90,6 +90,12 @@ class PhotonTrackedTarget { return corners; } + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above + * 0.2 are likely to be ambiguous. -1 if invalid. + */ + double GetPoseAmbiguity() const { return poseAmbiguity; } + /** * Returns the pose of the target relative to the robot. * @return The pose of the target relative to the robot. @@ -109,6 +115,7 @@ class PhotonTrackedTarget { double skew = 0; int fiducialId; frc::Transform3d cameraToTarget; + double poseAmbiguity; 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 a4e0ec3581..c7d5743fda 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()), + 0.25, List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -81,6 +82,7 @@ void testSimplePipelineResult() { 4.0, 2, new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), @@ -93,6 +95,7 @@ void testSimplePipelineResult() { 6.7, 3, new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, List.of( new TargetCorner(1, 2), new TargetCorner(3, 4), 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 0893a00904..b28998dc2c 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -27,7 +27,7 @@ import org.photonvision.common.dataflow.structures.Packet; public class PhotonTrackedTarget { - public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4); + public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1); private double yaw; private double pitch; @@ -35,6 +35,7 @@ public class PhotonTrackedTarget { private double skew; private int fiducialId; private Transform3d cameraToTarget = new Transform3d(); + private double poseAmbiguity; private List targetCorners; public PhotonTrackedTarget() {} @@ -47,6 +48,7 @@ public PhotonTrackedTarget( double skew, int id, Transform3d pose, + double ambiguity, List corners) { assert corners.size() == 4; this.yaw = yaw; @@ -56,6 +58,7 @@ public PhotonTrackedTarget( this.fiducialId = id; this.cameraToTarget = pose; this.targetCorners = corners; + this.poseAmbiguity = ambiguity; } public double getYaw() { @@ -79,6 +82,14 @@ public int getFiducialId() { return fiducialId; } + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be + * ambiguous. -1 if invalid. + */ + public double getPoseAmbiguity() { + return poseAmbiguity; + } + /** * 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 @@ -134,6 +145,8 @@ public Packet createFromPacket(Packet packet) { y = packet.decodeDouble(); z = packet.decodeDouble(); var rotation = new Rotation3d(new Quaternion(w, x, y, z)); + this.cameraToTarget = new Transform3d(translation, rotation); + this.poseAmbiguity = packet.decodeDouble(); this.targetCorners = new ArrayList<>(4); for (int i = 0; i < 4; i++) { @@ -142,8 +155,6 @@ public Packet createFromPacket(Packet packet) { targetCorners.add(new TargetCorner(cx, cy)); } - this.cameraToTarget = new Transform3d(translation, rotation); - return packet; } @@ -166,6 +177,7 @@ public Packet populatePacket(Packet packet) { packet.encode(cameraToTarget.getRotation().getQuaternion().getX()); packet.encode(cameraToTarget.getRotation().getQuaternion().getY()); packet.encode(cameraToTarget.getRotation().getQuaternion().getZ()); + packet.encode(poseAmbiguity); for (int i = 0; i < 4; i++) { packet.encode(targetCorners.get(i).x);