Add PhotonPoseEstimator constructor without a PhotonCamera instance (#840)

- Made alternate constructor for ```PhotonPoseEstimator``` that doesn't need ```PhotonCamera```
- Changed ```update``` function to take in missing cameraMatrixData and coeffsData that were previously received from PhotonCamera.
- Changed the internal ```update``` and ```multiTagPNP``` function to take in cameraMatrixData & coeffsData
- If not needed for the specified strategy then the parameters are simply not used. Also if PhotonCamera is used in constructor it instead backs up to that.

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

.gitignore

@@ -10,6 +10,8 @@ Python/app/handlers/__pycache__/
 
 \.vscode/
 
+/.vs
+
 backend/settings/
 /.vscode/
 # Compiled class file

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -26,12 +26,16 @@ package org.photonvision;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.wpilibj.DriverStation;
 import java.util.ArrayList;
 import java.util.HashSet;
@@ -85,14 +89,14 @@ public class PhotonPoseEstimator {
      * Create a new PhotonPoseEstimator.
      *
      * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
-     *     with respect to the FIRST field using the <a
+     *     with respect to the FIRST field using the <a href=
-     *     href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
+     *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
      *     Coordinate System</a>.
      * @param strategy The strategy it should use to determine the best pose.
      * @param camera PhotonCamera
      * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
-     *     robot ➔ camera) in the <a
+     *     robot ➔ camera) in the <a href=
-     *     href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
+     *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
      *     Coordinate System</a>.
      */
     public PhotonPoseEstimator(
@@ -106,6 +110,14 @@ public class PhotonPoseEstimator {
         this.robotToCamera = robotToCamera;
     }
 
+    public PhotonPoseEstimator(
+            AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
+        this.fieldTags = fieldTags;
+        this.primaryStrategy = strategy;
+        this.camera = null;
+        this.robotToCamera = robotToCamera;
+    }
+
     /** Invalidates the pose cache. */
     private void invalidatePoseCache() {
         poseCacheTimestampSeconds = -1;
@@ -251,7 +263,7 @@ public class PhotonPoseEstimator {
 
         PhotonPipelineResult cameraResult = camera.getLatestResult();
 
-        return update(cameraResult);
+        return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
     }
 
     /**
@@ -263,12 +275,33 @@ public class PhotonPoseEstimator {
      *     pipeline results used to create the estimate
      */
     public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
+        if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
+        return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
+    }
+
+    /**
+     * Updates the estimated position of the robot. Returns empty if there are no cameras set or no
+     * targets were found from the cameras.
+     *
+     * @param cameraResult The latest pipeline result from the camera
+     * @param cameraMatrixData Camera calibration data that can be used in the case of no assigned
+     *     PhotonCamera.
+     * @param coeffsData Camera calibration data that can be used in the case of no assigned
+     *     PhotonCamera
+     * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
+     *     pipeline results used to create the estimate
+     */
+    public Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult,
+            Optional<Matrix<N3, N3>> cameraMatrixData,
+            Optional<Matrix<N5, N1>> coeffsData) {
         // 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
+        // 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) {
@@ -283,11 +316,14 @@ public class PhotonPoseEstimator {
             return Optional.empty();
         }
 
-        return update(cameraResult, this.primaryStrategy);
+        return update(cameraResult, cameraMatrixData, coeffsData, this.primaryStrategy);
     }
 
     private Optional<EstimatedRobotPose> update(
-            PhotonPipelineResult cameraResult, PoseStrategy strat) {
+            PhotonPipelineResult cameraResult,
+            Optional<Matrix<N3, N3>> cameraMatrixData,
+            Optional<Matrix<N5, N1>> coeffsData,
+            PoseStrategy strat) {
         Optional<EstimatedRobotPose> estimatedPose;
         switch (strat) {
             case LOWEST_AMBIGUITY:
@@ -307,7 +343,7 @@ public class PhotonPoseEstimator {
                 estimatedPose = averageBestTargetsStrategy(cameraResult);
                 break;
             case MULTI_TAG_PNP:
-                estimatedPose = multiTagPNPStrategy(cameraResult);
+                estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrixData, coeffsData);
                 break;
             default:
                 DriverStation.reportError(
@@ -322,7 +358,10 @@ public class PhotonPoseEstimator {
         return estimatedPose;
     }
 
-    private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
+    private Optional<EstimatedRobotPose> multiTagPNPStrategy(
+            PhotonPipelineResult result,
+            Optional<Matrix<N3, N3>> cameraMatrixOpt,
+            Optional<Matrix<N5, N1>> distCoeffsOpt) {
         // Arrays we need declared up front
         var visCorners = new ArrayList<TargetCorner>();
         var knownVisTags = new ArrayList<AprilTag>();
@@ -331,7 +370,7 @@ public class PhotonPoseEstimator {
 
         if (result.getTargets().size() < 2) {
             // Run fallback strategy instead
-            return update(result, this.multiTagFallbackStrategy);
+            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
         }
 
         for (var target : result.getTargets()) {
@@ -352,8 +391,6 @@ public class PhotonPoseEstimator {
             fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
         }
 
-        var cameraMatrixOpt = camera.getCameraMatrix();
-        var distCoeffsOpt = camera.getDistCoeffs();
         boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
 
         // multi-target solvePNP

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

@@ -58,12 +58,23 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
                                    units::meter_t cornerY, frc::Pose3d tagPose);
 }  // namespace detail
 
+PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+                                         PoseStrategy strat,
+                                         frc::Transform3d robotToCamera)
+    : aprilTags(tags),
+      strategy(strat),
+      camera(nullptr),
+      m_robotToCamera(robotToCamera),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()),
+      poseCacheTimestamp(-1_s) {}
+
 PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          PoseStrategy strat, PhotonCamera&& cam,
                                          frc::Transform3d robotToCamera)
     : aprilTags(tags),
       strategy(strat),
-      camera(std::move(cam)),
+      camera(std::make_shared<PhotonCamera>(std::move(cam))),
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
@@ -84,12 +95,27 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
-  auto result = camera.GetLatestResult();
+  if (!camera) {
-  return Update(result);
+    FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!",
+                    "");
+    return std::nullopt;
+  }
+  auto result = camera->GetLatestResult();
+  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result) {
+  // If camera is null, best we can do is pass null calibration data
+  if (!camera) {
+    return Update(result, std::nullopt, std::nullopt, this->strategy);
+  }
+  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+    const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
+    std::optional<cv::Mat> coeffsData) {
   // Time in the past -- give up, since the following if expects times > 0
   if (result.GetTimestamp() < 0_s) {
     return std::nullopt;
@@ -110,11 +136,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     return std::nullopt;
   }
 
-  return Update(result, this->strategy);
+  return Update(result, cameraMatrixData, coeffsData, this->strategy);
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
-    PhotonPipelineResult result, PoseStrategy strategy) {
+    PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
+    std::optional<cv::Mat> coeffsData, PoseStrategy strategy) {
   std::optional<EstimatedRobotPose> ret = std::nullopt;
 
   switch (strategy) {
@@ -135,7 +162,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       ret = AverageBestTargetsStrategy(result);
       break;
     case ::photonlib::MULTI_TAG_PNP:
-      ret = MultiTagPnpStrategy(result);
+      ret = MultiTagPnpStrategy(result, coeffsData, cameraMatrixData);
       break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -328,11 +355,13 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
-    PhotonPipelineResult result) {
+    PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+    std::optional<cv::Mat> distCoeffs) {
   using namespace frc;
 
   if (!result.HasTargets() || result.GetTargets().size() < 2) {
-    return Update(result, this->multiTagFallbackStrategy);
+    return Update(result, std::nullopt, std::nullopt,
+                  this->multiTagFallbackStrategy);
   }
 
   auto const targets = result.GetTargets();
@@ -364,8 +393,6 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
   cv::Mat const rvec(3, 1, cv::DataType<double>::type);
   cv::Mat const tvec(3, 1, cv::DataType<double>::type);
 
-  auto const camMat = camera.GetCameraMatrix();
-  auto const distCoeffs = camera.GetDistCoeffs();
   if (!camMat || !distCoeffs) {
     return std::nullopt;
   }

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -90,6 +90,23 @@ class PhotonPoseEstimator {
                                PoseStrategy strategy, PhotonCamera&& camera,
                                frc::Transform3d robotToCamera);
 
+  /**
+   * Create a new PhotonPoseEstimator.
+   *
+   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
+   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
+   * at (1.0,2.0,3.0) </code> }
+   *
+   * @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).
+   */
+  explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
+                               PoseStrategy strategy,
+                               frc::Transform3d robotToCamera);
+
   /**
    * Get the AprilTagFieldLayout being used by the PositionEstimator.
    *
@@ -181,14 +198,22 @@ class PhotonPoseEstimator {
    */
   std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
 
-  inline PhotonCamera& GetCamera() { return camera; }
+  /**
+   * Update the pose estimator.
+   */
+  std::optional<EstimatedRobotPose> Update(
+      const PhotonPipelineResult& result,
+      std::optional<cv::Mat> cameraMatrixData,
+      std::optional<cv::Mat> coeffsData);
+
+  inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
 
  private:
   frc::AprilTagFieldLayout aprilTags;
   PoseStrategy strategy;
   PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
 
-  PhotonCamera camera;
+  std::shared_ptr<PhotonCamera> camera;
   frc::Transform3d m_robotToCamera;
 
   frc::Pose3d lastPose;
@@ -198,8 +223,9 @@ class PhotonPoseEstimator {
 
   inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
 
-  std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
+  std::optional<EstimatedRobotPose> Update(
-                                           PoseStrategy strategy);
+      PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
+      std::optional<cv::Mat> coeffsData, PoseStrategy strategy);
 
   /**
    * Return the estimated position of the robot with the lowest position
@@ -242,7 +268,8 @@ class PhotonPoseEstimator {
    timestamp of this estimation.
    */
   std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
-      PhotonPipelineResult result);
+      PhotonPipelineResult result, std::optional<cv::Mat> camMat,
+      std::optional<cv::Mat> distCoeffs);
 
   /**
    * Return the average of the best target poses using ambiguity as weight.

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -257,8 +257,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           0.4, corners, detectedCorners}};
 
-  estimator.GetCamera().testResult = {2_ms, targetsThree};
+  estimator.GetCamera()->testResult = {2_ms, targetsThree};
-  estimator.GetCamera().testResult.SetTimestamp(units::second_t(21));
+  estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21));
 
   estimatedPose = estimator.Update();
   ASSERT_TRUE(estimatedPose);
@@ -345,14 +345,14 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
       aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
 
   // empty input, expect empty out
-  estimator.GetCamera().testResult = {2_ms, {}};
+  estimator.GetCamera()->testResult = {2_ms, {}};
-  estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
+  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 = {3_ms, targets};
-  estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
+  estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15));
   estimatedPose = estimator.Update();
   EXPECT_TRUE(estimatedPose);
   EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);