From 545e016d0466355287784ff00a7ae64c5ad440aa Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 13 Feb 2023 18:22:22 -0800 Subject: [PATCH] Cache pose calculations in PhotonPoseEstimator (#788) * Add pose caching to Java * Refactor strategy fallthrough * Hopefully add pose caching to C++ * Make Java switch same order as enum and C++ switch * C++ absolute value in timestamp check * Fix Java NPE * Use `units::second_t` in timestamp Co-authored-by: Matt * Expand Java unit test * Copy comments into C++ * Add tests to C++ * Run format * Update PhotonCamera.cpp * Probably fix bad access exception * a * init timestamp * Remove prints --------- Co-authored-by: Matt Co-authored-by: Joseph Eng --- .../org/photonvision/PhotonPoseEstimator.java | 38 ++++++++++- .../native/cpp/photonlib/PhotonCamera.cpp | 5 +- .../cpp/photonlib/PhotonPoseEstimator.cpp | 30 ++++++-- .../include/photonlib/PhotonPoseEstimator.h | 14 +++- .../photonvision/PhotonPoseEstimatorTest.java | 68 +++++++++++++++++++ .../native/cpp/PhotonPoseEstimatorTest.cpp | 55 ++++++++++++++- 6 files changed, 198 insertions(+), 12 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index ec0498287..b1df1eef9 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -78,6 +78,7 @@ public class PhotonPoseEstimator { private Pose3d lastPose; private Pose3d referencePose; + protected double poseCacheTimestampSeconds = -1; private final Set reportedErrors = new HashSet<>(); /** @@ -105,6 +106,17 @@ public class PhotonPoseEstimator { this.robotToCamera = robotToCamera; } + /** Invalidates the pose cache. */ + private void invalidatePoseCache() { + poseCacheTimestampSeconds = -1; + } + + private void checkUpdate(Object oldObj, Object newObj) { + if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { + invalidatePoseCache(); + } + } + /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * @@ -120,6 +132,7 @@ public class PhotonPoseEstimator { * @param fieldTags the AprilTagFieldLayout */ public void setFieldTags(AprilTagFieldLayout fieldTags) { + checkUpdate(this.fieldTags, fieldTags); this.fieldTags = fieldTags; } @@ -138,6 +151,7 @@ public class PhotonPoseEstimator { * @param strategy the strategy to set */ public void setPrimaryStrategy(PoseStrategy strategy) { + checkUpdate(this.primaryStrategy, strategy); this.primaryStrategy = strategy; } @@ -148,6 +162,7 @@ public class PhotonPoseEstimator { * @param strategy the strategy to set */ public void setMultiTagFallbackStrategy(PoseStrategy strategy) { + checkUpdate(this.multiTagFallbackStrategy, strategy); if (strategy == PoseStrategy.MULTI_TAG_PNP) { DriverStation.reportWarning( "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null); @@ -172,6 +187,7 @@ public class PhotonPoseEstimator { * @param referencePose the referencePose to set */ public void setReferencePose(Pose3d referencePose) { + checkUpdate(this.referencePose, referencePose); this.referencePose = referencePose; } @@ -182,7 +198,7 @@ public class PhotonPoseEstimator { * @param referencePose the referencePose to set */ public void setReferencePose(Pose2d referencePose) { - this.referencePose = new Pose3d(referencePose); + setReferencePose(new Pose3d(referencePose)); } /** @@ -247,6 +263,22 @@ public class PhotonPoseEstimator { * pipeline results used to create the estimate */ public Optional update(PhotonPipelineResult cameraResult) { + // 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(); } @@ -264,9 +296,11 @@ public class PhotonPoseEstimator { case CLOSEST_TO_CAMERA_HEIGHT: estimatedPose = closestToCameraHeightStrategy(cameraResult); break; + case CLOSEST_TO_REFERENCE_POSE: + estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); + break; case CLOSEST_TO_LAST_POSE: setReferencePose(lastPose); - case CLOSEST_TO_REFERENCE_POSE: estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); break; case AVERAGE_BEST_TARGETS: diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 18c950278..82c079d0c 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -74,7 +74,10 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} PhotonPipelineResult PhotonCamera::GetLatestResult() { - if (test) return testResult; + if (test) { + return testResult; + } + // Prints warning if not connected VerifyVersion(); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index c9e59c49e..5b7ff5485 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -24,6 +24,7 @@ #include "photonlib/PhotonPoseEstimator.h" +#include #include #include #include @@ -39,6 +40,7 @@ #include #include #include +#include #include #include "photonlib/PhotonCamera.h" @@ -64,7 +66,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, camera(std::move(cam)), m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()) {} + referencePose(frc::Pose3d()), + poseCacheTimestamp(-1_s) {} void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { if (strategy == MULTI_TAG_PNP) { @@ -74,6 +77,9 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { ""); strategy = LOWEST_AMBIGUITY; } + if (this->multiTagFallbackStrategy != strategy) { + InvalidatePoseCache(); + } multiTagFallbackStrategy = strategy; } @@ -84,6 +90,22 @@ std::optional PhotonPoseEstimator::Update() { std::optional PhotonPoseEstimator::Update( const PhotonPipelineResult& result) { + // Time in the past -- give up, since the following if expects times > 0 + if (result.GetTimestamp() < 0_s) { + 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 && + 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; } @@ -118,11 +140,7 @@ std::optional PhotonPoseEstimator::Update( default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", ""); - return std::nullopt; - } - - if (!ret) { - // TODO + ret = std::nullopt; } return ret; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 01c91778e..5d76a851e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -109,7 +109,12 @@ class PhotonPoseEstimator { * * @param strategy the strategy to set */ - inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; } + inline void SetPoseStrategy(PoseStrategy strat) { + if (strategy != strat) { + InvalidatePoseCache(); + } + strategy = strat; + } /** * Set the Position Estimation Strategy used in multi-tag mode when @@ -133,6 +138,9 @@ class PhotonPoseEstimator { * @param referencePose the referencePose to set */ inline void SetReferencePose(frc::Pose3d referencePose) { + if (this->referencePose != referencePose) { + InvalidatePoseCache(); + } this->referencePose = referencePose; } @@ -186,6 +194,10 @@ class PhotonPoseEstimator { frc::Pose3d lastPose; frc::Pose3d referencePose; + units::second_t poseCacheTimestamp; + + inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } + std::optional Update(PhotonPipelineResult result, PoseStrategy strategy); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 209d4a1c0..a6de5a351 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -25,6 +25,8 @@ package org.photonvision; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -384,6 +386,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); + cameraOne.result.setTimestampSeconds(1); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -469,6 +472,71 @@ class PhotonPoseEstimatorTest { assertEquals(1, pose.getZ(), .01); } + @Test + void cacheIsInvalidated() { + PhotonCameraInjector cameraOne = new PhotonCameraInjector(); + var result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 0, + 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))))); + result.setTimestampSeconds(20); + + PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + aprilTags, + PoseStrategy.AVERAGE_BEST_TARGETS, + cameraOne, + new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + + // Empty result, expect empty result + cameraOne.result = new PhotonPipelineResult(); + cameraOne.result.setTimestampSeconds(1); + Optional estimatedPose = estimator.update(); + assertFalse(estimatedPose.isPresent()); + + // Set actual result + cameraOne.result = result; + estimatedPose = estimator.update(); + 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(); + 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(); + assertEquals(20, estimatedPose.get().timestampSeconds, .01); + assertEquals(20, estimator.poseCacheTimestampSeconds); + } + @Test void averageBestPoses() { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 3ae4d578e..7c67f02d1 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -231,6 +231,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { estimator.SetLastPose( frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); auto estimatedPose = estimator.Update(); + ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; wpi::SmallVector targetsThree{ @@ -257,12 +258,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { 0.4, corners, detectedCorners}}; estimator.GetCamera().testResult = {2_ms, targetsThree}; - estimator.GetCamera().testResult.SetTimestamp(units::second_t(7)); + estimator.GetCamera().testResult.SetTimestamp(units::second_t(21)); estimatedPose = estimator.Update(); + ASSERT_TRUE(estimatedPose); pose = estimatedPose.value().estimatedPose; - EXPECT_NEAR(7.0, units::unit_cast(estimatedPose.value().timestamp), + 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); @@ -310,3 +312,52 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { EXPECT_NEAR(2.15, units::unit_cast(pose.Y()), .01); EXPECT_NEAR(2.15, units::unit_cast(pose.Z()), .01); } + +TEST(PhotonPoseEstimatorTest, PoseCache) { + photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2"); + + wpi::SmallVector targets{ + photonlib::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 0, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photonlib::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photonlib::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + + photonlib::PhotonPoseEstimator estimator( + aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); + + // empty input, expect empty out + estimator.GetCamera().testResult = {2_ms, {}}; + estimator.GetCamera().testResult.SetTimestamp(units::second_t(1)); + auto estimatedPose = estimator.Update(); + EXPECT_FALSE(estimatedPose); + + // Set result, and update -- expect present and timestamp to be 15 + estimator.GetCamera().testResult = {3_ms, targets}; + estimator.GetCamera().testResult.SetTimestamp(units::second_t(15)); + estimatedPose = estimator.Update(); + EXPECT_TRUE(estimatedPose); + EXPECT_NEAR(15, estimatedPose.value().timestamp.to(), 1e-6); + + // And again -- now pose cache should be empty + estimatedPose = estimator.Update(); + EXPECT_FALSE(estimatedPose); +}