diff --git a/docs/source/docs/apriltag-pipelines/multitag.md b/docs/source/docs/apriltag-pipelines/multitag.md index f33e3dfb7..ab701480d 100644 --- a/docs/source/docs/apriltag-pipelines/multitag.md +++ b/docs/source/docs/apriltag-pipelines/multitag.md @@ -23,7 +23,7 @@ Ensure that your camera is calibrated and 3D mode is enabled. Navigate to the Ou By default, enabling multi-target will disable calculating camera-to-target transforms for each observed AprilTag target to increase performance; the X/Y/angle numbers shown in the target table of the UI are instead calculated using the tag's expected location (per the field layout JSON) and the field-to-camera transform calculated using MultiTag. If you additionally want the individual camera-to-target transform calculated using SolvePNP for each target, enable "Always Do Single-Target Estimation". ::: -This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class ` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python). +This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class ` and calling `estimateCoprocMultiTagPose` (with an optional fallback like `estimateLowestAmbiguityPose`) to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python). ```{eval-rst} .. tab-set-code:: diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 4a05860ea..ea29a83cd 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -45,7 +45,6 @@ import org.wpilib.math.numbers.N3; import org.wpilib.math.numbers.N8; import org.wpilib.math.util.Pair; import org.wpilib.vision.apriltag.AprilTagFieldLayout; -import org.wpilib.vision.camera.OpenCvLoader; /** * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a @@ -123,13 +122,7 @@ public class PhotonPoseEstimator { private AprilTagFieldLayout fieldTags; private TargetModel tagModel = TargetModel.kAprilTag36h11; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; private final Set reportedErrors = new HashSet<>(); private final TimeInterpolatableBuffer headingBuffer = @@ -156,48 +149,6 @@ public class PhotonPoseEstimator { InstanceCount++; } - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @deprecated Use individual estimation methods with the 2 argument constructor instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.robotToCamera = robotToCamera; - - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO - || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) { - OpenCvLoader.forceStaticLoad(); - } - - InstanceCount++; - HAL.reportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); - } - - /** Invalidates the pose cache. */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (!Objects.equals(oldObj, newObj)) { - invalidatePoseCache(); - } - } - /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * @@ -217,7 +168,6 @@ public class PhotonPoseEstimator { * @param fieldTags the AprilTagFieldLayout */ public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); this.fieldTags = fieldTags; } @@ -239,113 +189,6 @@ public class PhotonPoseEstimator { this.tagModel = tagModel; } - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO - || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) { - OpenCvLoader.forceStaticLoad(); - } - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - 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; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(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 - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * 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 - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - /** * Add robot heading data to buffer. Must be called periodically for the * PNP_DISTANCE_TRIG_SOLVE strategy. @@ -413,253 +256,6 @@ public class PhotonPoseEstimator { this.robotToCamera = robotToCamera; } - /** - * Updates the estimated position of the robot, assuming no camera calibration is required for the - * selected strategy. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not - * provided in this overload. - * - * @param cameraResult The latest pipeline result from the camera - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update(PhotonPipelineResult cameraResult) { - return update(cameraResult, Optional.empty(), Optional.empty()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
  • The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the - * other function overload). - *
- * - * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
  • The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty. - *
- * - * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param constrainedPnpParams Constrained SolvePNP params, if needed. - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - Optional constrainedPnpParams) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result - if (poseCacheTimestampSeconds > 0 - && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) { - return Optional.empty(); - } - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update( - cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy); - } - - /** - * Internal convenience method for using a fallback strategy for update(). This should only be - * called after timestamp checks have been done by another update() overload. - * - * @param cameraResult The latest pipeline result from the camera - * @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP. - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private Optional update( - PhotonPipelineResult cameraResult, PoseStrategy strategy) { - return update(cameraResult, Optional.empty(), Optional.empty(), Optional.empty(), strategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - Optional constrainedPnpParams, - PoseStrategy strategy) { - Optional estimatedPose = - switch (strategy) { - case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult); - case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult); - case CLOSEST_TO_REFERENCE_POSE -> - estimateClosestToReferencePose(cameraResult, referencePose); - case CLOSEST_TO_LAST_POSE -> { - setReferencePose(lastPose); - yield estimateClosestToReferencePose(cameraResult, referencePose); - } - case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult); - case MULTI_TAG_PNP_ON_RIO -> { - if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) { - DriverStation.reportWarning( - "No camera calibration data provided for multi-tag-on-rio", - Thread.currentThread().getStackTrace()); - yield update(cameraResult, this.multiTagFallbackStrategy); - } - var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get()); - if (res.isEmpty()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - constrainedPnpParams, - this.multiTagFallbackStrategy); - } - yield res; - } - case MULTI_TAG_PNP_ON_COPROCESSOR -> { - if (cameraResult.getMultiTagResult().isEmpty()) { - yield update(cameraResult, this.multiTagFallbackStrategy); - } - yield estimateCoprocMultiTagPose(cameraResult); - } - case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult); - case CONSTRAINED_SOLVEPNP -> { - boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - - if (constrainedPnpParams.isEmpty()) { - yield Optional.empty(); - } - - // Need heading if heading fixed - if (!constrainedPnpParams.get().headingFree - && headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - - Pose3d fieldToRobotSeed; - // Attempt to use multi-tag to get a pose estimate seed - if (cameraResult.getMultiTagResult().isPresent()) { - fieldToRobotSeed = - Pose3d.kZero.plus( - cameraResult - .getMultiTagResult() - .get() - .estimatedPose - .best - .plus(robotToCamera.inverse())); - } else { - // HACK - use fallback strategy to gimme a seed pose - // TODO - make sure nested update doesn't break state - var nestedUpdate = - update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - if (nestedUpdate.isEmpty()) { - // best i can do is bail - yield Optional.empty(); - } - fieldToRobotSeed = nestedUpdate.get().estimatedPose; - } - - if (!constrainedPnpParams.get().headingFree) { - // If heading fixed, force rotation component - fieldToRobotSeed = - new Pose3d( - fieldToRobotSeed.getTranslation(), - new Rotation3d( - headingBuffer.getSample(cameraResult.getTimestampSeconds()).get())); - } - - var pnpResult = - estimateConstrainedSolvepnpPose( - cameraResult, - cameraMatrix.get(), - distCoeffs.get(), - fieldToRobotSeed, - constrainedPnpParams.get().headingFree, - constrainedPnpParams.get().headingScaleFactor); - if (!pnpResult.isPresent()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - yield pnpResult; - } - }; - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - /** * @param cameraResult A pipeline result from the camera. * @return Whether or not pose estimation should be performed. diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 576eb11c2..53cde3088 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include "photon/PhotonCamera.h" #include "photon/estimation/TargetModel.h" @@ -51,7 +50,6 @@ #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT #include -WPI_IGNORE_DEPRECATED namespace photon { namespace detail { @@ -69,186 +67,12 @@ PhotonPoseEstimator::PhotonPoseEstimator( wpi::math::Transform3d robotToCamera) : aprilTags(tags), m_robotToCamera(robotToCamera), - lastPose(wpi::math::Pose3d()), - referencePose(wpi::math::Pose3d()), - poseCacheTimestamp(-1_s), headingBuffer( wpi::math::TimeInterpolatableBuffer(1_s)) { HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); InstanceCount++; } -PhotonPoseEstimator::PhotonPoseEstimator( - wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat, - wpi::math::Transform3d robotToCamera) - : aprilTags(tags), - strategy(strat), - m_robotToCamera(robotToCamera), - lastPose(wpi::math::Pose3d()), - referencePose(wpi::math::Pose3d()), - poseCacheTimestamp(-1_s), - headingBuffer( - wpi::math::TimeInterpolatableBuffer(1_s)) { - InstanceCount++; - HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, ""); -} - -void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { - if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || - strategy == MULTI_TAG_PNP_ON_RIO) { - WPILIB_ReportError( - wpi::warn::Warning, - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", - ""); - strategy = LOWEST_AMBIGUITY; - } - if (this->multiTagFallbackStrategy != strategy) { - InvalidatePoseCache(); - } - multiTagFallbackStrategy = strategy; -} - -std::optional PhotonPoseEstimator::Update( - const PhotonPipelineResult& result, - std::optional cameraMatrixData, - std::optional cameraDistCoeffs, - std::optional constrainedPnpParams) { - // Time in the past -- give up, since the following if expects times > 0 - if (result.GetTimestamp() < 0_s) { - WPILIB_ReportError(wpi::warn::Warning, - "Result timestamp was reported in the past!"); - return std::nullopt; - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an empty result - if (poseCacheTimestamp > 0_s && - wpi::units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < - 0.001_ms) { - return std::nullopt; - } - - // Remember the timestamp of the current result used - poseCacheTimestamp = result.GetTimestamp(); - - // If no targets seen, trivial case -- return empty result - if (!result.HasTargets()) { - return std::nullopt; - } - - return Update(result, cameraMatrixData, cameraDistCoeffs, - constrainedPnpParams, this->strategy); -} - -std::optional PhotonPoseEstimator::Update( - const PhotonPipelineResult& result, - std::optional cameraMatrixData, - std::optional cameraDistCoeffs, - std::optional constrainedPnpParams, - PoseStrategy strategy) { - std::optional ret = std::nullopt; - - switch (strategy) { - case LOWEST_AMBIGUITY: - ret = EstimateLowestAmbiguityPose(result); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - ret = EstimateClosestToCameraHeightPose(result); - break; - case CLOSEST_TO_REFERENCE_POSE: - ret = EstimateClosestToReferencePose(result, this->referencePose); - break; - case CLOSEST_TO_LAST_POSE: - SetReferencePose(lastPose); - ret = EstimateClosestToReferencePose(result, this->referencePose); - break; - case AVERAGE_BEST_TARGETS: - ret = EstimateAverageBestTargetsPose(result); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - if (!result.MultiTagResult()) { - ret = Update(result, this->multiTagFallbackStrategy); - } else { - ret = EstimateCoprocMultiTagPose(result); - } - break; - case MULTI_TAG_PNP_ON_RIO: - if (!cameraMatrixData && !cameraDistCoeffs) { - WPILIB_ReportError( - wpi::warn::Warning, - "No camera calibration provided to multi-tag-on-rio!", ""); - ret = Update(result, this->multiTagFallbackStrategy); - } - ret = - EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs); - if (!ret) { - ret = Update(result, this->multiTagFallbackStrategy); - } - break; - case CONSTRAINED_SOLVEPNP: { - if (!cameraMatrixData || !cameraDistCoeffs) { - WPILIB_ReportError( - wpi::warn::Warning, - "No camera calibration data provided for Constrained SolvePnP!"); - ret = Update(result, this->multiTagFallbackStrategy); - break; - } - - if (!constrainedPnpParams) { - return {}; - } - - if (!constrainedPnpParams->headingFree && - !headingBuffer.Sample(result.GetTimestamp()).has_value()) { - ret = Update(result, cameraMatrixData, cameraDistCoeffs, {}, - this->multiTagFallbackStrategy); - break; - } - - wpi::math::Pose3d fieldToRobotSeed; - - if (result.MultiTagResult().has_value()) { - fieldToRobotSeed = - wpi::math::Pose3d{} + (result.MultiTagResult()->estimatedPose.best + - m_robotToCamera.Inverse()); - } else { - std::optional nestedUpdate = - Update(result, cameraMatrixData, cameraDistCoeffs, {}, - this->multiTagFallbackStrategy); - - if (!nestedUpdate.has_value()) { - return {}; - } - - fieldToRobotSeed = nestedUpdate->estimatedPose; - } - - ret = EstimateConstrainedSolvepnpPose( - result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed, - constrainedPnpParams->headingFree, - constrainedPnpParams->headingScalingFactor); - - if (!ret) { - ret = Update(result, cameraMatrixData, cameraDistCoeffs, {}, - this->multiTagFallbackStrategy); - } - break; - } - case PNP_DISTANCE_TRIG_SOLVE: - ret = EstimatePnpDistanceTrigSolvePose(result); - break; - default: - WPILIB_ReportError(wpi::warn::Warning, "Invalid Pose Strategy selected!", - ""); - ret = std::nullopt; - } - - if (ret) { - lastPose = ret->estimatedPose; - } - return ret; -} - bool ShouldEstimate(const PhotonPipelineResult& result) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { @@ -359,7 +183,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( wpi::units::meter_t smallestDifference = wpi::units::meter_t(std::numeric_limits::infinity()); wpi::units::second_t stateTimestamp = wpi::units::second_t(0); - wpi::math::Pose3d pose = lastPose; + wpi::math::Pose3d pose; auto targets = cameraResult.GetTargets(); for (auto& target : targets) { diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index d3551927c..a32b0b18a 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -97,24 +97,6 @@ class PhotonPoseEstimator { explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags, wpi::math::Transform3d robotToCamera); - /** - * Create a new PhotonPoseEstimator. - * - * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with - * respect to the FIRST field. - * @param strategy The strategy it should use to determine the best pose. - * @param robotToCamera Transform3d from the center of the robot to the camera - * mount positions (ie, robot ➔ camera). - * @deprecated Use individual estimation methods with the 2 argument - * constructor instead. - */ - [[deprecated( - "Use individual estimation methods with the 2 argument constructor " - "instead.")]] - explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags, - PoseStrategy strategy, - wpi::math::Transform3d robotToCamera); - /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * @@ -124,67 +106,6 @@ class PhotonPoseEstimator { return aprilTags; } - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - PoseStrategy GetPoseStrategy() const { - return strategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - inline void SetPoseStrategy(PoseStrategy strat) { - if (strategy != strat) { - InvalidatePoseCache(); - } - strategy = strat; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when - * only one tag can be seen. Must NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - void SetMultiTagFallbackStrategy(PoseStrategy strategy); - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - wpi::math::Pose3d GetReferencePose() const { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the - * CLOSEST_TO_REFERENCE_POSE strategy. - * - * @param referencePose the referencePose to set - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - inline void SetReferencePose(wpi::math::Pose3d referencePose) { - if (this->referencePose != referencePose) { - InvalidatePoseCache(); - } - this->referencePose = referencePose; - } - /** * @return The current transform from the center of the robot to the camera * mount position. @@ -203,18 +124,6 @@ class PhotonPoseEstimator { m_robotToCamera = robotToCamera; } - /** - * 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 - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - inline void SetLastPose(wpi::math::Pose3d lastPose) { - this->lastPose = lastPose; - } - /** * Add robot heading data to the buffer. Must be called periodically for the * PNP_DISTANCE_TRIG_SOLVE strategy. @@ -270,29 +179,6 @@ class PhotonPoseEstimator { ResetHeadingData(timestamp, heading.ToRotation2d()); } - /** - * Update the pose estimator. If updating multiple times per loop, you should - * call this exactly once per new result, in order of increasing result - * timestamp. - * - * @param result The vision targeting result to process - * @param cameraIntrinsics The camera calibration pinhole coefficients matrix. - * Only required if doing multitag-on-rio, and may be nullopt otherwise. - * @param distCoeffsData The camera calibration distortion coefficients. Only - * required if doing multitag-on-rio, and may be nullopt otherwise. - * @param constrainedPnpParams Constrained SolvePNP params, if needed. - * @deprecated Use individual estimation methods instead. - */ - [[deprecated("Use individual estimation methods instead.")]] - std::optional Update( - const photon::PhotonPipelineResult& result, - std::optional cameraMatrixData = - std::nullopt, - std::optional coeffsData = - std::nullopt, - std::optional constrainedPnpParams = - std::nullopt); - /** * Return the estimated position of the robot with the lowest position * ambiguity from a List of pipeline results. @@ -420,43 +306,12 @@ class PhotonPoseEstimator { private: wpi::apriltag::AprilTagFieldLayout aprilTags; - PoseStrategy strategy; - PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY; wpi::math::Transform3d m_robotToCamera; - wpi::math::Pose3d lastPose; - wpi::math::Pose3d referencePose; - - wpi::units::second_t poseCacheTimestamp; - wpi::math::TimeInterpolatableBuffer headingBuffer; inline static int InstanceCount = 1; - - inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } - - /** - * Internal convenience method for using a fallback strategy for update(). - * This should only be called after timestamp checks have been done by another - * update() overload. - * - * @param cameraResult A pipeline result from the camera. - * @param strategy The pose strategy to use - * @return An EstimatedRobotPose with an estimated pose, timestamp, and - * targets used to create the estimate. - */ - std::optional Update(const PhotonPipelineResult& result, - PoseStrategy strategy) { - return Update(result, std::nullopt, std::nullopt, std::nullopt, strategy); - } - - std::optional Update( - const PhotonPipelineResult& result, - std::optional cameraMatrixData, - std::optional coeffsData, - std::optional constrainedPnpParams, - PoseStrategy strategy); }; } // namespace photon diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java deleted file mode 100644 index 5d776a314..000000000 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ /dev/null @@ -1,936 +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 static org.junit.jupiter.api.Assertions.assertAll; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertNull; -import static org.junit.jupiter.api.Assertions.assertTrue; -import static org.junit.jupiter.api.Assertions.fail; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.junit.jupiter.api.AfterAll; -import org.junit.jupiter.api.AutoClose; -import org.junit.jupiter.api.BeforeAll; -import org.junit.jupiter.api.Test; -import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.estimation.TargetModel; -import org.photonvision.jni.LibraryLoader; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionTargetSim; -import org.photonvision.targeting.MultiTargetPNPResult; -import org.photonvision.targeting.PhotonPipelineMetadata; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.PnpResult; -import org.photonvision.targeting.TargetCorner; -import org.wpilib.hardware.hal.HAL; -import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.geometry.Pose3d; -import org.wpilib.math.geometry.Quaternion; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.geometry.Rotation3d; -import org.wpilib.math.geometry.Transform3d; -import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.geometry.Translation3d; -import org.wpilib.math.linalg.MatBuilder; -import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.util.Nat; -import org.wpilib.math.util.Units; -import org.wpilib.util.runtime.RuntimeLoader; -import org.wpilib.vision.apriltag.AprilTag; -import org.wpilib.vision.apriltag.AprilTagFieldLayout; -import org.wpilib.vision.apriltag.AprilTagFields; - -class LegacyPhotonPoseEstimatorTest { - static AprilTagFieldLayout aprilTags; - @AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector(); - - @BeforeAll - public static void init() throws IOException { - if (!LibraryLoader.loadWpiLibraries()) { - fail(); - } - RuntimeLoader.loadLibrary("photontargetingJNI"); - - HAL.initialize(1000, 0); - - List tagList = new ArrayList<>(2); - tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d()))); - tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d()))); - double fieldLength = Units.feetToMeters(54.0); - double fieldWidth = Units.feetToMeters(27.0); - aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth); - } - - @AfterAll - public static void teardown() { - HAL.shutdown(); - } - - @Test - void testLowestAmbiguityStrategy() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 11 * 1000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d()); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - - assertEquals(11, estimatedPose.get().timestampSeconds); - assertEquals(1, pose.getX(), .01); - assertEquals(3, pose.getY(), .01); - assertEquals(2, pose.getZ(), .01); - } - - @Test - void testClosestToCameraHeightStrategy() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 4000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 1, - -1, - -1, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()), - new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, - new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - - assertEquals(4, estimatedPose.get().timestampSeconds); - assertEquals(4, pose.getX(), .01); - assertEquals(4, pose.getY(), .01); - assertEquals(0, pose.getZ(), .01); - } - - @Test - void closestToReferencePoseStrategy() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 17000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 1, - -1, - -1, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()), - new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_REFERENCE_POSE, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); - estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d())); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - - assertEquals(17, estimatedPose.get().timestampSeconds); - assertEquals(1, pose.getX(), .01); - assertEquals(1.1, pose.getY(), .01); - assertEquals(.9, pose.getZ(), .01); - } - - @Test - void closestToLastPose() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 1000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 1, - -1, - -1, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()), - new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_LAST_POSE, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); - - estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d())); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - - cameraOne.result = - new PhotonPipelineResult( - 0, - 7000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 1, - -1, - -1, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 0, - -1, - -1, - new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()), - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()), - new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - estimatedPose = estimator.update(cameraOne.result); - pose = estimatedPose.get().estimatedPose; - - assertEquals(7, estimatedPose.get().timestampSeconds); - assertEquals(.9, pose.getX(), .01); - assertEquals(1.1, pose.getY(), .01); - assertEquals(1, pose.getZ(), .01); - } - - @Test - void pnpDistanceTrigSolve() { - List simTargets = - aprilTags.getTags().stream() - .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID)) - .toList(); - try (PhotonCameraSim cameraOneSim = - new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) { - /* Compound Rolled + Pitched + Yaw */ - Transform3d compoundTestTransform = - new Transform3d( - -Units.inchesToMeters(12), - -Units.inchesToMeters(11), - 3, - new Rotation3d( - Units.degreesToRadians(37), - Units.degreesToRadians(6), - Units.degreesToRadians(60))); - - var estimator = - new PhotonPoseEstimator( - aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); - - /* this is the real pose of the robot base we test against */ - var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); - PhotonPipelineResult result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); - var bestTarget = result.getBestTarget(); - assertNotNull(bestTarget); - assertEquals(0, bestTarget.fiducialId); - - estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); - var estimatedPose = estimator.update(result); - - var pose = estimatedPose.get().estimatedPose; - assertEquals(realPose.getX(), pose.getX(), .01); - assertEquals(realPose.getY(), pose.getY(), .01); - assertEquals(0.0, pose.getZ(), .01); - - /* Straight on */ - Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); - - estimator.setRobotToCameraTransform(straightOnTestTransform); - - /* Pose to compare with */ - realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); - result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); - - estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); - estimatedPose = estimator.update(result); - - pose = estimatedPose.get().estimatedPose; - assertEquals(realPose.getX(), pose.getX(), .01); - assertEquals(realPose.getY(), pose.getY(), .01); - assertEquals(0.0, pose.getZ(), .01); - } - } - - @Test - void cacheIsInvalidated() { - var result = - new PhotonPipelineResult( - 0, - 20_000_000, - 1_100_000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.AVERAGE_BEST_TARGETS, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); - - // Initial state, expect no timestamp - assertEquals(-1, estimator.poseCacheTimestampSeconds); - - // Empty result, expect empty result - cameraOne.result = new PhotonPipelineResult(); - cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6); - Optional estimatedPose = estimator.update(cameraOne.result); - assertFalse(estimatedPose.isPresent()); - - // Set actual result - cameraOne.result = result; - estimatedPose = estimator.update(cameraOne.result); - assertTrue(estimatedPose.isPresent()); - assertEquals(20, estimatedPose.get().timestampSeconds, .01); - assertEquals(20, estimator.poseCacheTimestampSeconds); - - // And again -- pose cache should mean this is empty - cameraOne.result = result; - estimatedPose = estimator.update(cameraOne.result); - assertFalse(estimatedPose.isPresent()); - // Expect the old timestamp to still be here - assertEquals(20, estimator.poseCacheTimestampSeconds); - - // Set new field layout -- right after, the pose cache timestamp should be -1 - estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0)); - assertEquals(-1, estimator.poseCacheTimestampSeconds); - // Update should cache the current timestamp (20) again - cameraOne.result = result; - estimatedPose = estimator.update(cameraOne.result); - assertEquals(20, estimatedPose.get().timestampSeconds, .01); - assertEquals(20, estimator.poseCacheTimestampSeconds); - - // Setting a value from None to a non-None should invalidate the cache - assertNull(estimator.getReferencePose()); - assertEquals(20, estimator.poseCacheTimestampSeconds); - estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero)); - assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf"); - } - - @Test - void averageBestPoses() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 20 * 1000000, - 1100000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()), - new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), // 1 1 1 ambig: .7 - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), // 2 2 2 ambig .3 - new PhotonTrackedTarget( - 9.0, - -2.0, - 19.0, - 3.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()), - new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()), - 0.4, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.AVERAGE_BEST_TARGETS, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - - assertEquals(20, estimatedPose.get().timestampSeconds, .01); - assertEquals(2.15, pose.getX(), .01); - assertEquals(2.15, pose.getY(), .01); - assertEquals(2.15, pose.getZ(), .01); - } - - @Test - void testMultiTagOnRioFallback() { - cameraOne.result = - new PhotonPipelineResult( - 0, - 11 * 1_000_000, - 1_100_000, - 1024, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 0, - -1, - -1, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.7, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 1, - -1, - -1, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.3, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - PhotonPoseEstimator estimator = - new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero); - estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - Optional estimatedPose = estimator.update(cameraOne.result); - Pose3d pose = estimatedPose.get().estimatedPose; - // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy - assertAll( - () -> assertEquals(11, estimatedPose.get().timestampSeconds), - () -> assertEquals(1, pose.getX(), 1e-9), - () -> assertEquals(3, pose.getY(), 1e-9), - () -> assertEquals(2, pose.getZ(), 1e-9)); - } - - @Test - public void testConstrainedPnpOneTag() { - var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0); - var cameraMat = - MatBuilder.fill( - Nat.N3(), - Nat.N3(), - 399.37500000000006, - 0, - 319.5, - 0, - 399.16666666666674, - 239.5, - 0, - 0, - 1); - - /* - * Ground truth: - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0. - * 31064262190452635 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0. - * 24478552235412665 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0. - * 0836470779150917 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0. - * 914649865171567 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3. - * 191446451763934 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4. - * 44396966389316 - * 29.989279,NT:/photonvision/YOUR CAMERA - * NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0. - * 4995793771070878 - */ - List corners8 = - List.of( - new TargetCorner(98.09875447066685, 331.0093220119495), - new TargetCorner(122.20226758624413, 335.50083894738486), - new TargetCorner(127.17118732489361, 313.81406314178633), - new TargetCorner(104.28543773760417, 309.6516557438994)); - - var result = - new PhotonPipelineResult( - new PhotonPipelineMetadata(10000, 2000, 1, 100), - List.of( - new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)), - Optional.of( - new MultiTargetPNPResult( - new PnpResult( - new Transform3d( - // From ground truth - new Translation3d( - 3.1665557336121353, 4.430673446050584, 0.48678786477534686), - new Rotation3d( - new Quaternion( - 0.3132532247418243, - 0.24722671090692333, - -0.08413452932300695, - 0.9130568172784148))), - 0.1), - new ArrayList(8)))); - - final double camPitch = Units.degreesToRadians(30.0); - final Transform3d kRobotToCam = - new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); - - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.CONSTRAINED_SOLVEPNP, - kRobotToCam); - - estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero); - - Optional estimatedPose = - estimator.update( - result, - Optional.of(cameraMat), - Optional.of(distortion), - Optional.of(new ConstrainedSolvepnpParams(true, 0))); - Pose3d pose = estimatedPose.get().estimatedPose; - System.out.println(pose); - } - - @Test - void testConstrainedPnpEmptyCase() { - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.CONSTRAINED_SOLVEPNP, - Transform3d.kZero); - PhotonPipelineResult result = new PhotonPipelineResult(); - var estimate = estimator.update(result); - assertEquals(estimate, Optional.empty()); - } - - private static class PhotonCameraInjector extends PhotonCamera { - public PhotonCameraInjector() { - super("Test"); - } - - PhotonPipelineResult result; - - @Override - public List getAllUnreadResults() { - return List.of(result); - } - - @Override - public PhotonPipelineResult getLatestResult() { - return result; - } - } -} diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp deleted file mode 100644 index 0317c8482..000000000 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ /dev/null @@ -1,667 +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 - -#include "gtest/gtest.h" -#include "photon/PhotonCamera.h" -#include "photon/PhotonPoseEstimator.h" -#include "photon/dataflow/structures/Packet.h" -#include "photon/simulation/PhotonCameraSim.h" -#include "photon/simulation/SimCameraProperties.h" -#include "photon/simulation/VisionTargetSim.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" -#include "photon/targeting/PnpResult.h" -WPI_IGNORE_DEPRECATED - -namespace units = wpi::units; - -static std::vector tags = { - {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), wpi::math::Rotation3d())}, - {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), wpi::math::Rotation3d())}}; - -static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; - -static std::vector corners{ - photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.}, - photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}}; -static std::vector detectedCorners{ - photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.}, - photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}}; - -TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - - photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - wpi::math::Transform3d{}); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), - .02); - 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(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::vector tags = { - {0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), wpi::math::Rotation3d())}, - {1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), wpi::math::Rotation3d())}, - }; - auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft); - - std::vector> cameras; - - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - // ID 0 at 3,3,3 - // ID 1 at 5,5,5 - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(17_s); - - photon::PhotonPoseEstimator estimator( - aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}}); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - ASSERT_TRUE(estimatedPose); - - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), - .02); - EXPECT_NEAR(4, units::unit_cast(pose.X()), .01); - EXPECT_NEAR(4, units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(0, units::unit_cast(pose.Z()), .01); -} - -TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); - - photon::PhotonPoseEstimator estimator(aprilTags, - photon::CLOSEST_TO_REFERENCE_POSE, {}); - estimator.SetReferencePose(wpi::math::Pose3d( - 1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), - .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(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); - - photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, - {}); - estimator.SetLastPose(wpi::math::Pose3d( - 1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad))); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - std::vector targetsThree{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); - - // std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(21.0, units::unit_cast(estimatedPose.value().timestamp), - .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(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - cameraOne.test = true; - - std::vector targets; - targets.reserve(tags.size()); - for (const auto& tag : tags) { - targets.push_back( - photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID)); - } - photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim( - &cameraOne, photon::SimCameraProperties::PERFECT_90DEG()); - - /* Compound Rolled + Pitched + Yaw */ - wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d( - -12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg)); - - photon::PhotonPoseEstimator estimator( - aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); - - /* real pose of the robot base to test against */ - wpi::math::Pose3d realPose = wpi::math::Pose3d( - 7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad)); - - photon::PhotonPipelineResult result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); - cameraOne.testResult = {result}; - cameraOne.testResult[0].SetReceiveTimestamp(17_s); - - estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation()); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(units::unit_cast(realPose.X()), - units::unit_cast(pose.X()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Y()), - units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Z()), - units::unit_cast(pose.Z()), .01); - - /* Straight on */ - wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d( - 0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)); - - estimator.SetRobotToCameraTransform(straightOnTestTransform); - realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m, - wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad)); - result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); - cameraOne.testResult = {result}; - cameraOne.testResult[0].SetReceiveTimestamp(18_s); - - estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation()); - - estimatedPose = std::nullopt; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(units::unit_cast(realPose.X()), - units::unit_cast(pose.X()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Y()), - units::unit_cast(pose.Y()), .01); - EXPECT_NEAR(units::unit_cast(realPose.Z()), - units::unit_cast(pose.Z()), .01); -} - -TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - - photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, - {}); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.value().timestamp), - .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); -} - -TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test2"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.4, corners, detectedCorners}}; - - cameraOne.test = true; - - photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, - {}); - - // empty input, expect empty out - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - EXPECT_FALSE(estimatedPose); - - // Set result, and update -- expect present and timestamp to be 15 - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - EXPECT_NEAR((15_s - 3_ms).to(), - estimatedPose.value().timestamp.to(), 1e-6); - - // And again -- pose cache should result in returning std::nullopt - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - EXPECT_FALSE(estimatedPose); - - // If the camera produces a result that is > 1 micro second later, - // the pose cache should not be hit. - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16)); - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - EXPECT_NEAR((16_s - 3_ms).to(), - estimatedPose.value().timestamp.to(), 1e-6); - - // And again -- pose cache should result in returning std::nullopt - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - EXPECT_FALSE(estimatedPose); - - // Setting ReferencePose should also clear the cache - estimator.SetReferencePose( - wpi::math::Pose3d(units::meter_t(1), units::meter_t(2), units::meter_t(3), - wpi::math::Rotation3d())); - - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - - ASSERT_TRUE(estimatedPose); - EXPECT_NEAR((16_s - 3_ms).to(), - estimatedPose.value().timestamp.to(), 1e-6); -} - -TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - - std::vector targets{ - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m), - wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.7, corners, detectedCorners}, - photon::PhotonTrackedTarget{ - 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, - wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m), - wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)), - 0.3, corners, detectedCorners}}; - - cameraOne.test = true; - cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - - photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - wpi::math::Transform3d{}); - - std::optional estimatedPose; - for (const auto& result : cameraOne.GetAllUnreadResults()) { - estimatedPose = estimator.Update(result); - } - ASSERT_TRUE(estimatedPose); - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - // Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy - EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), - .02); - 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(LegacyPhotonPoseEstimatorTest, CopyResult) { - std::vector targets{}; - - auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; - testResult.SetReceiveTimestamp(units::second_t(11)); - - auto test2 = testResult; - - EXPECT_NEAR(testResult.GetTimestamp().to(), - test2.GetTimestamp().to(), 0.001); -} - -TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { - photon::PhotonPoseEstimator estimator( - wpi::apriltag::AprilTagFieldLayout::LoadField( - wpi::apriltag::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d()); - - photon::PhotonPipelineResult result; - auto estimate = estimator.Update(result); - EXPECT_FALSE(estimate.has_value()); -} - -TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { - photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - auto distortion = Eigen::VectorXd::Zero(8); - auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5}, - {0, 399.16666666666674, 239.5}, - {0, 0, 1}}; - - // Create corners data matching the Java test - std::vector corners8{ - photon::TargetCorner{98.09875447066685, 331.0093220119495}, - photon::TargetCorner{122.20226758624413, 335.50083894738486}, - photon::TargetCorner{127.17118732489361, 313.81406314178633}, - photon::TargetCorner{104.28543773760417, 309.6516557438994}}; - - wpi::math::Transform3d poseTransform( - wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m, - 0.48678786477534686_m), - wpi::math::Rotation3d( - wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333, - -0.08413452932300695, 0.9130568172784148))); - - std::vector targets{ - photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform, - poseTransform, 0.0, corners8, corners8}}; - - auto multiTagResult = std::make_optional( - photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0}, - std::vector{8}); - - photon::PhotonPipelineResult result{ - photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult}; - - cameraOne.test = true; - cameraOne.testResult = {result}; - cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - - const units::radian_t camPitch = 30_deg; - const wpi::math::Transform3d kRobotToCam{ - wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), - wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; - - photon::PhotonPoseEstimator estimator( - wpi::apriltag::AprilTagFieldLayout::LoadField( - wpi::apriltag::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, kRobotToCam); - - estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), - wpi::math::Rotation2d()); - - auto estimatedPose = - estimator.Update(cameraOne.testResult[0], cameraMat, distortion, - photon::ConstrainedSolvepnpParams{true, 0}); - - ASSERT_TRUE(estimatedPose.has_value()); - - wpi::math::Pose3d pose = estimatedPose.value().estimatedPose; - - EXPECT_NEAR(3.58, units::unit_cast(pose.X()), 0.01); - EXPECT_NEAR(4.13, units::unit_cast(pose.Y()), 0.01); - EXPECT_NEAR(0.0, units::unit_cast(pose.Z()), 0.01); - - EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy); -}