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

Clarify when estimate methods return empty (#2323)

Browse Source 
## Description

Clarifies when estimate methods return an empty optional. This also
fixes some parity issues with Constrained SolvePnP not checking the
optional returned by the heading buffer.

Resolves #2322.

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2025.3.2
- [ ] If this PR touches pipeline settings or anything related to data
exchange, the frontend typing is updated
- [ ] If this PR addresses a bug, a regression test for it is added
This commit is contained in:
Gold856
2026-01-21 18:13:47 -05:00
committed by GitHub
parent 12f74423d9
commit e73420d62a
4 changed files with 46 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
docs/source/docs/programming/photonlib/robot-pose-estimator.md
View File

@@ -108,7 +108,7 @@ When taking in a result from a `PhotonCamera`, PhotonPoseEstimator offers nine p
flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms.
This also requires addHeadingData to be called every frame so heading data is up to date.
Calling one of the `estimate<strategy>Pose()` methods on your `PhotonPoseEstimator` will return an `Optional<EstimatedRobotPose>`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated. The recommended way to use the estimatePose methods is to
Calling one of the `estimate<strategy>Pose()` methods on your `PhotonPoseEstimator` will return an `Optional<EstimatedRobotPose>`, which will be empty if there are no detected tags, not enough detected tags (for multi-tag strategies), missing data (typically heading data), or if the internal solvers failed (this is a rare scenario). `EstimatedRobotPose` includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated. The recommended way to use the estimatePose methods is to
1. do estimation with one of MultiTag methods, check if the result is empty, then
2. fallback to single tag estimation using a method like `estimateLowestAmbiguityPose`.

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

@@ -686,7 +686,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets or heading data.
*/
public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
@@ -758,7 +758,8 @@ public class PhotonPoseEstimator {
* @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.
* create the estimate, or an empty optional if there's no targets or heading data, or if the
* solver fails to solve the problem.
*/
public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
PhotonPipelineResult cameraResult,
@@ -770,6 +771,18 @@ public class PhotonPoseEstimator {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
// Need heading if heading fixed
if (!headingFree) {
if (headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
return Optional.empty();
} else {
// If heading fixed, force rotation component
seedPose =
new Pose3d(
seedPose.getTranslation(),
new Rotation3d(headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
}
}
var pnpResult =
VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
cameraMatrix,
@@ -799,7 +812,8 @@ public class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets, no multi-tag results, or
* multi-tag is disabled in the web UI.
*/
public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
@@ -829,7 +843,8 @@ public class PhotonPoseEstimator {
* @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.
* create the estimate, or an empty optional if there's less than 2 targets visible or
* SolvePnP fails.
*/
public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
@@ -861,7 +876,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets.
*/
public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
@@ -911,7 +926,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets.
*/
public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
@@ -989,7 +1004,7 @@ public class PhotonPoseEstimator {
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference against.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets.
*/
public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
PhotonPipelineResult cameraResult, Pose3d referencePose) {
@@ -1062,7 +1077,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* create the estimate, or an empty optional if there's no targets.
*/
public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {

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

@@ -634,13 +634,18 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
// Need heading if heading fixed
if (!headingFree) {
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
if (!headingBuffer.Sample(cameraResult.GetTimestamp())) {
return std::nullopt;
} else {
// If heading fixed, force rotation component
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
}
std::vector<photon::PhotonTrackedTarget> targets{
cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};

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

@@ -294,7 +294,7 @@ class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
* targets used to create the estimate, or std::nullopt if there's no targets.
*/
std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult);
@@ -306,7 +306,7 @@ class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp and
* targets used to create the estimate.
* targets used to create the estimate, or std::nullopt if there's no targets.
*/
std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult);
@@ -319,7 +319,7 @@ class PhotonPoseEstimator {
* @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.
* targets used to create the estimate, or std::nullopt if there's no targets.
*/
std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
@@ -331,7 +331,8 @@ class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
* targets used to create the estimate or std::nullopt if there's no targets,
* no multi-tag results, or multi-tag is disabled in the web UI.
*/
std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult);
@@ -345,7 +346,8 @@ class PhotonPoseEstimator {
* @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.
* targets used to create the estimate, or std::nullopt if there's less than 2
* targets visible or SolvePnP fails.
*/
std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
@@ -363,7 +365,8 @@ class PhotonPoseEstimator {
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
* targets used to create the estimate, or std::nullopt if there's no targets
* or heading data.
*/
std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult);
@@ -372,7 +375,7 @@ class PhotonPoseEstimator {
* 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.
* targets used to create the estimate, or std::nullopt if there's no targets.
*/
std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult);
@@ -401,7 +404,8 @@ class PhotonPoseEstimator {
* 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.
* targets used to create the estimate, or std::nullopt if there's no targets
* or heading data, or if the solver fails to solve the problem.
*/
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,