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/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<Integer> 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<EstimatedRobotPose> 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:

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

photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp

@@ -24,6 +24,7 @@
 
 #include "photonlib/PhotonPoseEstimator.h"
 
+#include <cmath>
 #include <iostream>
 #include <limits>
 #include <map>
@@ -39,6 +40,7 @@
 #include <opencv2/calib3d.hpp>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/core/types.hpp>
+#include <units/math.h>
 #include <units/time.h>
 
 #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<EstimatedRobotPose> PhotonPoseEstimator::Update() {
 
 std::optional<EstimatedRobotPose> 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<EstimatedRobotPose> PhotonPoseEstimator::Update(
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                       "");
-      return std::nullopt;
+      ret = std::nullopt;
-  }
-
-  if (!ret) {
-    // TODO
   }
 
   return ret;

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<EstimatedRobotPose> Update(PhotonPipelineResult result,
                                            PoseStrategy strategy);
 

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);
+}