From c161992998366fbae42ea70ab11c6a1a7e682c7f Mon Sep 17 00:00:00 2001 From: "Ryan Lee (SW-GPU)" Date: Thu, 19 Jan 2023 13:50:29 -0800 Subject: [PATCH 1/5] Reduce calculation in strategy functions --- .../org/photonvision/PhotonPoseEstimator.java | 193 ++++++++++-------- .../cpp/photonlib/RobotPoseEstimator.cpp | 51 ++--- 2 files changed, 131 insertions(+), 113 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 57389bf0fe..fefb9e8a65 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -41,12 +41,17 @@ import org.photonvision.targeting.PhotonTrackedTarget; /** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set + * The PhotonPoseEstimator class filters or combines readings from all the + * AprilTags visible at a + * given timestamp on the field to produce a single robot in field pose, using + * the strategy set * below. Example usage can be found in our apriltagExample example project. */ public class PhotonPoseEstimator { - /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */ + /** + * Position estimation strategies that can be used by the + * {@link PhotonPoseEstimator} class. + */ public enum PoseStrategy { /** Choose the Pose with the lowest ambiguity. */ LOWEST_AMBIGUITY, @@ -76,16 +81,20 @@ public enum PoseStrategy { /** * Create a new PhotonPoseEstimator. * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCameras and - * @param robotToCamera Transform3d from the center of the robot to the camera mount positions - * (ie, robot ➔ camera) in the Robot - * Coordinate System. + * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag + * IDs to Pose3d objects + * with respect to the FIRST field using the Field + * Coordinate System. + * @param strategy The strategy it should use to determine the best pose. + * @param camera PhotonCameras and + * @param robotToCamera Transform3d from the center of the robot to the camera + * mount positions + * (ie, robot ➔ camera) in the Robot + * Coordinate System. */ public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, @@ -144,7 +153,8 @@ public Pose3d getReferencePose() { } /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE + * Update the stored reference pose for use when using the + * CLOSEST_TO_REFERENCE_POSE * strategy. * * @param referencePose the referencePose to set @@ -154,7 +164,8 @@ public void setReferencePose(Pose3d referencePose) { } /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE + * Update the stored reference pose for use when using the + * CLOSEST_TO_REFERENCE_POSE * strategy. * * @param referencePose the referencePose to set @@ -164,7 +175,8 @@ public void setReferencePose(Pose2d referencePose) { } /** - * Update the stored last pose. Useful for setting the initial estimate when using the + * Update the stored last pose. Useful for setting the initial estimate when + * using the * CLOSEST_TO_LAST_POSE strategy. * * @param lastPose the lastPose to set @@ -174,7 +186,8 @@ public void setLastPose(Pose3d lastPose) { } /** - * Update the stored last pose. Useful for setting the initial estimate when using the + * Update the stored last pose. Useful for setting the initial estimate when + * using the * CLOSEST_TO_LAST_POSE strategy. * * @param lastPose the lastPose to set @@ -184,11 +197,13 @@ public void setLastPose(Pose2d lastPose) { } /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns + * Poll data from the configured cameras and update the estimated position of + * the robot. Returns * empty if there are no cameras set or no targets were found from the cameras. * - * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and - * pipeline results used to create the estimate + * @return an EstimatedRobotPose with an estimated pose, and information about + * the camera(s) and + * pipeline results used to create the estimate */ public Optional update() { if (camera == null) { @@ -231,12 +246,14 @@ public Optional update() { } /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of + * Return the estimated position of the robot with the lowest position ambiguity + * from a List of * pipeline results. * * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. + * @return the estimated position of the robot in the FCS and the estimated + * timestamp of this + * estimation. */ private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { PhotonTrackedTarget lowestAmbiguityTarget = null; @@ -254,7 +271,8 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul // Although there are confirmed to be targets, none of them may be fiducial // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); + if (lowestAmbiguityTarget == null) + return Optional.empty(); int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); @@ -275,12 +293,14 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul } /** - * Return the estimated position of the robot using the target with the lowest delta height + * 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. + * @return the estimated position of the robot in the FCS and the estimated + * timestamp of this + * estimation. */ private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { double smallestHeightDifference = 10e9; @@ -292,7 +312,8 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) continue; + if (targetFiducialId == -1) + continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -301,41 +322,35 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin continue; } - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); - } + double alternateTransformDelta = Math.abs( + robotToCamera.getZ() + - targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .getZ()); + double bestTransformDelta = Math.abs( + robotToCamera.getZ() + - targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .getZ()); - if (bestTransformDelta < smallestHeightDifference) { + if (alternateTransformDelta < smallestHeightDifference && alternateTransformDelta < bestTransformDelta) { + smallestHeightDifference = alternateTransformDelta; + closestHeightTarget = new EstimatedRobotPose( + targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()), + result.getTimestampSeconds()); + } else if (bestTransformDelta < smallestHeightDifference) { smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); + closestHeightTarget = new EstimatedRobotPose( + targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()), + result.getTimestampSeconds()); } } @@ -344,13 +359,16 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin } /** - * Return the estimated position of the robot using the target with the lowest delta in the vector + * 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 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. + * @param result pipeline result + * @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. */ private Optional closestToReferencePoseStrategy( PhotonPipelineResult result, Pose3d referencePose) { @@ -370,7 +388,8 @@ private Optional closestToReferencePoseStrategy( // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) continue; + if (targetFiducialId == -1) + continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -379,29 +398,24 @@ private Optional closestToReferencePoseStrategy( continue; } - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); + Pose3d altTransformPosition = targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); + Pose3d bestTransformPosition = targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - if (altDifference < smallestPoseDelta) { + if (altDifference < smallestPoseDelta && altDifference < bestDifference) { smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds()); - } - if (bestDifference < smallestPoseDelta) { + lowestDeltaPose = new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds()); + } else if (bestDifference < smallestPoseDelta) { smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds()); + lowestDeltaPose = new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds()); } } return Optional.ofNullable(lowestDeltaPose); @@ -411,8 +425,9 @@ private Optional closestToReferencePoseStrategy( * 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. + * @return the estimated position of the robot in the FCS and the estimated + * timestamp of this + * estimation. */ private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { List> estimatedRobotPoses = new ArrayList<>(); @@ -424,7 +439,8 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) continue; + if (targetFiducialId == -1) + continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -462,7 +478,8 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe Translation3d transform = new Translation3d(); Rotation3d rotation = new Rotation3d(); - if (estimatedRobotPoses.isEmpty()) return Optional.empty(); + if (estimatedRobotPoses.isEmpty()) + return Optional.empty(); for (Pair pair : estimatedRobotPoses) { // Total ambiguity is non-zero confirmed because if it was zero, that pose was diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp index be0e93e101..ed759fdcc6 100644 --- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp @@ -153,22 +153,23 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() { continue; } frc::Pose3d targetPose = fiducialPose.value(); - units::meter_t alternativeDifference = units::math::abs( - p.second.Z() - - targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .Z()); - units::meter_t bestDifference = units::math::abs( - p.second.Z() - - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z()); - if (alternativeDifference < smallestHeightDifference) { + auto altCameraTransform = + targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()); + units::meter_t alternativeDifference = + units::math::abs(p.second.Z() - altCameraTransform.Z()); + auto bestCameraTransform = + targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); + units::meter_t bestDifference = + units::math::abs(p.second.Z() - bestCameraTransform.Z()); + + if (alternativeDifference < smallestHeightDifference && + alternativeDifference < bestDifference) { smallestHeightDifference = alternativeDifference; - pose = targetPose.TransformBy( - target.GetAlternateCameraToTarget().Inverse()); + pose = altCameraTransform; stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - if (bestDifference < smallestHeightDifference) { + } else if (bestDifference < smallestHeightDifference) { smallestHeightDifference = bestDifference; - pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); + pose = bestCameraTransform; stateTimestamp = p.first->GetLatestResult().GetTimestamp(); } } @@ -198,25 +199,25 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() { continue; } frc::Pose3d targetPose = fiducialPose.value(); + auto altCameraTransform = + targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()); units::meter_t alternativeDifference = units::math::abs(referencePose.Translation().Distance( - targetPose - .TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .Translation())); + altCameraTransform.Translation())); + auto bestCameraTansform = + targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); units::meter_t bestDifference = units::math::abs(referencePose.Translation().Distance( - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .Translation())); - if (alternativeDifference < smallestDifference) { + bestCameraTansform.Translation())); + + if (alternativeDifference < smallestDifference && + alternativeDifference < bestDifference) { smallestDifference = alternativeDifference; - pose = targetPose.TransformBy( - target.GetAlternateCameraToTarget().Inverse()); + pose = altCameraTransform; stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - - if (bestDifference < smallestDifference) { + } else if (bestDifference < smallestDifference) { smallestDifference = bestDifference; - pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); + pose = besttCameraTransform; stateTimestamp = p.first->GetLatestResult().GetTimestamp(); } } From f406d3cbbd08ea918b6f4edacac99462125a9750 Mon Sep 17 00:00:00 2001 From: "Ryan Lee (SW-GPU)" Date: Fri, 20 Jan 2023 01:45:51 -0800 Subject: [PATCH 2/5] fix clang format issue --- .../org/photonvision/PhotonPoseEstimator.java | 185 ++++++++---------- 1 file changed, 83 insertions(+), 102 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index fefb9e8a65..1e04ecc41e 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -41,17 +41,12 @@ import org.photonvision.targeting.PhotonTrackedTarget; /** - * The PhotonPoseEstimator class filters or combines readings from all the - * AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using - * the strategy set + * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a + * given timestamp on the field to produce a single robot in field pose, using the strategy set * below. Example usage can be found in our apriltagExample example project. */ public class PhotonPoseEstimator { - /** - * Position estimation strategies that can be used by the - * {@link PhotonPoseEstimator} class. - */ + /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */ public enum PoseStrategy { /** Choose the Pose with the lowest ambiguity. */ LOWEST_AMBIGUITY, @@ -81,20 +76,16 @@ public enum PoseStrategy { /** * Create a new PhotonPoseEstimator. * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag - * IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCameras and - * @param robotToCamera Transform3d from the center of the robot to the camera - * mount positions - * (ie, robot ➔ camera) in the Robot - * Coordinate System. + * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects + * with respect to the FIRST field using the Field + * Coordinate System. + * @param strategy The strategy it should use to determine the best pose. + * @param camera PhotonCameras and + * @param robotToCamera Transform3d from the center of the robot to the camera mount positions + * (ie, robot ➔ camera) in the Robot + * Coordinate System. */ public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, @@ -153,8 +144,7 @@ public Pose3d getReferencePose() { } /** - * Update the stored reference pose for use when using the - * CLOSEST_TO_REFERENCE_POSE + * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE * strategy. * * @param referencePose the referencePose to set @@ -164,8 +154,7 @@ public void setReferencePose(Pose3d referencePose) { } /** - * Update the stored reference pose for use when using the - * CLOSEST_TO_REFERENCE_POSE + * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE * strategy. * * @param referencePose the referencePose to set @@ -175,8 +164,7 @@ public void setReferencePose(Pose2d referencePose) { } /** - * Update the stored last pose. Useful for setting the initial estimate when - * using the + * Update the stored last pose. Useful for setting the initial estimate when using the * CLOSEST_TO_LAST_POSE strategy. * * @param lastPose the lastPose to set @@ -186,8 +174,7 @@ public void setLastPose(Pose3d lastPose) { } /** - * Update the stored last pose. Useful for setting the initial estimate when - * using the + * Update the stored last pose. Useful for setting the initial estimate when using the * CLOSEST_TO_LAST_POSE strategy. * * @param lastPose the lastPose to set @@ -197,13 +184,11 @@ public void setLastPose(Pose2d lastPose) { } /** - * Poll data from the configured cameras and update the estimated position of - * the robot. Returns + * Poll data from the configured cameras and update the estimated position of the robot. Returns * empty if there are no cameras set or no targets were found from the cameras. * - * @return an EstimatedRobotPose with an estimated pose, and information about - * the camera(s) and - * pipeline results used to create the estimate + * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and + * pipeline results used to create the estimate */ public Optional update() { if (camera == null) { @@ -246,14 +231,12 @@ public Optional update() { } /** - * Return the estimated position of the robot with the lowest position ambiguity - * from a List of + * Return the estimated position of the robot with the lowest position ambiguity from a List of * pipeline results. * * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated - * timestamp of this - * estimation. + * @return the estimated position of the robot in the FCS and the estimated timestamp of this + * estimation. */ private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { PhotonTrackedTarget lowestAmbiguityTarget = null; @@ -271,8 +254,7 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul // Although there are confirmed to be targets, none of them may be fiducial // targets. - if (lowestAmbiguityTarget == null) - return Optional.empty(); + if (lowestAmbiguityTarget == null) return Optional.empty(); int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); @@ -293,14 +275,12 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul } /** - * Return the estimated position of the robot using the target with the lowest - * delta height + * 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. + * @return the estimated position of the robot in the FCS and the estimated timestamp of this + * estimation. */ private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { double smallestHeightDifference = 10e9; @@ -312,8 +292,7 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) - continue; + if (targetFiducialId == -1) continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -322,35 +301,40 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin continue; } - double alternateTransformDelta = Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference && alternateTransformDelta < bestTransformDelta) { + double alternateTransformDelta = + Math.abs( + robotToCamera.getZ() + - targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .getZ()); + double bestTransformDelta = + Math.abs( + robotToCamera.getZ() + - targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .getZ()); + + if (alternateTransformDelta < smallestHeightDifference + && alternateTransformDelta < bestTransformDelta) { smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); + closestHeightTarget = + new EstimatedRobotPose( + targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()), + result.getTimestampSeconds()); } else if (bestTransformDelta < smallestHeightDifference) { smallestHeightDifference = bestTransformDelta; - closestHeightTarget = new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds()); + closestHeightTarget = + new EstimatedRobotPose( + targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()), + result.getTimestampSeconds()); } } @@ -359,16 +343,13 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin } /** - * Return the estimated position of the robot using the target with the lowest - * delta in the vector + * 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 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. + * @param result pipeline result + * @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. */ private Optional closestToReferencePoseStrategy( PhotonPipelineResult result, Pose3d referencePose) { @@ -388,8 +369,7 @@ private Optional closestToReferencePoseStrategy( // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) - continue; + if (targetFiducialId == -1) continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -398,24 +378,28 @@ private Optional closestToReferencePoseStrategy( continue; } - Pose3d altTransformPosition = targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); + Pose3d altTransformPosition = + targetPosition + .get() + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); + Pose3d bestTransformPosition = + targetPosition + .get() + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(robotToCamera.inverse()); double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); if (altDifference < smallestPoseDelta && altDifference < bestDifference) { smallestPoseDelta = altDifference; - lowestDeltaPose = new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds()); + lowestDeltaPose = + new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds()); } else if (bestDifference < smallestPoseDelta) { smallestPoseDelta = bestDifference; - lowestDeltaPose = new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds()); + lowestDeltaPose = + new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds()); } } return Optional.ofNullable(lowestDeltaPose); @@ -425,9 +409,8 @@ private Optional closestToReferencePoseStrategy( * 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. + * @return the estimated position of the robot in the FCS and the estimated timestamp of this + * estimation. */ private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { List> estimatedRobotPoses = new ArrayList<>(); @@ -439,8 +422,7 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe // Don't report errors for non-fiducial targets. This could also be resolved by // adding -1 to // the initial HashSet. - if (targetFiducialId == -1) - continue; + if (targetFiducialId == -1) continue; Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); @@ -478,8 +460,7 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe Translation3d transform = new Translation3d(); Rotation3d rotation = new Rotation3d(); - if (estimatedRobotPoses.isEmpty()) - return Optional.empty(); + if (estimatedRobotPoses.isEmpty()) return Optional.empty(); for (Pair pair : estimatedRobotPoses) { // Total ambiguity is non-zero confirmed because if it was zero, that pose was From 202a9e8e8bdb28dfa2d96ec8b47bd274ac2e1d07 Mon Sep 17 00:00:00 2001 From: Ryan Lee Date: Fri, 20 Jan 2023 03:01:58 -0800 Subject: [PATCH 3/5] Apply suggestions from code review --- .../main/java/org/photonvision/PhotonPoseEstimator.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 1e04ecc41e..73a6180061 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -77,14 +77,14 @@ public enum PoseStrategy { * Create a new PhotonPoseEstimator. * * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field + * with respect to the FIRST field using the Field * Coordinate System. * @param strategy The strategy it should use to determine the best pose. * @param camera PhotonCameras and * @param robotToCamera Transform3d from the center of the robot to the camera mount positions - * (ie, robot ➔ camera) in the Robot + * (ie, robot ➔ camera) in the Robot * Coordinate System. */ public PhotonPoseEstimator( From ebb2170ef2222a0aefed9f5e31d80a0bb868a4f0 Mon Sep 17 00:00:00 2001 From: Ryan Lee Date: Fri, 20 Jan 2023 10:55:06 -0800 Subject: [PATCH 4/5] Update photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp --- photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp index ed759fdcc6..0cc4a17efe 100644 --- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp @@ -217,7 +217,7 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() { stateTimestamp = p.first->GetLatestResult().GetTimestamp(); } else if (bestDifference < smallestDifference) { smallestDifference = bestDifference; - pose = besttCameraTransform; + pose = bestCameraTransform; stateTimestamp = p.first->GetLatestResult().GetTimestamp(); } } From 8d0e896516accc80c5c56d03bdc25ab88b346c79 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 15 Oct 2023 11:58:47 -0400 Subject: [PATCH 5/5] Formatting fixes --- .../src/main/java/org/photonvision/PhotonPoseEstimator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 50ff421835..b1e70d468f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -486,7 +486,7 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .getZ()); if (alternateTransformDelta < smallestHeightDifference - && alternateTransformDelta < bestTransformDelta) { + && alternateTransformDelta < bestTransformDelta) { smallestHeightDifference = alternateTransformDelta; closestHeightTarget = new EstimatedRobotPose( @@ -496,7 +496,7 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), result.getTargets()); - } else if(bestTransformDelta < smallestHeightDifference) { + } else if (bestTransformDelta < smallestHeightDifference) { smallestHeightDifference = bestTransformDelta; closestHeightTarget = new EstimatedRobotPose(