diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index 7d9f9b6dcd..a355ab00f3 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -26,6 +26,7 @@ import edu.wpi.first.math.geometry.Pose3d; import java.util.List; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; /** An estimated pose based on pipeline result */ @@ -39,6 +40,9 @@ public class EstimatedRobotPose { /** A list of the targets used to compute this pose */ public final List targetsUsed; + /** The strategy actually used to produce this pose */ + public final PoseStrategy strategy; + /** * Constructs an EstimatedRobotPose * @@ -46,9 +50,13 @@ public class EstimatedRobotPose { * @param timestampSeconds timestamp of the estimate */ public EstimatedRobotPose( - Pose3d estimatedPose, double timestampSeconds, List targetsUsed) { + Pose3d estimatedPose, + double timestampSeconds, + List targetsUsed, + PoseStrategy strategy) { this.estimatedPose = estimatedPose; this.timestampSeconds = timestampSeconds; this.targetsUsed = targetsUsed; + this.strategy = strategy; } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 0eecc02880..a71acf8946 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -69,7 +69,10 @@ public enum PoseStrategy { AVERAGE_BEST_TARGETS, /** Use all visible tags to compute a single pose estimate.. */ - MULTI_TAG_PNP + MULTI_TAG_PNP_ON_COPROCESSOR, + + /** Use all visible tags to compute a single pose estimate.. */ + MULTI_TAG_PNP_ON_RIO } private AprilTagFieldLayout fieldTags; @@ -173,7 +176,8 @@ public void setPrimaryStrategy(PoseStrategy strategy) { */ public void setMultiTagFallbackStrategy(PoseStrategy strategy) { checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP) { + if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR + || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { DriverStation.reportWarning( "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); strategy = PoseStrategy.LOWEST_AMBIGUITY; @@ -357,8 +361,11 @@ private Optional update( case AVERAGE_BEST_TARGETS: estimatedPose = averageBestTargetsStrategy(cameraResult); break; - case MULTI_TAG_PNP: - estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs); + case MULTI_TAG_PNP_ON_RIO: + estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); + break; + case MULTI_TAG_PNP_ON_COPROCESSOR: + estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); break; default: DriverStation.reportError( @@ -373,7 +380,28 @@ private Optional update( return estimatedPose; } - private Optional multiTagPNPStrategy( + private Optional multiTagOnCoprocStrategy( + PhotonPipelineResult result, + Optional> cameraMatrixOpt, + Optional> distCoeffsOpt) { + if (result.getMultiTagResult().estimatedPose.isPresent) { + var best_tf = result.getMultiTagResult().estimatedPose.best; + var best = + new Pose3d() + .plus(best_tf) // field-to-camera + .plus(robotToCamera.inverse()); // field-to-robot + return Optional.of( + new EstimatedRobotPose( + best, + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); + } else { + return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + } + } + + private Optional multiTagOnRioStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { @@ -395,7 +423,11 @@ private Optional multiTagPNPStrategy( .plus(robotToCamera.inverse()); // field-to-robot return Optional.of( - new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets())); + new EstimatedRobotPose( + best, + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.MULTI_TAG_PNP_ON_RIO)); } /** @@ -440,7 +472,8 @@ private Optional lowestAmbiguityStrategy(PhotonPipelineResul .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets())); + result.getTargets(), + PoseStrategy.LOWEST_AMBIGUITY)); } /** @@ -494,7 +527,8 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .transformBy(target.getAlternateCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets()); + result.getTargets(), + PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); } if (bestTransformDelta < smallestHeightDifference) { @@ -506,7 +540,8 @@ private Optional closestToCameraHeightStrategy(PhotonPipelin .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets()); + result.getTargets(), + PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); } } @@ -568,13 +603,19 @@ private Optional closestToReferencePoseStrategy( smallestPoseDelta = altDifference; lowestDeltaPose = new EstimatedRobotPose( - altTransformPosition, result.getTimestampSeconds(), result.getTargets()); + altTransformPosition, + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.CLOSEST_TO_REFERENCE_POSE); } if (bestDifference < smallestPoseDelta) { smallestPoseDelta = bestDifference; lowestDeltaPose = new EstimatedRobotPose( - bestTransformPosition, result.getTimestampSeconds(), result.getTargets()); + bestTransformPosition, + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.CLOSEST_TO_REFERENCE_POSE); } } return Optional.ofNullable(lowestDeltaPose); @@ -617,7 +658,8 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(robotToCamera.inverse()), result.getTimestampSeconds(), - result.getTargets())); + result.getTargets(), + PoseStrategy.AVERAGE_BEST_TARGETS)); } totalAmbiguity += 1.0 / target.getPoseAmbiguity(); @@ -649,7 +691,10 @@ private Optional averageBestTargetsStrategy(PhotonPipelineRe return Optional.of( new EstimatedRobotPose( - new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets())); + new Pose3d(transform, rotation), + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.AVERAGE_BEST_TARGETS)); } /** diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java deleted file mode 100644 index 4dd6dcdaaa..0000000000 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ /dev/null @@ -1,419 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.ArrayList; -import java.util.HashSet; -import java.util.List; -import java.util.Optional; -import java.util.Set; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** - * @deprecated Use {@link PhotonPoseEstimator} - */ -@Deprecated -public class RobotPoseEstimator { - /** - * - * - *
    - *
  • LOWEST_AMBIGUITY: Choose the Pose with the lowest ambiguity - *
  • CLOSEST_TO_CAMERA_HEIGHT: Choose the Pose which is closest to the camera - * height - *
  • CLOSEST_TO_REFERENCE_POSE: Choose the Pose which is closest to the pose - * from setReferencePose() - *
  • CLOSEST_TO_LAST_POSE: Choose the Pose which is closest to the last pose - * calculated - *
- */ - public enum PoseStrategy { - LOWEST_AMBIGUITY, - CLOSEST_TO_CAMERA_HEIGHT, - CLOSEST_TO_REFERENCE_POSE, - CLOSEST_TO_LAST_POSE, - AVERAGE_BEST_TARGETS - } - - private AprilTagFieldLayout aprilTags; - private PoseStrategy strategy; - private List> cameras; - private Pose3d lastPose; - - private Pose3d referencePose; - private Set reportedErrors; - - /** - * Create a new RobotPoseEstimator. - * - * @param aprilTags A WPILib {@link 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 cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from - * the center of the robot to the camera mount positions (ie, robot ➔ camera). - */ - public RobotPoseEstimator( - AprilTagFieldLayout aprilTags, - PoseStrategy strategy, - List> cameras) { - this.aprilTags = aprilTags; - this.strategy = strategy; - this.cameras = cameras; - lastPose = new Pose3d(); - reportedErrors = new HashSet<>(); - } - - /** - * Update the estimated pose using the selected strategy. - * - * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null - * if no targets were seen - */ - public Optional> update() { - if (cameras.isEmpty()) { - DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false); - return Optional.empty(); - } - - Pair pair = getResultFromActiveStrategy(); - - if (pair != null) { - lastPose = pair.getFirst(); - } - - return Optional.ofNullable(pair); - } - - private Pair getResultFromActiveStrategy() { - switch (strategy) { - case LOWEST_AMBIGUITY: - return lowestAmbiguityStrategy(); - case CLOSEST_TO_CAMERA_HEIGHT: - return closestToCameraHeightStrategy(); - case CLOSEST_TO_REFERENCE_POSE: - return closestToReferencePoseStrategy(); - case CLOSEST_TO_LAST_POSE: - return closestToLastPoseStrategy(); - case AVERAGE_BEST_TARGETS: - return averageBestTargetsStrategy(); - default: - DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false); - return null; - } - } - - private Pair lowestAmbiguityStrategy() { - int lowestAI = -1; - int lowestAJ = -1; - double lowestAmbiguityScore = 10; - ArrayList results = new ArrayList(cameras.size()); - - // Sample result from each camera - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - results.add(p.getFirst().getLatestResult()); - } - - // Loop over each ambiguity of all the cameras - for (int i = 0; i < cameras.size(); i++) { - List targets = results.get(i).targets; - for (int j = 0; j < targets.size(); j++) { - if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) { - lowestAI = i; - lowestAJ = j; - lowestAmbiguityScore = targets.get(j).getPoseAmbiguity(); - } - } - } - - // No targets, return null - if (lowestAI == -1 || lowestAJ == -1) { - return null; - } - - // Pick the lowest and do the heavy calculations - PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ); - - Optional fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId()); - if (fiducialPose.isEmpty()) { - reportFiducialPoseError(bestTarget.getFiducialId()); - return null; - } - - return Pair.of( - fiducialPose - .get() - .transformBy(bestTarget.getBestCameraToTarget().inverse()) - .transformBy(cameras.get(lowestAI).getSecond().inverse()), - results.get(lowestAI).getLatencyMillis()); - } - - private Pair closestToCameraHeightStrategy() { - double smallestHeightDifference = Double.MAX_VALUE; - double latency = 0; - Pose3d pose = null; - - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); - if (fiducialPose.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - Pose3d targetPose = fiducialPose.get(); - double alternativeDifference = - Math.abs( - p.getSecond().getZ() - - targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ()); - double bestDifference = - Math.abs( - p.getSecond().getZ() - - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ()); - if (alternativeDifference < smallestHeightDifference) { - smallestHeightDifference = alternativeDifference; - pose = - targetPose - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()); - latency = result.getLatencyMillis(); - } - if (bestDifference < smallestHeightDifference) { - smallestHeightDifference = bestDifference; - pose = - targetPose - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()); - latency = result.getLatencyMillis(); - } - } - } - - return Pair.of(pose, latency); - } - - private Pair closestToReferencePoseStrategy() { - if (referencePose == null) { - DriverStation.reportError( - "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!", - false); - return null; - } - double smallestDifference = 10e9; - double latency = 0; - Pose3d pose = null; - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); - if (fiducialPose.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - Pose3d targetPose = fiducialPose.get(); - Pose3d botBestPose = - targetPose - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()); - Pose3d botAltPose = - targetPose - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()); - double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose)); - double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose)); - if (alternativeDifference < smallestDifference) { - smallestDifference = alternativeDifference; - pose = botAltPose; - latency = result.getLatencyMillis(); - } - if (bestDifference < smallestDifference) { - smallestDifference = bestDifference; - pose = botBestPose; - latency = result.getLatencyMillis(); - } - } - } - return Pair.of(pose, latency); - } - - private Pair closestToLastPoseStrategy() { - setReferencePose(lastPose); - return closestToReferencePoseStrategy(); - } - - /** Return the average of the best target poses using ambiguity as weight */ - private Pair averageBestTargetsStrategy() { - // Pair of Double, Double = Ambiguity, Mili - List>> tempPoses = new ArrayList<>(); - double totalAmbiguity = 0; - for (int i = 0; i < cameras.size(); i++) { - Pair p = cameras.get(i); - var result = p.getFirst().getLatestResult(); - List targets = result.targets; - for (int j = 0; j < targets.size(); j++) { - PhotonTrackedTarget target = targets.get(j); - Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); - if (fiducialPose.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - Pose3d targetPose = fiducialPose.get(); - try { - totalAmbiguity += 1. / target.getPoseAmbiguity(); - } catch (ArithmeticException e) { - // A total ambiguity of zero exists, using that pose instead!", - return Pair.of( - targetPose - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()), - result.getLatencyMillis()); - } - tempPoses.add( - Pair.of( - targetPose - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(p.getSecond().inverse()), - Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis()))); - } - } - - Translation3d transform = new Translation3d(); - Rotation3d rotation = new Rotation3d(); - double latency = 0; - - if (tempPoses.isEmpty()) { - return null; - } - - if (totalAmbiguity == 0) { - Pose3d p = tempPoses.get(0).getFirst(); - double l = tempPoses.get(0).getSecond().getSecond(); - return Pair.of(p, l); - } - - for (Pair> pair : tempPoses) { - double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity; - transform = transform.plus(pair.getFirst().getTranslation().times(weight)); - rotation = rotation.plus(pair.getFirst().getRotation().times(weight)); - latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well - } - - return Pair.of(new Pose3d(transform, rotation), latency); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - /** - * @param aprilTags the aprilTags to set - */ - public void setAprilTags(AprilTagFieldLayout aprilTags) { - this.aprilTags = aprilTags; - } - - /** - * @return the aprilTags - */ - public AprilTagFieldLayout getAprilTags() { - return aprilTags; - } - - /** - * @return the strategy - */ - public PoseStrategy getStrategy() { - return strategy; - } - - /** - * @param strategy the strategy to set - */ - public void setStrategy(PoseStrategy strategy) { - this.strategy = strategy; - } - - /** - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp new file mode 100644 index 0000000000..70ffe8f903 --- /dev/null +++ b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp @@ -0,0 +1,109 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "photonlib/MultiTargetPNPResult.h" + +using namespace photonlib; + +Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) { + packet << target.result; + + size_t i; + for (i = 0; i < target.fiducialIdsUsed.size(); i++) { + packet << target.fiducialIdsUsed[i]; + } + for (; i < target.fiducialIdsUsed.capacity(); i++) { + packet << -128; + } + + return packet; +} + +Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) { + packet >> target.result; + + target.fiducialIdsUsed.clear(); + for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) { + int8_t id; + packet >> id; + + if (id > -128) { + target.fiducialIdsUsed.push_back(id); + } + } + + return packet; +} + +// Encode a transform3d +Packet& operator<<(Packet& packet, const frc::Transform3d& transform) { + packet << transform.Translation().X().value() + << transform.Translation().Y().value() + << transform.Translation().Z().value() + << transform.Rotation().GetQuaternion().W() + << transform.Rotation().GetQuaternion().X() + << transform.Rotation().GetQuaternion().Y() + << transform.Rotation().GetQuaternion().Z(); + + return packet; +} + +// Decode a transform3d +Packet& operator>>(Packet& packet, frc::Transform3d& transform) { + frc::Transform3d ret; + + // We use these for best and alt transforms below + double x = 0; + double y = 0; + double z = 0; + double w = 0; + + // decode and unitify translation + packet >> x >> y >> z; + const auto translation = frc::Translation3d( + units::meter_t(x), units::meter_t(y), units::meter_t(z)); + + // decode and add units to rotation + packet >> w >> x >> y >> z; + const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + + transform = frc::Transform3d(translation, rotation); + + return packet; +} + +Packet& operator<<(Packet& packet, const PNPResults& result) { + packet << result.isValid << result.best << result.alt + << result.bestReprojectionErr << result.altReprojectionErr + << result.ambiguity; + + return packet; +} +Packet& operator>>(Packet& packet, PNPResults& result) { + packet >> result.isValid >> result.best >> result.alt >> + result.bestReprojectionErr >> result.altReprojectionErr >> + result.ambiguity; + + return packet; +} diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index 9f6d0a8218..462dd25a90 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -40,7 +40,7 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const { Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { // Encode latency and number of targets. - packet << result.latency.value() * 1000 + packet << result.latency.value() * 1000 << result.m_pnpResults << static_cast(result.targets.size()); // Encode the information of each target. @@ -52,9 +52,9 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { // Decode latency, existence of targets, and number of targets. - int8_t targetCount = 0; double latencyMillis = 0; - packet >> latencyMillis >> targetCount; + int8_t targetCount = 0; + packet >> latencyMillis >> result.m_pnpResults >> targetCount; result.latency = units::second_t(latencyMillis / 1000.0); result.targets.clear(); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 9282e3bdd5..b2bf8cc686 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -81,7 +81,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, poseCacheTimestamp(-1_s) {} void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { - if (strategy == MULTI_TAG_PNP) { + if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || + strategy == MULTI_TAG_PNP_ON_RIO) { FRC_ReportError( frc::warn::Warning, "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", @@ -161,8 +162,12 @@ std::optional PhotonPoseEstimator::Update( case AVERAGE_BEST_TARGETS: ret = AverageBestTargetsStrategy(result); break; - case ::photonlib::MULTI_TAG_PNP: - ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs); + case MULTI_TAG_PNP_ON_COPROCESSOR: + ret = + MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs); + break; + case MULTI_TAG_PNP_ON_RIO: + ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs); break; default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", @@ -204,7 +209,7 @@ std::optional PhotonPoseEstimator::LowestAmbiguityStrategy( fiducialPose.value() .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp(), result.GetTargets()}; + result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY}; } std::optional @@ -240,14 +245,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy( pose = EstimatedRobotPose{ targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp(), result.GetTargets()}; + result.GetTimestamp(), result.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()}; + result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT}; } } @@ -298,7 +303,8 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy( } } - return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()}; + return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(), + CLOSEST_TO_REFERENCE_POSE}; } std::optional> detail::CalcTagCorners( @@ -354,7 +360,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { rv, radian_t{rv.norm()})); } -std::optional PhotonPoseEstimator::MultiTagPnpStrategy( +std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( + PhotonPipelineResult result, std::optional camMat, + std::optional distCoeffs) { + if (result.MultiTagResult().result.isValid) { + const auto field2camera = result.MultiTagResult().result.best; + + const auto fieldToRobot = + frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); + return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(), + result.GetTargets(), + MULTI_TAG_PNP_ON_COPROCESSOR); + } + + return Update(result, std::nullopt, std::nullopt, + this->multiTagFallbackStrategy); +} + +std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs) { using namespace frc; @@ -407,7 +430,7 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( return photonlib::EstimatedRobotPose( pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(), - result.GetTargets()); + result.GetTargets(), MULTI_TAG_PNP_ON_RIO); } std::optional @@ -433,7 +456,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { return EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), - result.GetTimestamp(), result.GetTargets()}; + result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS}; } totalAmbiguity += 1. / target.GetPoseAmbiguity(); @@ -453,6 +476,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { } return EstimatedRobotPose{frc::Pose3d(transform, rotation), - result.GetTimestamp(), result.GetTargets()}; + result.GetTimestamp(), result.GetTargets(), + AVERAGE_BEST_TARGETS}; } } // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp deleted file mode 100644 index 4d25a0a482..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "photonlib/RobotPoseEstimator.h" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" - -namespace photonlib { -RobotPoseEstimator::RobotPoseEstimator( - std::shared_ptr tags, PoseStrategy strat, - std::vector, frc::Transform3d>> - cams) - : aprilTags(tags), - strategy(strat), - cameras(std::move(cams)), - lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()) {} - -std::pair RobotPoseEstimator::Update() { - if (cameras.empty()) { - return std::make_pair(lastPose, units::second_t(0)); - } - - std::pair pair; - switch (strategy) { - case LOWEST_AMBIGUITY: - pair = LowestAmbiguityStrategy(); - lastPose = pair.first; - return pair; - case CLOSEST_TO_CAMERA_HEIGHT: - pair = ClosestToCameraHeightStrategy(); - lastPose = pair.first; - return pair; - case CLOSEST_TO_REFERENCE_POSE: - pair = ClosestToReferencePoseStrategy(); - lastPose = pair.first; - return pair; - case CLOSEST_TO_LAST_POSE: - SetReferencePose(lastPose); - pair = ClosestToReferencePoseStrategy(); - lastPose = pair.first; - return pair; - case AVERAGE_BEST_TARGETS: - pair = AverageBestTargetsStrategy(); - lastPose = pair.first; - return pair; - default: - FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", - ""); - } - - return std::make_pair(lastPose, units::second_t(0)); -} - -std::pair -RobotPoseEstimator::LowestAmbiguityStrategy() { - int lowestAI = -1; - int lowestAJ = -1; - double lowestAmbiguityScore = std::numeric_limits::infinity(); - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) { - lowestAI = i; - lowestAJ = j; - lowestAmbiguityScore = targets[j].GetPoseAmbiguity(); - } - } - } - - if (lowestAI == -1 || lowestAJ == -1) { - return std::make_pair(lastPose, units::second_t(0)); - } - - PhotonTrackedTarget bestTarget = - cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ]; - - std::optional fiducialPose = - aprilTags->GetTagPose(bestTarget.GetFiducialId()); - if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - bestTarget.GetFiducialId()); - return std::make_pair(lastPose, units::second_t(0)); - } - - return std::make_pair( - fiducialPose.value() - .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) - .TransformBy(cameras[lowestAI].second.Inverse()), - cameras[lowestAI].first->GetLatestResult().GetTimestamp()); -} - -std::pair -RobotPoseEstimator::ClosestToCameraHeightStrategy() { - units::meter_t smallestHeightDifference = - units::meter_t(std::numeric_limits::infinity()); - units::second_t stateTimestamp = units::second_t(0); - frc::Pose3d pose = lastPose; - - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - PhotonTrackedTarget target = targets[j]; - std::optional fiducialPose = - aprilTags->GetTagPose(target.GetFiducialId()); - if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); - 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) { - smallestHeightDifference = alternativeDifference; - pose = targetPose.TransformBy( - target.GetAlternateCameraToTarget().Inverse()); - stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - if (bestDifference < smallestHeightDifference) { - smallestHeightDifference = bestDifference; - pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); - stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - } - } - return std::make_pair(pose, stateTimestamp); -} - -std::pair -RobotPoseEstimator::ClosestToReferencePoseStrategy() { - units::meter_t smallestDifference = - units::meter_t(std::numeric_limits::infinity()); - units::second_t stateTimestamp = units::second_t(0); - frc::Pose3d pose = lastPose; - - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - PhotonTrackedTarget target = targets[j]; - std::optional fiducialPose = - aprilTags->GetTagPose(target.GetFiducialId()); - if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); - continue; - } - frc::Pose3d targetPose = fiducialPose.value(); - units::meter_t alternativeDifference = - units::math::abs(referencePose.Translation().Distance( - targetPose - .TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .Translation())); - units::meter_t bestDifference = - units::math::abs(referencePose.Translation().Distance( - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .Translation())); - if (alternativeDifference < smallestDifference) { - smallestDifference = alternativeDifference; - pose = targetPose.TransformBy( - target.GetAlternateCameraToTarget().Inverse()); - stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - - if (bestDifference < smallestDifference) { - smallestDifference = bestDifference; - pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()); - stateTimestamp = p.first->GetLatestResult().GetTimestamp(); - } - } - } - - return std::make_pair(pose, stateTimestamp); -} - -std::pair -RobotPoseEstimator::AverageBestTargetsStrategy() { - std::vector>> - tempPoses; - double totalAmbiguity = 0; - units::second_t timstampSum = units::second_t(0); - - for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) { - std::pair, frc::Transform3d> p = cameras[i]; - std::span targets = - p.first->GetLatestResult().GetTargets(); - timstampSum += p.first->GetLatestResult().GetTimestamp(); - for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { - PhotonTrackedTarget target = targets[j]; - std::optional fiducialPose = - aprilTags->GetTagPose(target.GetFiducialId()); - if (!fiducialPose) { - FRC_ReportError(frc::warn::Warning, - "Tried to get pose of unknown April Tag: {}", - target.GetFiducialId()); - continue; - } - - frc::Pose3d targetPose = fiducialPose.value(); - if (target.GetPoseAmbiguity() == 0) { - FRC_ReportError(frc::warn::Warning, - "Pose ambiguity of zero exists, using that instead!", - ""); - return std::make_pair( - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()), - p.first->GetLatestResult().GetLatency() / 1000.); - } - totalAmbiguity += 1. / target.GetPoseAmbiguity(); - - tempPoses.push_back(std::make_pair( - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()), - std::make_pair(target.GetPoseAmbiguity(), - p.first->GetLatestResult().GetTimestamp()))); - } - } - - frc::Translation3d transform = frc::Translation3d(); - frc::Rotation3d rotation = frc::Rotation3d(); - - for (std::pair>& pair : - tempPoses) { - double weight = (1. / pair.second.first) / totalAmbiguity; - transform = transform + pair.first.Translation() * weight; - rotation = rotation + pair.first.Rotation() * weight; - } - - return std::make_pair(frc::Pose3d(transform, rotation), - timstampSum / cameras.size()); -} -} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h new file mode 100644 index 0000000000..5ff2da0b2d --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h @@ -0,0 +1,61 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include + +#include "photonlib/Packet.h" + +namespace photonlib { + +class PNPResults { + public: + // This could be wrapped in an std::optional, but chose to do it this way to + // mirror Java + bool isValid; + + frc::Transform3d best; + double bestReprojectionErr; + + frc::Transform3d alt; + double altReprojectionErr; + + double ambiguity; + + friend Packet& operator<<(Packet& packet, const PNPResults& result); + friend Packet& operator>>(Packet& packet, PNPResults& result); +}; + +class MultiTargetPnpResult { + public: + PNPResults result; + wpi::SmallVector fiducialIdsUsed; + + friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result); + friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result); +}; + +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index e6f6c4d49b..7679a8d46b 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -31,6 +31,7 @@ #include #include +#include "photonlib/MultiTargetPNPResult.h" #include "photonlib/Packet.h" #include "photonlib/PhotonTrackedTarget.h" @@ -87,6 +88,8 @@ class PhotonPipelineResult { */ units::second_t GetTimestamp() const { return timestamp; } + const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; } + /** * Sets the timestamp in seconds * @param timestamp The timestamp in seconds @@ -119,6 +122,7 @@ class PhotonPipelineResult { units::second_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; + MultiTargetPnpResult m_pnpResults; inline static bool HAS_WARNED = false; }; } // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 21f6a343cb..7c93ac9ce9 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -44,7 +44,8 @@ enum PoseStrategy { CLOSEST_TO_REFERENCE_POSE, CLOSEST_TO_LAST_POSE, AVERAGE_BEST_TARGETS, - MULTI_TAG_PNP + MULTI_TAG_PNP_ON_COPROCESSOR, + MULTI_TAG_PNP_ON_RIO, }; struct EstimatedRobotPose { @@ -57,11 +58,16 @@ struct EstimatedRobotPose { /** A list of the targets used to compute this pose */ wpi::SmallVector targetsUsed; + /** The strategy actually used to produce this pose */ + PoseStrategy strategy; + EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_, - std::span targets) + std::span targets, + PoseStrategy strategy_) : estimatedPose(pose_), timestamp(time_), - targetsUsed(targets.data(), targets.data() + targets.size()) {} + targetsUsed(targets.data(), targets.data() + targets.size()), + strategy(strategy_) {} }; /** @@ -260,14 +266,23 @@ class PhotonPoseEstimator { std::optional 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 MultiTagOnCoprocStrategy( + PhotonPipelineResult result, std::optional camMat, + std::optional distCoeffs); + /** * 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 MultiTagPnpStrategy( + std::optional MultiTagOnRioStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs); diff --git a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h deleted file mode 100644 index c9282b5920..0000000000 --- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h +++ /dev/null @@ -1,187 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "photonlib/PhotonCamera.h" - -namespace frc { -class AprilTagFieldLayout; -} // namespace frc - -namespace photonlib { -enum PoseStrategy : int { - LOWEST_AMBIGUITY, - CLOSEST_TO_CAMERA_HEIGHT, - CLOSEST_TO_REFERENCE_POSE, - CLOSEST_TO_LAST_POSE, - AVERAGE_BEST_TARGETS -}; - -/** - * The RobotPoseEstimator class filters or combines readings from all the - * fiducials 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. - */ -class RobotPoseEstimator { - public: - using map_value_type = - std::pair, frc::Transform3d>; - using size_type = std::vector::size_type; - - /** - * Create a new RobotPoseEstimator. - * - *

Example: {@code

Map map = new HashMap<>(); - *

map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is - * at (1.0,2.0,3.0) } - * - * @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 cameras An ArrayList of Pairs of PhotonCameras and their respective - * Transform3ds from the center of the robot to the cameras. - */ - explicit RobotPoseEstimator( - std::shared_ptr aprilTags, - PoseStrategy strategy, std::vector cameras); - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - * @return the AprilTagFieldLayout - */ - std::shared_ptr getFieldLayout() const { - return aprilTags; - } - - /** - * Set the cameras to be used by the PoseEstimator. - * - * @param cameras cameras to set. - */ - inline void SetCameras( - const std::vector, - frc::Transform3d>>& cameras) { - this->cameras = cameras; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - PoseStrategy GetPoseStrategy() const { return strategy; } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - 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 - */ - inline void SetReferencePose(frc::Pose3d referencePose) { - this->referencePose = referencePose; - } - - /** - * 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 - */ - inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; } - - std::pair Update(); - - private: - std::shared_ptr aprilTags; - PoseStrategy strategy; - std::vector cameras; - frc::Pose3d lastPose; - frc::Pose3d referencePose; - - /** - * 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::pair LowestAmbiguityStrategy(); - - /** - * 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::pair ClosestToCameraHeightStrategy(); - - /** - * 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::pair ClosestToReferencePoseStrategy(); - - /** - * 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::pair AverageBestTargetsStrategy(); -}; - -} // namespace photonlib diff --git a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java index 5ead3be618..fa70467c5f 100644 --- a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java +++ b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java @@ -84,7 +84,10 @@ public void testMeme() throws IOException, InterruptedException { var pe = new PhotonPoseEstimator( - tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera); + tagLayout, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + cam, + robotToCamera); var field = new Field2d(); SmartDashboard.putData(field); diff --git a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp deleted file mode 100644 index 09a2bdf461..0000000000 --- a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp +++ /dev/null @@ -1,442 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" -#include "photonlib/RobotPoseEstimator.h" - -static wpi::SmallVector, 4> corners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; -static std::vector> detectedCorners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; - -TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}}; - - std::shared_ptr aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->test = true; - cameraOne->testResult = {2_s, targets}; - cameraOne->testResult.SetTimestamp(units::second_t(11)); - - wpi::SmallVector targetsTwo{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->test = true; - cameraTwo->testResult = {4_s, targetsTwo}; - cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16))); - - cameras.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - cameras.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - photonlib::RobotPoseEstimator estimator(aprilTags, - photonlib::LOWEST_AMBIGUITY, cameras); - std::pair estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(11, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(3, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}, - }; - std::shared_ptr aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->test = true; - cameraOne->testResult = {2_s, targets}; - cameraOne->testResult.SetTimestamp(units::second_t(4)); - - wpi::SmallVector targetsTwo{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->test = true; - cameraTwo->testResult = {4_s, targetsTwo}; - cameraOne->testResult.SetTimestamp(units::second_t(12)); - - cameras.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - cameras.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, cameras); - std::pair estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(12, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(4, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}, - }; - std::shared_ptr aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->test = true; - cameraOne->testResult = {2_s, targets}; - cameraOne->testResult.SetTimestamp(units::second_t(4)); - - wpi::SmallVector targetsTwo{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->test = true; - cameraTwo->testResult = {4_s, targetsTwo}; - cameraTwo->testResult.SetTimestamp(units::second_t(17)); - - cameras.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - cameras.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras); - estimator.SetReferencePose( - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); - std::pair estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(17, units::unit_cast(estimatedPose.second) / 1000.0, .01); - EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(.9, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, ClosestToLastPose) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}}; - std::shared_ptr aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->test = true; - cameraOne->testResult = {2_s, targets}; - - wpi::SmallVector targetsTwo{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->test = true; - cameraTwo->testResult = {4_s, targetsTwo}; - - cameras.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - cameras.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_LAST_POSE, cameras); - estimator.SetLastPose( - frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); - std::pair estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - wpi::SmallVector targetsThree{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 0, - frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->testResult = {2_s, targetsThree}; - cameraOne->testResult.SetTimestamp(units::second_t(7)); - - wpi::SmallVector targetsFour{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->testResult = {4_s, targetsFour}; - cameraTwo->testResult.SetTimestamp(units::second_t(13)); - - std::vector< - std::pair, frc::Transform3d>> - camerasUpdated; - camerasUpdated.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - camerasUpdated.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - estimator.SetCameras(camerasUpdated); - estimatedPose = estimator.Update(); - pose = estimatedPose.first; - - EXPECT_NEAR(7.0, units::unit_cast(estimatedPose.second) / 1000.0, - .01); - EXPECT_NEAR(.9, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(1, units::unit_cast(pose.Z()), .01); -} - -TEST(RobotPoseEstimatorTest, AverageBestPoses) { - std::vector tags = { - {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), - frc::Rotation3d())}, - {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), - frc::Rotation3d())}}; - std::shared_ptr aprilTags = - std::make_shared(tags, 54_ft, 27_ft); - - std::vector< - std::pair, frc::Transform3d>> - cameras; - - std::shared_ptr cameraOne = - std::make_shared("test"); - std::shared_ptr cameraTwo = - std::make_shared("test"); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, - frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne->test = true; - cameraOne->testResult = {2_s, targets}; - cameraOne->testResult.SetTimestamp(units::second_t(10)); - - wpi::SmallVector targetsTwo{ - photonlib::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, - frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraTwo->test = true; - cameraTwo->testResult = {4_s, targetsTwo}; - cameraTwo->testResult.SetTimestamp(units::second_t(20)); - - cameras.push_back(std::make_pair( - cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - cameras.push_back(std::make_pair( - cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)))); - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::AVERAGE_BEST_TARGETS, cameras); - std::pair estimatedPose = - estimator.Update(); - frc::Pose3d pose = estimatedPose.first; - - EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.second) / 1000.0, - .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(2.15, units::unit_cast(pose.Z()), .01); -}