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

Open up pose estimator strategy methods (#2252)

Browse Source
This commit is contained in:
Gold856
2026-01-11 13:58:53 -05:00
committed by GitHub
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -43,6 +43,20 @@ public class EstimatedRobotPose {
/** The strategy actually used to produce this pose */
public final PoseStrategy strategy;
/**
* Constructs an EstimatedRobotPose
*
* @param estimatedPose estimated pose
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = null;
}
/**
* Constructs an EstimatedRobotPose
*

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

@@ -117,7 +117,7 @@ public class PhotonPoseEstimator {
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error const.
* heading estimate against the tag corner reprojection error cost.
*/
public static final record ConstrainedSolvepnpParams(
boolean headingFree, double headingScaleFactor) {}
@@ -144,12 +144,35 @@ public class PhotonPoseEstimator {
* "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>.
*/
public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
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;
@@ -221,7 +244,9 @@ public class PhotonPoseEstimator {
* 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;
}
@@ -230,7 +255,9 @@ public class PhotonPoseEstimator {
* 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);
@@ -246,7 +273,9 @@ public class PhotonPoseEstimator {
* 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
@@ -262,7 +291,9 @@ public class PhotonPoseEstimator {
* 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;
}
@@ -272,7 +303,9 @@ public class PhotonPoseEstimator {
* 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;
@@ -283,7 +316,9 @@ public class PhotonPoseEstimator {
* 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));
}
@@ -293,7 +328,9 @@ public class PhotonPoseEstimator {
* <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;
}
@@ -303,7 +340,9 @@ public class PhotonPoseEstimator {
* <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));
}
@@ -389,9 +428,11 @@ public class PhotonPoseEstimator {
* 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
* @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());
}
@@ -411,9 +452,11 @@ public class PhotonPoseEstimator {
* 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
* @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,
@@ -436,9 +479,11 @@ public class PhotonPoseEstimator {
* @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
* @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,
@@ -475,7 +520,7 @@ public class PhotonPoseEstimator {
*
* @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
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
@@ -491,21 +536,122 @@ public class PhotonPoseEstimator {
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
case LOWEST_AMBIGUITY -> lowestAmbiguityStrategy(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> closestToCameraHeightStrategy(cameraResult);
case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
case CLOSEST_TO_REFERENCE_POSE ->
closestToReferencePoseStrategy(cameraResult, referencePose);
estimateClosestToReferencePose(cameraResult, referencePose);
case CLOSEST_TO_LAST_POSE -> {
setReferencePose(lastPose);
yield closestToReferencePoseStrategy(cameraResult, referencePose);
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;
}
case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
case MULTI_TAG_PNP_ON_RIO ->
multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
case CONSTRAINED_SOLVEPNP ->
constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
};
if (estimatedPose.isPresent()) {
@@ -515,12 +661,43 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.getBestTarget();
/**
* @param cameraResult A pipeline result from the camera.
* @return Whether or not pose estimation should be performed.
*/
private boolean shouldEstimate(PhotonPipelineResult cameraResult) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return false;
}
// If no targets seen, trivial case -- can't do estimation
return cameraResult.hasTargets();
}
/**
* Return the estimated position of the robot by using distance data from best visible tag to
* compute a Pose. This runs on the RoboRIO in order to access the robot's yaw heading, and MUST
* have addHeadingData called every frame so heading data is up-to-date.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget bestTarget = cameraResult.getBestTarget();
if (bestTarget == null) return Optional.empty();
var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds());
var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds());
if (headingSampleOpt.isEmpty()) {
return Optional.empty();
}
@@ -555,98 +732,82 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(robotPose),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
}
private Optional<EstimatedRobotPose> constrainedPnpStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
/**
* Return the estimated position of the robot by solving a constrained version of the
* Perspective-n-Point problem with the robot's drivebase flat on the floor. This computation
* takes place on the RoboRIO, and typically takes not more than 2ms. See {@link
* org.photonvision.jni.ConstrainedSolvepnpJni} for tuning handles this strategy exposes.
* Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
* heading error * heading scale factor. This strategy needs addHeadingData called every frame so
* heading data is up-to-date.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param seedPose An initial guess at robot pose, refined via optimization. Better guesses will
* converge faster. Can come from any pose source, but some battle-tested sources are {@link
* #estimateCoprocMultiTagPose(PhotonPipelineResult)}, or {@link
* #estimateLowestAmbiguityPose(PhotonPipelineResult)} if MultiTag results aren't available.
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error cont.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
PhotonPipelineResult cameraResult,
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
Pose3d seedPose,
boolean headingFree,
double headingScaleFactor) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (result.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
result.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(
result,
cameraMatrixOpt,
distCoeffsOpt,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
return 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(result.getTimestampSeconds()).get()));
}
var pnpResult =
VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
cameraMatrixOpt.get(),
distCoeffsOpt.get(),
result.getTargets(),
cameraMatrix,
distCoeffs,
cameraResult.getTargets(),
robotToCamera,
fieldToRobotSeed,
seedPose,
fieldTags,
tagModel,
constrainedPnpParams.get().headingFree,
headingBuffer.getSample(result.getTimestampSeconds()).get(),
constrainedPnpParams.get().headingScaleFactor);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
headingFree,
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get(),
headingScaleFactor);
if (!pnpResult.isPresent()) return Optional.empty();
var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CONSTRAINED_SOLVEPNP));
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().isEmpty()) {
return update(result, this.multiTagFallbackStrategy);
/**
* Return the estimated position of the robot by using all visible tags to compute a single pose
* estimate on coprocessor. This option needs to be enabled on the PhotonVision web UI as well.
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (cameraResult.getMultiTagResult().isEmpty() || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best_tf = cameraResult.getMultiTagResult().get().estimatedPose.best;
var best =
Pose3d.kZero
.plus(best_tf) // field-to-camera
@@ -655,33 +816,31 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
return update(result, this.multiTagFallbackStrategy);
}
if (result.getTargets().size() < 2) {
return update(result, this.multiTagFallbackStrategy);
/**
* Return the estimated position of the robot by using all visible tags to compute a single pose
* estimate on the RoboRIO. This can take a lot of time due to the RIO's weak computing power.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
if (cameraResult.getTargets().size() < 2 || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
cameraMatrix, distCoeffs, cameraResult.getTargets(), fieldTags, tagModel);
if (!pnpResult.isPresent()) return Optional.empty();
var best =
Pose3d.kZero
@@ -691,25 +850,29 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
* Return the estimated position of the robot with the lowest position ambiguity from a pipeline
* result.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget lowestAmbiguityTarget = null;
double lowestAmbiguityScore = 10;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
double targetPoseAmbiguity = target.getPoseAmbiguity();
// Make sure the target is a Fiducial target.
if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
@@ -737,8 +900,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}
@@ -746,15 +909,19 @@ public class PhotonPoseEstimator {
* Return the estimated position of the robot using the target with the lowest delta height
* difference between the estimated and actual height of the camera.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
double smallestHeightDifference = 10e9;
EstimatedRobotPose closestHeightTarget = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -792,8 +959,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
@@ -805,8 +972,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}
@@ -819,13 +986,16 @@ public class PhotonPoseEstimator {
* Return the estimated position of the robot using the target with the lowest delta in the vector
* magnitude between it and the reference pose.
*
* @param result pipeline result
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference against.
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
PhotonPipelineResult result, Pose3d referencePose) {
public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
PhotonPipelineResult cameraResult, Pose3d referencePose) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
if (referencePose == null) {
DriverStation.reportError(
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
@@ -836,7 +1006,7 @@ public class PhotonPoseEstimator {
double smallestPoseDelta = 10e9;
EstimatedRobotPose lowestDeltaPose = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -870,8 +1040,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
@@ -879,8 +1049,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
@@ -890,15 +1060,19 @@ public class PhotonPoseEstimator {
/**
* Return the average of the best target poses using ambiguity as weight.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -923,8 +1097,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
@@ -958,8 +1132,8 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}

325
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -24,13 +24,9 @@
#include "photon/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <optional>
#include <span>
#include <string>
#include <utility>
#include <vector>
@@ -46,6 +42,7 @@
#include <units/angle.h>
#include <units/math.h>
#include <units/time.h>
#include <wpi/deprecated.h>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -55,7 +52,7 @@
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
WPI_IGNORE_DEPRECATED
namespace photon {
namespace detail {
@@ -67,6 +64,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat,
frc::Transform3d robotToCamera)
@@ -138,39 +148,94 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
switch (strategy) {
case LOWEST_AMBIGUITY:
ret = LowestAmbiguityStrategy(result);
ret = EstimateLowestAmbiguityPose(result);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
ret = ClosestToCameraHeightStrategy(result);
ret = EstimateClosestToCameraHeightPose(result);
break;
case CLOSEST_TO_REFERENCE_POSE:
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
ret = EstimateAverageBestTargetsPose(result);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret = MultiTagOnCoprocStrategy(result);
if (!result.MultiTagResult()) {
ret = Update(result, this->multiTagFallbackStrategy);
} else {
ret = EstimateCoprocMultiTagPose(result);
}
break;
case MULTI_TAG_PNP_ON_RIO:
if (cameraMatrixData && cameraDistCoeffs) {
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
} else {
if (!cameraMatrixData && !cameraDistCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!",
"");
ret = Update(result, this->multiTagFallbackStrategy);
}
ret =
EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
if (!ret) {
ret = Update(result, this->multiTagFallbackStrategy);
}
break;
case CONSTRAINED_SOLVEPNP:
ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams);
case CONSTRAINED_SOLVEPNP: {
using namespace frc;
if (!cameraMatrixData || !cameraDistCoeffs) {
FRC_ReportError(
frc::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
break;
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
ret = EstimateConstrainedSolvepnpPose(
result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
constrainedPnpParams->headingFree,
constrainedPnpParams->headingScalingFactor);
if (!ret) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
}
break;
}
case PNP_DISTANCE_TRIG_SOLVE:
ret = PnpDistanceTrigSolveStrategy(result);
ret = EstimatePnpDistanceTrigSolvePose(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -184,10 +249,26 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return ret;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) {
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return false;
}
// If no targets seen, trivial case -- can't do estimation
return result.HasTargets();
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
auto foundIt = targets.end();
for (auto it = targets.begin(); it != targets.end(); ++it) {
if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
@@ -214,18 +295,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
return EstimatedRobotPose{
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToCameraHeightStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
for (auto& target : result.GetTargets()) {
for (auto& target : cameraResult.GetTargets()) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
@@ -250,14 +334,16 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -265,14 +351,17 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToReferencePoseStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -298,17 +387,17 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = altPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = bestPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
}
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(),
CLOSEST_TO_REFERENCE_POSE};
}
@@ -361,41 +450,31 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (!result.MultiTagResult()) {
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (!cameraResult.MultiTagResult() || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"PhotonPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix cameraMatrix,
PhotonCamera::DistortionMatrix distCoeffs) {
// Need at least 2 targets
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
if (cameraResult.GetTargets().size() < 2 || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
auto const targets = result.GetTargets();
const auto targets = cameraResult.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
// (pixels).
@@ -418,7 +497,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
// We should only do multi-tag if at least 2 tags (* 4 corners/tag)
if (imagePoints.size() < 8) {
return Update(result, this->multiTagFallbackStrategy);
return std::nullopt;
}
// Output mats for results
@@ -426,27 +505,31 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
{
cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
cv::eigen2cv(*camMat, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
cv::Mat cameraMatCV(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
cv::eigen2cv(cameraMatrix, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
cv::eigen2cv(distCoeffs, distCoeffsMatCV);
cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
tvec, false, cv::SOLVEPNP_SQPNP);
}
const Pose3d pose = detail::ToPose3d(tvec, rvec);
const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(),
MULTI_TAG_PNP_ON_RIO);
return photon::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
std::optional<frc::Rotation2d> headingSampleOpt =
headingBuffer.Sample(result.GetTimestamp());
headingBuffer.Sample(cameraResult.GetTimestamp());
if (!headingSampleOpt) {
FRC_ReportError(frc::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
@@ -485,17 +568,21 @@ PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
frc::Pose2d robotPose = frc::Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample);
return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -512,13 +599,15 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
std::make_pair(target.GetPoseAmbiguity(),
cameraResult.GetTimestamp())));
}
frc::Translation3d transform = frc::Translation3d();
@@ -532,77 +621,45 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(),
cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"StrPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
if (!headingFree) {
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
if (!constrainedPnpParams.value().headingFree) {
fieldToRobotSeed = frc::Pose3d{
fieldToRobotSeed.Translation(),
frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
}
std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
result.GetTargets().end()};
std::vector<photon::PhotonTrackedTarget> targets{
cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};
std::optional<photon::PnpResult> pnpResult =
VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
constrainedPnpParams->headingFree,
frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
constrainedPnpParams->headingScalingFactor);
cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
aprilTags, photon::kAprilTag36h11, headingFree,
frc::Rotation2d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
headingScaleFactor);
if (!pnpResult) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
return std::nullopt;
}
frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
PoseStrategy::CONSTRAINED_SOLVEPNP};
}
} // namespace photon

252
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -24,8 +24,6 @@
#pragma once
#include <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
@@ -34,11 +32,8 @@
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
enum PoseStrategy {
@@ -93,10 +88,26 @@ class PhotonPoseEstimator {
*
* @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 robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* @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 robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
* @deprecated Use individual estimation methods with the 2 argument
* constructor instead.
*/
[[deprecated(
"Use individual estimation methods with the 2 argument constructor "
"instead.")]]
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
@@ -112,14 +123,20 @@ class PhotonPoseEstimator {
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
[[deprecated("Use individual estimation methods instead.")]]
PoseStrategy GetPoseStrategy() const {
return strategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
@@ -132,22 +149,30 @@ class PhotonPoseEstimator {
* 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("Use individual estimation methods instead.")]]
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
[[deprecated("Use individual estimation methods instead.")]]
frc::Pose3d GetReferencePose() const {
return referencePose;
}
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetReferencePose(frc::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
@@ -178,8 +203,12 @@ class PhotonPoseEstimator {
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(frc::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
* Add robot heading data to the buffer. Must be called periodically for the
@@ -247,7 +276,9 @@ class PhotonPoseEstimator {
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
std::optional<EstimatedRobotPose> Update(
const photon::PhotonPipelineResult& result,
std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
@@ -257,6 +288,127 @@ class PhotonPoseEstimator {
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
std::nullopt);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
/**
* Return the estimated position of the robot by using all visible tags to
* compute a single pose estimate on coprocessor. This option needs to be
* enabled on the PhotonVision web UI as well.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot by using all visible tags to
* compute a single pose estimate on the RoboRIO. This can take a lot of time
* due to the RIO's weak computing power.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
PhotonCamera::DistortionMatrix distCoeffs);
/**
* Return the estimated position of the robot by using distance data from best
* visible tag to compute a Pose. This runs on the RoboRIO in order to access
* the robot's yaw heading, and MUST have AddHeadingData called every frame so
* heading data is up-to-date.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult);
/**
* Return the average of the best target poses using ambiguity as weight.
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot by solving a constrained version
* of the Perspective-n-Point problem with the robot's drivebase flat on the
* floor. This computation takes place on the RoboRIO, and typically takes not
* more than 2ms. See
* photon::VisionEstimation::EstimateRobotPoseConstrainedSolvePNP for tuning
* handles this strategy exposes. Internally, the cost function is a
* sum-squared of pixel reprojection error + (optionally) heading error *
* heading scale factor. This strategy needs addHeadingData called every frame
* so heading data is up-to-date.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param seedPose An initial guess at robot pose, refined via optimization.
* Better guesses will converge faster. Can come from any pose source, but
* some battle-tested sources are estimateCoprocMultiTagPose, or
* estimateLowestAmbiguityPose if MultiTag results aren't available.
* @param headingFree If true, heading is completely free to vary. If false,
* heading excursions from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of
* changing our robot heading estimate against the tag corner reprojection
* error cost.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor);
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
@@ -280,9 +432,9 @@ class PhotonPoseEstimator {
* 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 cameraResult A pipeline result from the camera.
* @param strategy The pose strategy to use
* @return an EstimatedRobotPose with an estimated pose, timestamp, and
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
@@ -296,84 +448,6 @@ class PhotonPoseEstimator {
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
/**
* Return the pose calculation using the best visible tag and the robot's
* heading
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation
*/
std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
PhotonPipelineResult result);
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
};
} // namespace photon