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 <matthew.morley.ca@gmail.com>

* 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 <matthew.morley.ca@gmail.com>
Co-authored-by: Joseph Eng <joseng2358@gmail.com>

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<EstimatedRobotPose> 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();

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<photonlib::PhotonTrackedTarget, 3> 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<double>(estimatedPose.value().timestamp),
+  EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
               .01);
   EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
@@ -310,3 +312,52 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
 }
+
+TEST(PhotonPoseEstimatorTest, PoseCache) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> 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<double>(), 1e-6);
+
+  // And again -- now pose cache should be empty
+  estimatedPose = estimator.Update();
+  EXPECT_FALSE(estimatedPose);
+}