Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11: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

3
photon-client/src/store/index.js
View File

@@ -94,7 +94,8 @@ export default new Vuex.Store({
blur: 0.0,
threads: 1,
debug: false,
refineEdges: true
refineEdges: true,
numIterations: 1,
}
}
],

31
photon-client/src/views/PipelineViews/AprilTagTab.vue
View File

@@ -12,40 +12,55 @@
<CVslider
v-model="decimate"
class="pt-2"
slider-cols="12"
slider-cols="8"
name="Decimate"
min="0"
max="3"
step=".5"
tooltip="Increases FPS at the expense of range by reducing image resolution initially"
@input="handlePipelineData('decimate')"
/>
<CVslider
v-model="blur"
class="pt-2"
slider-cols="12"
slider-cols="8"
name="Blur"
min="0"
max="5"
step=".01"
tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
@input="handlePipelineData('blur')"
/>
<CVslider
v-model="threads"
class="pt-2"
slider-cols="12"
slider-cols="8"
name="Threads"
min="1"
max="8"
step="1"
tooltip="Number of threads spawned by the AprilTag detector"
@input="handlePipelineData('threads')"
/>
<CVswitch
v-model="refineEdges"
class="pt-2"
slider-cols="12"
slider-cols="8"
name="Refine Edges"
tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
@input="handlePipelineData('refineEdges')"
/>
<CVslider
v-model="numIterations"
class="pt-2 pb-4"
slider-cols="8"
name="Pose Estimation Iterations"
min="0"
max="500"
step="1"
tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
@input="handlePipelineData('numIterations')"
/>
</div>
</template>
@@ -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

10
photon-client/src/views/PipelineViews/TargetsTab.vue
View File

@@ -46,6 +46,11 @@
Z Angle,&nbsp;&deg;
</th>
</template>
<template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
<th class="text-center" >
Ambiguity
</th>
</template>
</tr>
</thead>
<tbody>
@@ -73,6 +78,11 @@
<td>{{ parseFloat(value.pose.y).toFixed(2) }}&nbsp;m</td>
<td>{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}&deg;</td>
</template>
<template v-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
<td>
{{ parseFloat(value.ambiguity).toFixed(2) }}
</td>
</template>
</tr>
</tbody>
</template>

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()));
}

1
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -171,6 +171,7 @@ public class SimVisionSystem {
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))));

5
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -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;

7
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -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<std::pair<double, double>, 4> corners;
};
} // namespace photonlib

3
photon-lib/src/test/java/org/photonvision/PacketTest.java
View File

@@ -45,6 +45,7 @@ class PacketTest {
-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 @@ class PacketTest {
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 @@ class PacketTest {
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),

18
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -27,7 +27,7 @@ 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);
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<TargetCorner> targetCorners;
public PhotonTrackedTarget() {}
@@ -47,6 +48,7 @@ public class PhotonTrackedTarget {
double skew,
int id,
Transform3d pose,
double ambiguity,
List<TargetCorner> corners) {
assert corners.size() == 4;
this.yaw = yaw;
@@ -56,6 +58,7 @@ public class PhotonTrackedTarget {
this.fiducialId = id;
this.cameraToTarget = pose;
this.targetCorners = corners;
this.poseAmbiguity = ambiguity;
}
public double getYaw() {
@@ -79,6 +82,14 @@ public class PhotonTrackedTarget {
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 class PhotonTrackedTarget {
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 class PhotonTrackedTarget {
targetCorners.add(new TargetCorner(cx, cy));
}
this.cameraToTarget = new Transform3d(translation, rotation);
return packet;
}
@@ -166,6 +177,7 @@ public class PhotonTrackedTarget {
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);