Open up pose estimator strategy methods (#2252)

2026-06-28 02:11:40 +00:00 · 2026-01-11 13:58:53 -05:00
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
--- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -24,13 +24,9 @@

 #include "photon/PhotonPoseEstimator.h"

-#include <cmath>
-#include <iostream>
 #include <limits>
-#include <map>
-#include <memory>
+#include <optional>
 #include <span>
-#include <string>
 #include <utility>
 #include <vector>

@@ -46,6 +42,7 @@
 #include <units/angle.h>
 #include <units/math.h>
 #include <units/time.h>
+#include <wpi/deprecated.h>

 #include "photon/PhotonCamera.h"
 #include "photon/estimation/TargetModel.h"
@@ -55,7 +52,7 @@

 #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
 #include <opencv2/core/eigen.hpp>
-
+WPI_IGNORE_DEPRECATED
 namespace photon {

 namespace detail {
@@ -67,6 +64,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
                                   units::meter_t cornerY, frc::Pose3d tagPose);
 }  // namespace detail

+PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+                                         frc::Transform3d robotToCamera)
+    : aprilTags(tags),
+      m_robotToCamera(robotToCamera),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()),
+      poseCacheTimestamp(-1_s),
+      headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
+  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
+             InstanceCount);
+  InstanceCount++;
+}
+
 PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                         PoseStrategy strat,
                                         frc::Transform3d robotToCamera)
@@ -138,39 +148,94 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(

  switch (strategy) {
    case LOWEST_AMBIGUITY:
-      ret = LowestAmbiguityStrategy(result);
+      ret = EstimateLowestAmbiguityPose(result);
      break;
    case CLOSEST_TO_CAMERA_HEIGHT:
-      ret = ClosestToCameraHeightStrategy(result);
+      ret = EstimateClosestToCameraHeightPose(result);
      break;
    case CLOSEST_TO_REFERENCE_POSE:
-      ret = ClosestToReferencePoseStrategy(result);
+      ret = EstimateClosestToReferencePose(result, this->referencePose);
      break;
    case CLOSEST_TO_LAST_POSE:
      SetReferencePose(lastPose);
-      ret = ClosestToReferencePoseStrategy(result);
+      ret = EstimateClosestToReferencePose(result, this->referencePose);
      break;
    case AVERAGE_BEST_TARGETS:
-      ret = AverageBestTargetsStrategy(result);
+      ret = EstimateAverageBestTargetsPose(result);
      break;
    case MULTI_TAG_PNP_ON_COPROCESSOR:
-      ret = MultiTagOnCoprocStrategy(result);
+      if (!result.MultiTagResult()) {
+        ret = Update(result, this->multiTagFallbackStrategy);
+      } else {
+        ret = EstimateCoprocMultiTagPose(result);
+      }
      break;
    case MULTI_TAG_PNP_ON_RIO:
-      if (cameraMatrixData && cameraDistCoeffs) {
-        ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
-      } else {
+      if (!cameraMatrixData && !cameraDistCoeffs) {
        FRC_ReportError(frc::warn::Warning,
                        "No camera calibration provided to multi-tag-on-rio!",
                        "");
+        ret = Update(result, this->multiTagFallbackStrategy);
+      }
+      ret =
+          EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
+      if (!ret) {
+        ret = Update(result, this->multiTagFallbackStrategy);
      }
      break;
-    case CONSTRAINED_SOLVEPNP:
-      ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
-                                   constrainedPnpParams);
+    case CONSTRAINED_SOLVEPNP: {
+      using namespace frc;
+
+      if (!cameraMatrixData || !cameraDistCoeffs) {
+        FRC_ReportError(
+            frc::warn::Warning,
+            "No camera calibration data provided for Constrained SolvePnP!");
+        ret = Update(result, this->multiTagFallbackStrategy);
+        break;
+      }
+
+      if (!constrainedPnpParams) {
+        return {};
+      }
+
+      if (!constrainedPnpParams->headingFree &&
+          !headingBuffer.Sample(result.GetTimestamp()).has_value()) {
+        ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                     this->multiTagFallbackStrategy);
+        break;
+      }
+
+      frc::Pose3d fieldToRobotSeed;
+
+      if (result.MultiTagResult().has_value()) {
+        fieldToRobotSeed =
+            frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
+                             m_robotToCamera.Inverse());
+      } else {
+        std::optional<EstimatedRobotPose> nestedUpdate =
+            Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                   this->multiTagFallbackStrategy);
+
+        if (!nestedUpdate.has_value()) {
+          return {};
+        }
+
+        fieldToRobotSeed = nestedUpdate->estimatedPose;
+      }
+
+      ret = EstimateConstrainedSolvepnpPose(
+          result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
+          constrainedPnpParams->headingFree,
+          constrainedPnpParams->headingScalingFactor);
+
+      if (!ret) {
+        ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
+                     this->multiTagFallbackStrategy);
+      }
      break;
+    }
    case PNP_DISTANCE_TRIG_SOLVE:
-      ret = PnpDistanceTrigSolveStrategy(result);
+      ret = EstimatePnpDistanceTrigSolvePose(result);
      break;
    default:
      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -184,10 +249,26 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
  return ret;
 }

-std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
-    PhotonPipelineResult result) {
+bool ShouldEstimate(const PhotonPipelineResult& result) {
+  // Time in the past -- give up, since the following if expects times > 0
+  if (result.GetTimestamp() < 0_s) {
+    FRC_ReportError(frc::warn::Warning,
+                    "Result timestamp was reported in the past!");
+    return false;
+  }
+
+  // If no targets seen, trivial case -- can't do estimation
+  return result.HasTargets();
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateLowestAmbiguityPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
  auto foundIt = targets.end();
  for (auto it = targets.begin(); it != targets.end(); ++it) {
    if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
@@ -214,18 +295,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
  return EstimatedRobotPose{
      fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
          .TransformBy(m_robotToCamera.Inverse()),
-      result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
+      cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY};
 }

 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::ClosestToCameraHeightStrategy(
-    PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
  units::meter_t smallestHeightDifference =
      units::meter_t(std::numeric_limits<double>::infinity());

  std::optional<EstimatedRobotPose> pose = std::nullopt;

-  for (auto& target : result.GetTargets()) {
+  for (auto& target : cameraResult.GetTargets()) {
    std::optional<frc::Pose3d> fiducialPose =
        aprilTags.GetTagPose(target.GetFiducialId());
    if (!fiducialPose) {
@@ -250,14 +334,16 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
      pose = EstimatedRobotPose{
          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
              .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          CLOSEST_TO_CAMERA_HEIGHT};
    }
    if (bestDifference < smallestHeightDifference) {
      smallestHeightDifference = bestDifference;
      pose = EstimatedRobotPose{
          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
              .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          CLOSEST_TO_CAMERA_HEIGHT};
    }
  }

@@ -265,14 +351,17 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
 }

 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::ClosestToReferencePoseStrategy(
-    PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateClosestToReferencePose(
+    PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
  units::meter_t smallestDifference =
      units::meter_t(std::numeric_limits<double>::infinity());
  units::second_t stateTimestamp = units::second_t(0);
  frc::Pose3d pose = lastPose;

-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
  for (auto& target : targets) {
    std::optional<frc::Pose3d> fiducialPose =
        aprilTags.GetTagPose(target.GetFiducialId());
@@ -298,17 +387,17 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
    if (alternativeDifference < smallestDifference) {
      smallestDifference = alternativeDifference;
      pose = altPose;
-      stateTimestamp = result.GetTimestamp();
+      stateTimestamp = cameraResult.GetTimestamp();
    }

    if (bestDifference < smallestDifference) {
      smallestDifference = bestDifference;
      pose = bestPose;
-      stateTimestamp = result.GetTimestamp();
+      stateTimestamp = cameraResult.GetTimestamp();
    }
  }

-  return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
+  return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(),
                            CLOSEST_TO_REFERENCE_POSE};
 }

@@ -361,41 +450,31 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
                Rotation3d(rv));
 }

-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
-    PhotonPipelineResult result) {
-  if (!result.MultiTagResult()) {
-    return Update(result, this->multiTagFallbackStrategy);
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateCoprocMultiTagPose(
+    PhotonPipelineResult cameraResult) {
+  if (!cameraResult.MultiTagResult() || !ShouldEstimate(cameraResult)) {
+    return std::nullopt;
  }

-  const auto field2camera = result.MultiTagResult()->estimatedPose.best;
+  const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;

  const auto fieldToRobot =
      frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
-  return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
-                                    result.GetTargets(),
+  return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
+                                    cameraResult.GetTargets(),
                                    MULTI_TAG_PNP_ON_COPROCESSOR);
 }

-std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
-    PhotonPipelineResult result,
-    std::optional<PhotonCamera::CameraMatrix> camMat,
-    std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
-  using namespace frc;
-
-  if (!camMat || !distCoeffs) {
-    FRC_ReportError(frc::warn::Warning,
-                    "No camera calibration data provided to "
-                    "PhotonPoseEstimator::MultiTagOnRioStrategy!",
-                    "");
-    return Update(result, this->multiTagFallbackStrategy);
-  }
-
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
+    PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix cameraMatrix,
+    PhotonCamera::DistortionMatrix distCoeffs) {
  // Need at least 2 targets
-  if (!result.HasTargets() || result.GetTargets().size() < 2) {
-    return Update(result, this->multiTagFallbackStrategy);
+  if (cameraResult.GetTargets().size() < 2 || !ShouldEstimate(cameraResult)) {
+    return std::nullopt;
  }

-  auto const targets = result.GetTargets();
+  const auto targets = cameraResult.GetTargets();

  // List of corners mapped from 3d space (meters) to the 2d camera screen
  // (pixels).
@@ -418,7 +497,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(

  // We should only do multi-tag if at least 2 tags (* 4 corners/tag)
  if (imagePoints.size() < 8) {
-    return Update(result, this->multiTagFallbackStrategy);
+    return std::nullopt;
  }

  // Output mats for results
@@ -426,27 +505,31 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
  cv::Mat const tvec(3, 1, cv::DataType<double>::type);

  {
-    cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
-    cv::eigen2cv(*camMat, cameraMatCV);
-    cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
-    cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
+    cv::Mat cameraMatCV(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+    cv::eigen2cv(cameraMatrix, cameraMatCV);
+    cv::Mat distCoeffsMatCV(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+    cv::eigen2cv(distCoeffs, distCoeffsMatCV);

    cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
                 tvec, false, cv::SOLVEPNP_SQPNP);
  }

-  const Pose3d pose = detail::ToPose3d(tvec, rvec);
+  const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);

-  return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
-                                    result.GetTimestamp(), result.GetTargets(),
-                                    MULTI_TAG_PNP_ON_RIO);
+  return photon::EstimatedRobotPose(
+      pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
+      cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO);
 }

 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
-  PhotonTrackedTarget bestTarget = result.GetBestTarget();
+PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
+  PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
  std::optional<frc::Rotation2d> headingSampleOpt =
-      headingBuffer.Sample(result.GetTimestamp());
+      headingBuffer.Sample(cameraResult.GetTimestamp());
  if (!headingSampleOpt) {
    FRC_ReportError(frc::warn::Warning,
                    "There was no heading data! Use AddHeadingData to add it!");
@@ -485,17 +568,21 @@ PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
  frc::Pose2d robotPose = frc::Pose2d(
      fieldToCameraTranslation + camToRobotTranslation, headingSample);

-  return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
-                            result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
+  return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
 }

 std::optional<EstimatedRobotPose>
-PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
+PhotonPoseEstimator::EstimateAverageBestTargetsPose(
+    PhotonPipelineResult cameraResult) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
      tempPoses;
  double totalAmbiguity = 0;

-  auto targets = result.GetTargets();
+  auto targets = cameraResult.GetTargets();
  for (auto& target : targets) {
    std::optional<frc::Pose3d> fiducialPose =
        aprilTags.GetTagPose(target.GetFiducialId());
@@ -512,13 +599,15 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
      return EstimatedRobotPose{
          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
              .TransformBy(m_robotToCamera.Inverse()),
-          result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
+          cameraResult.GetTimestamp(), cameraResult.GetTargets(),
+          AVERAGE_BEST_TARGETS};
    }
    totalAmbiguity += 1. / target.GetPoseAmbiguity();

    tempPoses.push_back(std::make_pair(
        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
-        std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
+        std::make_pair(target.GetPoseAmbiguity(),
+                       cameraResult.GetTimestamp())));
  }

  frc::Translation3d transform = frc::Translation3d();
@@ -532,77 +621,45 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
  }

  return EstimatedRobotPose{frc::Pose3d(transform, rotation),
-                            result.GetTimestamp(), result.GetTargets(),
-                            AVERAGE_BEST_TARGETS};
+                            cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
 }

-std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
-    photon::PhotonPipelineResult result,
-    std::optional<photon::PhotonCamera::CameraMatrix> camMat,
-    std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
-    std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
-  using namespace frc;
-
-  if (!camMat || !distCoeffs) {
-    FRC_ReportError(frc::warn::Warning,
-                    "No camera calibration data provided to "
-                    "StrPoseEstimator::MultiTagOnRioStrategy!",
-                    "");
-    return Update(result, this->multiTagFallbackStrategy);
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
+    photon::PhotonPipelineResult cameraResult,
+    photon::PhotonCamera::CameraMatrix cameraMatrix,
+    photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
+    bool headingFree, double headingScaleFactor) {
+  if (!ShouldEstimate(cameraResult)) {
+    return std::nullopt;
+  }
+  if (!headingFree) {
+    seedPose = frc::Pose3d{
+        seedPose.Translation(),
+        frc::Rotation3d{
+            headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
  }

-  if (!constrainedPnpParams) {
-    return {};
-  }
-
-  if (!constrainedPnpParams->headingFree &&
-      !headingBuffer.Sample(result.GetTimestamp()).has_value()) {
-    return Update(result, camMat, distCoeffs, {},
-                  this->multiTagFallbackStrategy);
-  }
-
-  frc::Pose3d fieldToRobotSeed;
-
-  if (result.MultiTagResult().has_value()) {
-    fieldToRobotSeed =
-        frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
-                         m_robotToCamera.Inverse());
-  } else {
-    std::optional<EstimatedRobotPose> nestedUpdate =
-        Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
-
-    if (!nestedUpdate.has_value()) {
-      return {};
-    }
-
-    fieldToRobotSeed = nestedUpdate->estimatedPose;
-  }
-
-  if (!constrainedPnpParams.value().headingFree) {
-    fieldToRobotSeed = frc::Pose3d{
-        fieldToRobotSeed.Translation(),
-        frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
-  }
-
-  std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
-                                                   result.GetTargets().end()};
+  std::vector<photon::PhotonTrackedTarget> targets{
+      cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};

  std::optional<photon::PnpResult> pnpResult =
      VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
-          camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
-          fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
-          constrainedPnpParams->headingFree,
-          frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
-          constrainedPnpParams->headingScalingFactor);
+          cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
+          aprilTags, photon::kAprilTag36h11, headingFree,
+          frc::Rotation2d{
+              headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
+          headingScaleFactor);

  if (!pnpResult) {
-    return Update(result, camMat, distCoeffs, {},
-                  this->multiTagFallbackStrategy);
+    return std::nullopt;
  }

  frc::Pose3d best = frc::Pose3d{} + pnpResult->best;

-  return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
+  return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
+                            cameraResult.GetTargets(),
                            PoseStrategy::CONSTRAINED_SOLVEPNP};
 }
 }  // namespace photon
--- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -24,8 +24,6 @@

 #pragma once

-#include <memory>
-
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
@@ -34,11 +32,8 @@
 #include <opencv2/core/mat.hpp>

 #include "photon/PhotonCamera.h"
-#include "photon/dataflow/structures/Packet.h"
-#include "photon/targeting/MultiTargetPNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
-#include "photon/targeting/PnpResult.h"

 namespace photon {
 enum PoseStrategy {
@@ -93,10 +88,26 @@ class PhotonPoseEstimator {
   *
   * @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,
+                               frc::Transform3d robotToCamera);
+
+  /**
+   * Create a new PhotonPoseEstimator.
+   *
+   * @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).
+   * @deprecated Use individual estimation methods with the 2 argument
+   * constructor instead.
+   */
+  [[deprecated(
+      "Use individual estimation methods with the 2 argument constructor "
+      "instead.")]]
  explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
                               PoseStrategy strategy,
                               frc::Transform3d robotToCamera);
@@ -112,14 +123,20 @@ class PhotonPoseEstimator {
   * Get the Position Estimation Strategy being used by the Position Estimator.
   *
   * @return the strategy
+   * @deprecated Use individual estimation methods instead.
   */
-  PoseStrategy GetPoseStrategy() const { return strategy; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  PoseStrategy GetPoseStrategy() const {
+    return strategy;
+  }

  /**
   * Set the Position Estimation Strategy used by the Position Estimator.
   *
   * @param strategy the strategy to set
+   * @deprecated Use individual estimation methods instead.
   */
+  [[deprecated("Use individual estimation methods instead.")]]
  inline void SetPoseStrategy(PoseStrategy strat) {
    if (strategy != strat) {
      InvalidatePoseCache();
@@ -132,22 +149,30 @@ class PhotonPoseEstimator {
   * only one tag can be seen. Must NOT be MULTI_TAG_PNP
   *
   * @param strategy the strategy to set
+   * @deprecated Use individual estimation methods instead.
   */
+  [[deprecated("Use individual estimation methods instead.")]]
  void SetMultiTagFallbackStrategy(PoseStrategy strategy);

  /**
   * Return the reference position that is being used by the estimator.
   *
   * @return the referencePose
+   * @deprecated Use individual estimation methods instead.
   */
-  frc::Pose3d GetReferencePose() const { return referencePose; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  frc::Pose3d GetReferencePose() const {
+    return referencePose;
+  }

  /**
   * Update the stored reference pose for use when using the
   * CLOSEST_TO_REFERENCE_POSE strategy.
   *
   * @param referencePose the referencePose to set
+   * @deprecated Use individual estimation methods instead.
   */
+  [[deprecated("Use individual estimation methods instead.")]]
  inline void SetReferencePose(frc::Pose3d referencePose) {
    if (this->referencePose != referencePose) {
      InvalidatePoseCache();
@@ -178,8 +203,12 @@ class PhotonPoseEstimator {
   * using the CLOSEST_TO_LAST_POSE strategy.
   *
   * @param lastPose the lastPose to set
+   * @deprecated Use individual estimation methods instead.
   */
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+  [[deprecated("Use individual estimation methods instead.")]]
+  inline void SetLastPose(frc::Pose3d lastPose) {
+    this->lastPose = lastPose;
+  }

  /**
   * Add robot heading data to the buffer. Must be called periodically for the
@@ -247,7 +276,9 @@ class PhotonPoseEstimator {
   * @param distCoeffsData The camera calibration distortion coefficients. Only
   * required if doing multitag-on-rio, and may be nullopt otherwise.
   * @param constrainedPnpParams Constrained SolvePNP params, if needed.
+   * @deprecated Use individual estimation methods instead.
   */
+  [[deprecated("Use individual estimation methods instead.")]]
  std::optional<EstimatedRobotPose> Update(
      const photon::PhotonPipelineResult& result,
      std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
@@ -257,6 +288,127 @@ class PhotonPoseEstimator {
      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
          std::nullopt);

+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
+      PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
+
+  /**
+   * Return the estimated position of the robot by using all visible tags to
+   * compute a single pose estimate on coprocessor. This option needs to be
+   * enabled on the PhotonVision web UI as well.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot by using all visible tags to
+   * compute a single pose estimate on the RoboRIO. This can take a lot of time
+   * due to the RIO's weak computing power.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param cameraMatrix Camera intrinsics from camera calibration data.
+   * @param distCoeffs Distortion coefficients from camera calibration data.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
+      PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
+      PhotonCamera::DistortionMatrix distCoeffs);
+
+  /**
+   * Return the estimated position of the robot by using distance data from best
+   * visible tag to compute a Pose. This runs on the RoboRIO in order to access
+   * the robot's yaw heading, and MUST have AddHeadingData called every frame so
+   * heading data is up-to-date.
+   *
+   * <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+   *
+   * <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+   * @param cameraResult A pipeline result from the camera.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
+      PhotonPipelineResult cameraResult);
+
+  /**
+   * Return the estimated position of the robot by solving a constrained version
+   * of the Perspective-n-Point problem with the robot's drivebase flat on the
+   * floor. This computation takes place on the RoboRIO, and typically takes not
+   * more than 2ms. See
+   * photon::VisionEstimation::EstimateRobotPoseConstrainedSolvePNP for tuning
+   * handles this strategy exposes. Internally, the cost function is a
+   * sum-squared of pixel reprojection error + (optionally) heading error *
+   * heading scale factor. This strategy needs addHeadingData called every frame
+   * so heading data is up-to-date.
+   *
+   * @param cameraResult A pipeline result from the camera.
+   * @param cameraMatrix Camera intrinsics from camera calibration data.
+   * @param distCoeffs Distortion coefficients from camera calibration data.
+   * @param seedPose An initial guess at robot pose, refined via optimization.
+   * Better guesses will converge faster. Can come from any pose source, but
+   * some battle-tested sources are estimateCoprocMultiTagPose, or
+   * estimateLowestAmbiguityPose if MultiTag results aren't available.
+   * @param headingFree If true, heading is completely free to vary. If false,
+   * heading excursions from the provided heading measurement will be penalized
+   * @param headingScaleFactor If headingFree is false, this weights the cost of
+   * changing our robot heading estimate against the tag corner reprojection
+   * error cost.
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
+      photon::PhotonPipelineResult cameraResult,
+      photon::PhotonCamera::CameraMatrix cameraMatrix,
+      photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
+      bool headingFree, double headingScaleFactor);
+
 private:
  frc::AprilTagFieldLayout aprilTags;
  PoseStrategy strategy;
@@ -280,9 +432,9 @@ class PhotonPoseEstimator {
   * This should only be called after timestamp checks have been done by another
   * update() overload.
   *
-   * @param cameraResult The latest pipeline result from the camera
+   * @param cameraResult A pipeline result from the camera.
   * @param strategy The pose strategy to use
-   * @return an EstimatedRobotPose with an estimated pose, timestamp, and
+   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
   * targets used to create the estimate.
   */
  std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
@@ -296,84 +448,6 @@ class PhotonPoseEstimator {
      std::optional<PhotonCamera::DistortionMatrix> coeffsData,
      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
      PoseStrategy strategy);
-
-  /**
-   * Return the estimated position of the robot with the lowest position
-   * ambiguity from a List of pipeline results.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta height difference between the estimated and actual height of the
-   * camera.
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the estimated position of the robot using the target with the lowest
-   * delta in the vector magnitude between it and the reference pose.
-   *
-   * @param referencePose reference pose to check vector magnitude difference
-   * against.
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the pose calculated by combining all tags into one on coprocessor
-   *
-   * @return the estimated position of the robot in the FCS
-   */
-  std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the pose calculation using all targets in view in the same PNP()
-   calculation
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
-      PhotonPipelineResult result,
-      std::optional<PhotonCamera::CameraMatrix> camMat,
-      std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
-
-  /**
-   * Return the pose calculation using the best visible tag and the robot's
-   * heading
-   *
-   * @return the estimated position of the robot in the FCS and the estimated
-   * timestamp of this estimation
-   */
-  std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
-      PhotonPipelineResult result);
-
-  /**
-   * Return the average of the best target poses using ambiguity as weight.
-
-   * @return the estimated position of the robot in the FCS and the estimated
-   timestamp of this estimation.
-   */
-  std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
-      PhotonPipelineResult result);
-
-  std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
-      photon::PhotonPipelineResult result,
-      std::optional<photon::PhotonCamera::CameraMatrix> camMat,
-      std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
-      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
 };

 }  // namespace photon