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

[photonlib] Remove deprecated pose estimator methods (#2431)

Browse Source 
We added new API methods in 2026, and deprecated our past methods. This PR removes the deprecated methods.
This commit is contained in:
Sam Freund
2026-04-13 12:04:58 -05:00
committed by GitHub
parent 68fc1e7129
commit e970446c4c
6 changed files with 2 additions and 2330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -45,7 +45,6 @@ import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N8;
import org.wpilib.math.util.Pair;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.camera.OpenCvLoader;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -123,13 +122,7 @@ public class PhotonPoseEstimator {
private AprilTagFieldLayout fieldTags;
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private Transform3d robotToCamera;
private Pose3d lastPose;
private Pose3d referencePose;
protected double poseCacheTimestampSeconds = -1;
private final Set<Integer> reportedErrors = new HashSet<>();
private final TimeInterpolatableBuffer<Rotation2d> headingBuffer =
@@ -156,48 +149,6 @@ public class PhotonPoseEstimator {
InstanceCount++;
}
/**
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
* @deprecated Use individual estimation methods with the 2 argument constructor instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.robotToCamera = robotToCamera;
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
|| strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
OpenCvLoader.forceStaticLoad();
}
InstanceCount++;
HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
}
private void checkUpdate(Object oldObj, Object newObj) {
if (!Objects.equals(oldObj, newObj)) {
invalidatePoseCache();
}
}
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
@@ -217,7 +168,6 @@ public class PhotonPoseEstimator {
* @param fieldTags the AprilTagFieldLayout
*/
public void setFieldTags(AprilTagFieldLayout fieldTags) {
checkUpdate(this.fieldTags, fieldTags);
this.fieldTags = fieldTags;
}
@@ -239,113 +189,6 @@ public class PhotonPoseEstimator {
this.tagModel = tagModel;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO
|| strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) {
OpenCvLoader.forceStaticLoad();
}
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
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
|| strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
}
this.multiTagFallbackStrategy = strategy;
}
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Pose3d getReferencePose() {
return referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose2d lastPose) {
setLastPose(new Pose3d(lastPose));
}
/**
* Add robot heading data to buffer. Must be called periodically for the
* <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
@@ -413,253 +256,6 @@ public class PhotonPoseEstimator {
this.robotToCamera = robotToCamera;
}
/**
* Updates the estimated position of the robot, assuming no camera calibration is required for the
* selected strategy. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* </ul>
*
* Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not
* provided in this overload.
*
* @param cameraResult The latest pipeline result from the camera
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
return update(cameraResult, Optional.empty(), Optional.empty());
}
/**
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the
* other function overload).
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs) {
return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty());
}
/**
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty.
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
}
// If the pose cache timestamp was set, and the result is from the same
// timestamp, return an
// empty result
if (poseCacheTimestampSeconds > 0
&& Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
return Optional.empty();
}
// Remember the timestamp of the current result used
poseCacheTimestampSeconds = cameraResult.getTimestampSeconds();
// If no targets seen, trivial case -- return empty result
if (!cameraResult.hasTargets()) {
return Optional.empty();
}
return update(
cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy);
}
/**
* Internal convenience method for using a fallback strategy for update(). This should only be
* called after timestamp checks have been done by another update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strategy) {
return update(cameraResult, Optional.empty(), Optional.empty(), Optional.empty(), strategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
case CLOSEST_TO_REFERENCE_POSE ->
estimateClosestToReferencePose(cameraResult, referencePose);
case CLOSEST_TO_LAST_POSE -> {
setReferencePose(lastPose);
yield estimateClosestToReferencePose(cameraResult, referencePose);
}
case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult);
case MULTI_TAG_PNP_ON_RIO -> {
if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield update(cameraResult, this.multiTagFallbackStrategy);
}
var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get());
if (res.isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
constrainedPnpParams,
this.multiTagFallbackStrategy);
}
yield res;
}
case MULTI_TAG_PNP_ON_COPROCESSOR -> {
if (cameraResult.getMultiTagResult().isEmpty()) {
yield update(cameraResult, this.multiTagFallbackStrategy);
}
yield estimateCoprocMultiTagPose(cameraResult);
}
case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult);
case CONSTRAINED_SOLVEPNP -> {
boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
yield Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (cameraResult.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
cameraResult
.getMultiTagResult()
.get()
.estimatedPose
.best
.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
yield Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
}
var pnpResult =
estimateConstrainedSolvepnpPose(
cameraResult,
cameraMatrix.get(),
distCoeffs.get(),
fieldToRobotSeed,
constrainedPnpParams.get().headingFree,
constrainedPnpParams.get().headingScaleFactor);
if (!pnpResult.isPresent()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
yield pnpResult;
}
};
if (estimatedPose.isPresent()) {
lastPose = estimatedPose.get().estimatedPose;
}
return estimatedPose;
}
/**
* @param cameraResult A pipeline result from the camera.
* @return Whether or not pose estimation should be performed.