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, °
+
+ |
+ Ambiguity
+ |
+
@@ -73,6 +78,11 @@
{{ parseFloat(value.pose.y).toFixed(2) }} m |
{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}° |
+
+
+ {{ parseFloat(value.ambiguity).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);