Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-27 02:01:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Expose pose ambiguity (#483)

Browse Source 
* Expose pose ambiguity

* Run spotless

* Add tooltips and expose number of iterations
This commit is contained in:
Matt
2022-10-08 09:27:00 -04:00
committed by GitHub
parent dafee954e0
commit 2c6b0ddac3
11 changed files with 97 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -233,6 +233,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
t.getSkew(),
t.getFiducialId(),
t.getCameraToTarget3d(),
t.getPoseAmbiguity(),
cornerList));
}
return ret;

15
photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
View File

@@ -135,6 +135,21 @@ public class DetectionResult {
new Rotation3d(new MatBuilder<>(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="

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

@@ -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 class TrackedTarget implements Releasable {
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 class TrackedTarget implements Releasable {
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 class TrackedTarget implements Releasable {
ret.put("yaw", getYaw());
ret.put("skew", getSkew());
ret.put("area", getArea());
ret.put("ambiguity", getPoseAmbiguity());
if (getCameraToTarget3d() != null) {
ret.put("pose", transformToMap(getCameraToTarget3d()));
}