diff --git a/docs/source/docs/programming/photonlib/robot-pose-estimator.md b/docs/source/docs/programming/photonlib/robot-pose-estimator.md index daa4446e0..3bb551bb9 100644 --- a/docs/source/docs/programming/photonlib/robot-pose-estimator.md +++ b/docs/source/docs/programming/photonlib/robot-pose-estimator.md @@ -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 `estimatePose()` methods on your `PhotonPoseEstimator` will return an `Optional`, 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 `estimatePose()` methods on your `PhotonPoseEstimator` will return an `Optional`, 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`. diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index b820cdb44..2b560abe7 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -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 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 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 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 estimateRioMultiTagPose( PhotonPipelineResult cameraResult, Matrix cameraMatrix, Matrix 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 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 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 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 estimateAverageBestTargetsPose( PhotonPipelineResult cameraResult) { diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index fd9c1ea17..2e0b34a9b 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -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 targets{ cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()}; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index d5c2501ec..8e3cc8ffc 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -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 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 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 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 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 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 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 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 EstimateConstrainedSolvepnpPose( photon::PhotonPipelineResult cameraResult,