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>

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();
-  return Update(result);
+  if (!camera) {
+    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,
-                                           PoseStrategy strategy);
+  std::optional<EstimatedRobotPose> Update(
+      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.