Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Java-side bugfixes for RobotPoseEstimator and PhotonCamera (#685)

Browse Source 
Closes #684
Closes #683
Closes #681
Closes #680
Closes #678
This commit is contained in:
Chris Gerth
2023-01-01 21:40:48 -06:00
committed by GitHub
parent 7c49cfe625
commit aaa886bd73
5 changed files with 124 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -280,7 +280,7 @@ public class PhotonCamera {
prevHeartbeatValue = curHeartbeat;
}
return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC);
return ((now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC);
}
private void verifyVersion() {

173
photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
View File

@@ -24,7 +24,9 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
@@ -33,7 +35,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
public class RobotPoseEstimator {
@@ -58,7 +60,7 @@ public class RobotPoseEstimator {
AVERAGE_BEST_TARGETS
}
private Map<Integer, Pose3d> aprilTags;
private AprilTagFieldLayout aprilTags;
private PoseStrategy strategy;
private ArrayList<Pair<PhotonCamera, Transform3d>> cameras;
private Pose3d lastPose;
@@ -72,13 +74,14 @@ public class RobotPoseEstimator {
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>(); <p> map.put(1, new
* Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A Map linking AprilTag IDs to Pose3ds with respect to the FIRST field.
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the
* FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
* the center of the robot to the cameras.
*/
public RobotPoseEstimator(
Map<Integer, Pose3d> aprilTags,
AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
ArrayList<Pair<PhotonCamera, Transform3d>> cameras) {
this.aprilTags = aprilTags;
@@ -91,7 +94,8 @@ public class RobotPoseEstimator {
/**
* Update the estimated pose using the selected strategy.
*
* @return The updated estimated pose and the latency in milliseconds
* @return The updated estimated pose and the latency in milliseconds Estimated pose may be null
* if no targets were seen
*/
public Pair<Pose3d, Double> update() {
if (cameras.isEmpty()) {
@@ -123,18 +127,25 @@ public class RobotPoseEstimator {
return pair;
default:
DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
return Pair.of(lastPose, 0.);
return Pair.of(null, 0.);
}
}
private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
// Loop over each ambiguity of all the cameras
int lowestAI = -1;
int lowestAJ = -1;
double lowestAmbiguityScore = 10;
ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
// Sample result from each camera
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
results.add(p.getFirst().getLatestResult());
}
// Loop over each ambiguity of all the cameras
for (int i = 0; i < cameras.size(); i++) {
List<PhotonTrackedTarget> targets = results.get(i).targets;
for (int j = 0; j < targets.size(); j++) {
if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
lowestAI = i;
@@ -144,17 +155,17 @@ public class RobotPoseEstimator {
}
}
// No targets, return the last pose
// No targets, return null
if (lowestAI == -1 || lowestAJ == -1) {
return Pair.of(lastPose, 0.);
return Pair.of(null, 0.);
}
// Pick the lowest and do the heavy calculations
PhotonTrackedTarget bestTarget =
cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ);
PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
// If the map doesn't contain the ID fail
if (!aprilTags.containsKey(bestTarget.getFiducialId())) {
var tmp = aprilTags.getTagPose(bestTarget.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(bestTarget.getFiducialId())) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
@@ -165,26 +176,28 @@ public class RobotPoseEstimator {
return Pair.of(lastPose, 0.);
}
var tagPose = tmp.get();
return Pair.of(
aprilTags
.get(bestTarget.getFiducialId())
tagPose
.transformBy(bestTarget.getBestCameraToTarget().inverse())
.transformBy(cameras.get(lowestAI).getSecond().inverse()),
cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis());
results.get(lowestAI).getLatencyMillis());
}
private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
double smallestHeightDifference = 10e9;
double mili = 0;
Pose3d pose = lastPose;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
// If the map doesn't contain the ID fail
if (!aprilTags.containsKey(target.getFiducialId())) {
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
@@ -194,7 +207,7 @@ public class RobotPoseEstimator {
}
continue;
}
Pose3d targetPose = aprilTags.get(target.getFiducialId());
Pose3d targetPose = tmp.get();
double alternativeDifference =
Math.abs(
p.getSecond().getZ()
@@ -205,16 +218,23 @@ public class RobotPoseEstimator {
- targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
mili = p.getFirst().getLatestResult().getLatencyMillis();
pose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
mili = result.getLatencyMillis();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
mili = p.getFirst().getLatestResult().getLatencyMillis();
pose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
mili = result.getLatencyMillis();
}
}
}
return Pair.of(pose, mili);
}
@@ -223,18 +243,20 @@ public class RobotPoseEstimator {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
false);
return Pair.of(lastPose, 0.);
return Pair.of(null, 0.);
}
double smallestDifference = 10e9;
double mili = 0;
Pose3d pose = lastPose;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
// If the map doesn't contain the ID fail
if (!aprilTags.containsKey(target.getFiducialId())) {
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
@@ -244,26 +266,26 @@ public class RobotPoseEstimator {
}
continue;
}
Pose3d targetPose = aprilTags.get(target.getFiducialId());
double alternativeDifference =
Math.abs(
calculateDifference(
referencePose,
targetPose.transformBy(target.getAlternateCameraToTarget().inverse())));
double bestDifference =
Math.abs(
calculateDifference(
referencePose,
targetPose.transformBy(target.getBestCameraToTarget().inverse())));
Pose3d targetPose = tmp.get();
Pose3d botBestPose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
Pose3d botAltPose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
mili = p.getFirst().getLatestResult().getLatencyMillis();
pose = botAltPose;
mili = result.getLatencyMillis();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
mili = p.getFirst().getLatestResult().getLatencyMillis();
pose = botBestPose;
mili = result.getLatencyMillis();
}
}
}
@@ -277,11 +299,13 @@ public class RobotPoseEstimator {
double totalAmbiguity = 0;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
// If the map doesn't contain the ID fail
if (!aprilTags.containsKey(target.getFiducialId())) {
var tmp = aprilTags.getTagPose(target.getFiducialId());
if (tmp.isEmpty()) {
if (!reportedErrors.contains(target.getFiducialId())) {
DriverStation.reportWarning(
"[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
@@ -291,20 +315,23 @@ public class RobotPoseEstimator {
}
continue;
}
Pose3d targetPose = aprilTags.get(target.getFiducialId());
Pose3d targetPose = tmp.get();
try {
totalAmbiguity += 1. / target.getPoseAmbiguity();
} catch (ArithmeticException e) {
// A total ambiguity of zero exists, using that pose instead!",
return Pair.of(
targetPose.transformBy(target.getBestCameraToTarget().inverse()),
p.getFirst().getLatestResult().getLatencyMillis());
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
result.getLatencyMillis());
}
tempPoses.add(
Pair.of(
targetPose.transformBy(target.getBestCameraToTarget().inverse()),
Pair.of(
target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis())));
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
}
}
@@ -312,20 +339,25 @@ public class RobotPoseEstimator {
Rotation3d rotation = new Rotation3d();
double latency = 0;
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
try {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
} catch (ArithmeticException e) {
DriverStation.reportWarning(
"[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
false);
return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
if (tempPoses.size() > 0) {
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
try {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency +=
pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
} catch (ArithmeticException e) {
DriverStation.reportWarning(
"[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
false);
return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
}
}
return Pair.of(new Pose3d(transform, rotation), latency);
} else {
return new Pair<Pose3d, Double>(null, -1.0);
}
return Pair.of(new Pose3d(transform, rotation), latency);
}
/**
@@ -338,12 +370,12 @@ public class RobotPoseEstimator {
}
/** @param aprilTags the aprilTags to set */
public void setAprilTags(Map<Integer, Pose3d> aprilTags) {
public void setAprilTags(AprilTagFieldLayout aprilTags) {
this.aprilTags = aprilTags;
}
/** @return the aprilTags */
public Map<Integer, Pose3d> getAprilTags() {
public AprilTagFieldLayout getAprilTags() {
return aprilTags;
}
@@ -371,6 +403,15 @@ public class RobotPoseEstimator {
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
this.referencePose = new Pose3d(referencePose);
}
/**
* UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
*