From 99427d888a4dff19d9495e557fe62677be97f46e Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Sat, 1 Feb 2025 14:09:43 -0800 Subject: [PATCH] Fix fallback for the multitag on rio pose strategy (#1755) --- .../org/photonvision/PhotonPoseEstimator.java | 82 ++++++++++--------- .../native/cpp/photon/PhotonPoseEstimator.cpp | 35 ++++---- .../include/photon/PhotonPoseEstimator.h | 15 ++++ .../photonvision/PhotonPoseEstimatorTest.java | 67 +++++++++++++++ .../native/cpp/PhotonPoseEstimatorTest.cpp | 43 ++++++++++ 5 files changed, 184 insertions(+), 58 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 7b518351b..2cb8b6e21 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -286,8 +286,8 @@ public class PhotonPoseEstimator { *
  • No targets were found in the pipeline results. * * - * Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not - * provided + * 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 @@ -341,6 +341,20 @@ public class PhotonPoseEstimator { return update(cameraResult, cameraMatrix, distCoeffs, 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 + * @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(), strategy); + } + private Optional update( PhotonPipelineResult cameraResult, Optional> cameraMatrix, @@ -357,21 +371,8 @@ public class PhotonPoseEstimator { yield closestToReferencePoseStrategy(cameraResult, referencePose); } case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult); - case MULTI_TAG_PNP_ON_RIO -> { - if (cameraMatrix.isEmpty()) { - DriverStation.reportWarning( - "Camera matrix is empty for multi-tag-on-rio", - Thread.currentThread().getStackTrace()); - yield Optional.empty(); - } else if (distCoeffs.isEmpty()) { - DriverStation.reportWarning( - "Camera matrix is empty for multi-tag-on-rio", - Thread.currentThread().getStackTrace()); - yield Optional.empty(); - } else { - yield multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - } - } + case MULTI_TAG_PNP_ON_RIO -> + multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult); }; @@ -383,41 +384,44 @@ public class PhotonPoseEstimator { } private Optional multiTagOnCoprocStrategy(PhotonPipelineResult result) { - if (result.getMultiTagResult().isPresent()) { - var best_tf = result.getMultiTagResult().get().estimatedPose.best; - var best = - new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()) - .plus(robotToCamera.inverse()); // field-to-robot - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - // We can never fall back on another multitag strategy - return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy); + if (result.getMultiTagResult().isEmpty()) { + return update(result, this.multiTagFallbackStrategy); } + + var best_tf = result.getMultiTagResult().get().estimatedPose.best; + var best = + new Pose3d() + .plus(best_tf) // field-to-camera + .relativeTo(fieldTags.getOrigin()) + .plus(robotToCamera.inverse()); // field-to-robot + return Optional.of( + new EstimatedRobotPose( + best, + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); } private Optional multiTagOnRioStrategy( PhotonPipelineResult result, Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) { + DriverStation.reportWarning( + "No camera calibration data provided for multi-tag-on-rio", + Thread.currentThread().getStackTrace()); + return update(result, this.multiTagFallbackStrategy); + } + + if (result.getTargets().size() < 2) { + return update(result, this.multiTagFallbackStrategy); } var pnpResult = VisionEstimation.estimateCamPosePNP( cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent()) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); + if (!pnpResult.isPresent()) return update(result, this.multiTagFallbackStrategy); var best = new Pose3d() .plus(pnpResult.get().best) // field-to-camera diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 64c15129e..2b67dd2d5 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -350,18 +350,17 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( PhotonPipelineResult result) { - if (result.MultiTagResult()) { - const auto field2camera = result.MultiTagResult()->estimatedPose.best; - - const auto fieldToRobot = - frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); - return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(), - result.GetTargets(), - MULTI_TAG_PNP_ON_COPROCESSOR); + if (!result.MultiTagResult()) { + return Update(result, this->multiTagFallbackStrategy); } - return Update(result, std::nullopt, std::nullopt, - this->multiTagFallbackStrategy); + const auto field2camera = result.MultiTagResult()->estimatedPose.best; + + const auto fieldToRobot = + frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); + return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(), + result.GetTargets(), + MULTI_TAG_PNP_ON_COPROCESSOR); } std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( @@ -370,19 +369,17 @@ std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( std::optional distCoeffs) { using namespace frc; - // Need at least 2 targets - if (!result.HasTargets() || result.GetTargets().size() < 2) { - return Update(result, std::nullopt, std::nullopt, - this->multiTagFallbackStrategy); - } - if (!camMat || !distCoeffs) { FRC_ReportError(frc::warn::Warning, "No camera calibration data provided to " "PhotonPoseEstimator::MultiTagOnRioStrategy!", ""); - return Update(result, std::nullopt, std::nullopt, - this->multiTagFallbackStrategy); + return Update(result, this->multiTagFallbackStrategy); + } + + // Need at least 2 targets + if (!result.HasTargets() || result.GetTargets().size() < 2) { + return Update(result, this->multiTagFallbackStrategy); } auto const targets = result.GetTargets(); @@ -408,7 +405,7 @@ std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( // We should only do multi-tag if at least 2 tags (* 4 corners/tag) if (imagePoints.size() < 8) { - return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy); + return Update(result, this->multiTagFallbackStrategy); } // Output mats for results diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 435334bff..7b129a666 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -204,6 +204,21 @@ class PhotonPoseEstimator { 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 The latest 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, strategy); + } + std::optional Update( const PhotonPipelineResult& result, std::optional cameraMatrixData, diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 35ae0593c..9cab92dda 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -24,6 +24,7 @@ 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.assertTrue; @@ -645,6 +646,72 @@ class PhotonPoseEstimatorTest { assertEquals(2.15, pose.getZ(), .01); } + @Test + void testMultiTagOnRioFallback() { + PhotonCameraInjector camera = new PhotonCameraInjector(); + camera.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(camera.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)); + } + private static class PhotonCameraInjector extends PhotonCamera { public PhotonCameraInjector() { super("Test"); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 0838c3e07..c95fa57ed 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -419,6 +419,49 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { EXPECT_FALSE(estimatedPose); } + +TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, + 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}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + 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 = {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, + frc::Transform3d{}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + ASSERT_TRUE(estimatedPose); + frc::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(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{};