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

Multi-tag pnp in robot code (#787)

Browse Source 
---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>
This commit is contained in:
Matt
2023-02-13 17:57:01 -05:00
committed by GitHub
parent a2dfe48679
commit 5b86360b9b
33 changed files with 1785 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -24,6 +24,7 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
@@ -37,8 +38,10 @@ import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -61,11 +64,15 @@ public class PhotonPoseEstimator {
CLOSEST_TO_LAST_POSE,
/** Choose the Pose with the lowest ambiguity. */
AVERAGE_BEST_TARGETS
AVERAGE_BEST_TARGETS,
/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP
}
private AprilTagFieldLayout fieldTags;
private PoseStrategy strategy;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private final PhotonCamera camera;
private Transform3d robotToCamera;
@@ -93,7 +100,7 @@ public class PhotonPoseEstimator {
PhotonCamera camera,
Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.strategy = strategy;
this.primaryStrategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
}
@@ -121,8 +128,8 @@ public class PhotonPoseEstimator {
*
* @return the strategy
*/
public PoseStrategy getStrategy() {
return strategy;
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
/**
@@ -130,8 +137,23 @@ public class PhotonPoseEstimator {
*
* @param strategy the strategy to set
*/
public void setStrategy(PoseStrategy strategy) {
this.strategy = strategy;
public void setPrimaryStrategy(PoseStrategy strategy) {
this.primaryStrategy = strategy;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
* NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
}
this.multiTagFallbackStrategy = strategy;
}
/**
@@ -229,8 +251,13 @@ public class PhotonPoseEstimator {
return Optional.empty();
}
return update(cameraResult, this.primaryStrategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
switch (strategy) {
switch (strat) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
break;
@@ -245,6 +272,9 @@ public class PhotonPoseEstimator {
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult);
break;
default:
DriverStation.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
@@ -258,6 +288,55 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();
var fieldToCams = new ArrayList<Pose3d>();
var fieldToCamsAlt = new ArrayList<Pose3d>();
if (result.getTargets().size() < 2) {
// Run fallback strategy instead
update(result, this.multiTagFallbackStrategy);
}
for (var target : result.getTargets()) {
visCorners.addAll(target.getDetectedCorners());
Pose3d tagPose = fieldTags.getTagPose(target.getFiducialId()).get();
// actual layout poses of visible tags -- not exposed, so have to recreate
knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse()));
fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
}
var cameraMatrixOpt = camera.getCameraMatrix();
var distCoeffsOpt = camera.getDistCoeffs();
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// multi-target solvePNP
if (hasCalibData) {
var cameraMatrix = cameraMatrixOpt.get();
var distCoeffs = distCoeffsOpt.get();
var pnpResults =
VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
// var alt = new Pose3d()
// .plus(pnpResults.alt) // field-to-camera
// .plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
} else {
// TODO fallback strategy? Should we just always do solvePNP?
return Optional.empty();
}
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
@@ -299,7 +378,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
result.getTimestampSeconds(),
result.getTargets()));
}
/**
@@ -352,7 +432,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
result.getTimestampSeconds(),
result.getTargets());
}
if (bestTransformDelta < smallestHeightDifference) {
@@ -363,7 +444,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
result.getTimestampSeconds(),
result.getTargets());
}
}
@@ -424,12 +506,14 @@ public class PhotonPoseEstimator {
if (altDifference < smallestPoseDelta) {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
new EstimatedRobotPose(
altTransformPosition, result.getTimestampSeconds(), result.getTargets());
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
new EstimatedRobotPose(
bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
}
}
return Optional.ofNullable(lowestDeltaPose);
@@ -471,7 +555,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
result.getTimestampSeconds(),
result.getTargets()));
}
totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -502,7 +587,8 @@ public class PhotonPoseEstimator {
}
return Optional.of(
new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
new EstimatedRobotPose(
new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
}
/**