Add Constrained PNP Pose Strategies to C++ photonlib (#1908)

This adds the two missing pose strategies from the java version of
photonlib (Constrained PNP and the Trig solve), to C++ photonlib

---------

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

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

@@ -49,6 +49,8 @@
 #include <units/time.h>
 
 #include "photon/PhotonCamera.h"
+#include "photon/estimation/TargetModel.h"
+#include "photon/estimation/VisionEstimation.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
 
@@ -99,7 +101,8 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result,
     std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
-    std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
+    std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
+    std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
   // Time in the past -- give up, since the following if expects times > 0
   if (result.GetTimestamp() < 0_s) {
     FRC_ReportError(frc::warn::Warning,
@@ -122,13 +125,15 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     return std::nullopt;
   }
 
-  return Update(result, cameraMatrixData, cameraDistCoeffs, this->strategy);
+  return Update(result, cameraMatrixData, cameraDistCoeffs,
+                constrainedPnpParams, this->strategy);
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result,
     std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
     std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
+    std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
     PoseStrategy strategy) {
   std::optional<EstimatedRobotPose> ret = std::nullopt;
 
@@ -161,6 +166,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
                         "");
       }
       break;
+    case CONSTRAINED_SOLVEPNP:
+      ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
+                                   constrainedPnpParams);
+      break;
     case PNP_DISTANCE_TRIG_SOLVE:
       ret = PnpDistanceTrigSolveStrategy(result);
       break;
@@ -527,4 +536,74 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
                             result.GetTimestamp(), result.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);
+  }
+
+  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::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);
+
+  if (!pnpResult) {
+    return Update(result, camMat, distCoeffs, {},
+                  this->multiTagFallbackStrategy);
+  }
+
+  frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
+
+  return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
+                            PoseStrategy::CONSTRAINED_SOLVEPNP};
+}
 }  // namespace photon

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

@@ -28,6 +28,7 @@
 
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
 #include <frc/geometry/Transform3d.h>
 #include <frc/interpolation/TimeInterpolatableBuffer.h>
 #include <opencv2/core/mat.hpp>
@@ -48,7 +49,13 @@ enum PoseStrategy {
   AVERAGE_BEST_TARGETS,
   MULTI_TAG_PNP_ON_COPROCESSOR,
   MULTI_TAG_PNP_ON_RIO,
-  PNP_DISTANCE_TRIG_SOLVE,
+  CONSTRAINED_SOLVEPNP,
+  PNP_DISTANCE_TRIG_SOLVE
+};
+
+struct ConstrainedSolvepnpParams {
+  bool headingFree{false};
+  double headingScalingFactor{0.0};
 };
 
 struct EstimatedRobotPose {
@@ -239,11 +246,16 @@ class PhotonPoseEstimator {
    * Only required if doing multitag-on-rio, and may be nullopt otherwise.
    * @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.
    */
   std::optional<EstimatedRobotPose> Update(
-      const PhotonPipelineResult& result,
-      std::optional<PhotonCamera::CameraMatrix> cameraMatrixData = std::nullopt,
-      std::optional<PhotonCamera::DistortionMatrix> coeffsData = std::nullopt);
+      const photon::PhotonPipelineResult& result,
+      std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
+          std::nullopt,
+      std::optional<photon::PhotonCamera::DistortionMatrix> coeffsData =
+          std::nullopt,
+      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
+          std::nullopt);
 
  private:
   frc::AprilTagFieldLayout aprilTags;
@@ -275,13 +287,14 @@ class PhotonPoseEstimator {
    */
   std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
                                            PoseStrategy strategy) {
-    return Update(result, std::nullopt, std::nullopt, strategy);
+    return Update(result, std::nullopt, std::nullopt, std::nullopt, strategy);
   }
 
   std::optional<EstimatedRobotPose> Update(
       const PhotonPipelineResult& result,
       std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
       std::optional<PhotonCamera::DistortionMatrix> coeffsData,
+      std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
       PoseStrategy strategy);
 
   /**
@@ -355,6 +368,12 @@ class PhotonPoseEstimator {
    */
   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

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

@@ -40,6 +40,7 @@
 #include "photon/dataflow/structures/Packet.h"
 #include "photon/simulation/PhotonCameraSim.h"
 #include "photon/simulation/SimCameraProperties.h"
+#include "photon/simulation/VisionTargetSim.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
@@ -583,3 +584,76 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
   EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
               test2.GetTimestamp().to<double>(), 0.001);
 }
+
+TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
+  photon::PhotonPoseEstimator estimator(
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
+      photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
+
+  photon::PhotonPipelineResult result;
+  auto estimate = estimator.Update(result);
+  EXPECT_FALSE(estimate.has_value());
+}
+
+TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+  auto distortion = Eigen::VectorXd::Zero(8);
+  auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
+                                   {0, 399.16666666666674, 239.5},
+                                   {0, 0, 1}};
+
+  // Create corners data matching the Java test
+  std::vector<photon::TargetCorner> corners8{
+      photon::TargetCorner{98.09875447066685, 331.0093220119495},
+      photon::TargetCorner{122.20226758624413, 335.50083894738486},
+      photon::TargetCorner{127.17118732489361, 313.81406314178633},
+      photon::TargetCorner{104.28543773760417, 309.6516557438994}};
+
+  frc::Transform3d poseTransform(
+      frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
+                         0.48678786477534686_m),
+      frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
+                                      -0.08413452932300695,
+                                      0.9130568172784148)));
+
+  std::vector<photon::PhotonTrackedTarget> targets{
+      photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
+                                  poseTransform, 0.0, corners8, corners8}};
+
+  auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
+      photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
+      std::vector<int16_t>{8});
+
+  photon::PhotonPipelineResult result{
+      photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
+      multiTagResult};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
+
+  const units::radian_t camPitch = 30_deg;
+  const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
+                                     frc::Rotation3d(0_rad, -camPitch, 0_rad)};
+
+  photon::PhotonPoseEstimator estimator(
+      frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
+      photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
+
+  estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
+                           frc::Rotation2d());
+
+  auto estimatedPose =
+      estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
+                       photon::ConstrainedSolvepnpParams{true, 0});
+
+  ASSERT_TRUE(estimatedPose.has_value());
+
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
+  EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
+  EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
+
+  EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
+}

photon-targeting/build.gradle

@@ -72,6 +72,8 @@ model {
             nativeUtils.useRequiredLibrary(it, "wpimath_shared")
             nativeUtils.useRequiredLibrary(it, "wpinet_shared")
             nativeUtils.useRequiredLibrary(it, "ntcore_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
         "${nativeName}JNI"(JniNativeLibrarySpec) {
 

photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp

@@ -0,0 +1,215 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "photon/estimation/VisionEstimation.h"
+
+#include <iostream>
+#include <utility>
+#include <vector>
+
+#include "photon/constrained_solvepnp/wrap/casadi_wrapper.h"
+#include "photon/estimation/OpenCVHelp.h"
+#include "photon/targeting/MultiTargetPNPResult.h"
+
+namespace photon {
+namespace VisionEstimation {
+
+std::vector<frc::AprilTag> GetVisibleLayoutTags(
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout) {
+  std::vector<frc::AprilTag> retVal{};
+  for (const auto& tag : visTags) {
+    int id = tag.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
+    }
+  }
+  return retVal;
+}
+
+std::optional<PnpResult> EstimateCamPosePNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
+    const std::vector<PhotonTrackedTarget>& visTags,
+    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
+  if (visTags.size() == 0) {
+    return PnpResult();
+  }
+
+  std::vector<photon::TargetCorner> corners{};
+  std::vector<frc::AprilTag> knownTags{};
+
+  for (const auto& tgt : visTags) {
+    int id = tgt.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
+      auto currentCorners = tgt.GetDetectedCorners();
+      corners.insert(corners.end(), currentCorners.begin(),
+                     currentCorners.end());
+    }
+  }
+  if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+    return PnpResult{};
+  }
+
+  std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
+
+  if (knownTags.size() == 1) {
+    auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs,
+                                                tagModel.GetVertices(), points);
+    if (!camToTag) {
+      return PnpResult{};
+    }
+    frc::Pose3d bestPose =
+        knownTags[0].pose.TransformBy(camToTag->best.Inverse());
+    frc::Pose3d altPose{};
+    if (camToTag->ambiguity != 0) {
+      altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse());
+    }
+    frc::Pose3d o{};
+    PnpResult result{};
+    result.best = frc::Transform3d{o, bestPose};
+    result.alt = frc::Transform3d{o, altPose};
+    result.ambiguity = camToTag->ambiguity;
+    result.bestReprojErr = camToTag->bestReprojErr;
+    result.altReprojErr = camToTag->altReprojErr;
+    return result;
+  } else {
+    std::vector<frc::Translation3d> objectTrls{};
+    for (const auto& tag : knownTags) {
+      auto verts = tagModel.GetFieldVertices(tag.pose);
+      objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
+    }
+    auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
+                                          points);
+    if (ret) {
+      // Invert best/alt transforms
+      ret->best = ret->best.Inverse();
+      ret->alt = ret->alt.Inverse();
+    }
+
+    return ret;
+  }
+}
+
+std::optional<photon::PnpResult> EstimateRobotPoseConstrainedSolvePNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
+    const std::vector<photon::PhotonTrackedTarget>& visTags,
+    const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed,
+    const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel,
+    bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac) {
+  if (visTags.size() == 0) {
+    return photon::PnpResult();
+  }
+
+  std::vector<photon::TargetCorner> corners{};
+  std::vector<frc::AprilTag> knownTags{};
+
+  for (const auto& tgt : visTags) {
+    int id = tgt.GetFiducialId();
+    auto maybePose = layout.GetTagPose(id);
+    if (maybePose) {
+      knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
+      auto currentCorners = tgt.GetDetectedCorners();
+      corners.insert(corners.end(), currentCorners.begin(),
+                     currentCorners.end());
+    }
+  }
+  if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+    return photon::PnpResult{};
+  }
+
+  std::vector<cv::Point2f> points =
+      photon::OpenCVHelp::CornersToPoints(corners);
+
+  cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
+  cv::eigen2cv(cameraMatrix, cameraMat);
+  cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
+  cv::eigen2cv(distCoeffs, distCoeffsMat);
+
+  cv::undistortImagePoints(points, points, cameraMat, distCoeffsMat);
+
+  Eigen::Matrix4d robotToCameraBase{
+      (Eigen::Matrix4d() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1)
+          .finished()};
+  Eigen::Matrix<constrained_solvepnp::casadi_real, 4, 4, Eigen::ColMajor>
+      robotToCamera{robot2Camera.ToMatrix() * robotToCameraBase};
+
+  Eigen::Matrix<constrained_solvepnp::casadi_real, 2, Eigen::Dynamic,
+                Eigen::ColMajor>
+      pointObservations{};
+  pointObservations.resize(2, points.size());
+  for (size_t i = 0; i < points.size(); i++) {
+    pointObservations(0, i) = points[i].x;
+    pointObservations(1, i) = points[i].y;
+  }
+
+  std::vector<frc::Translation3d> objectTrls{};
+  for (const auto& tag : knownTags) {
+    auto verts = tagModel.GetFieldVertices(tag.pose);
+    objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
+  }
+
+  Eigen::Matrix<constrained_solvepnp::casadi_real, 4, Eigen::Dynamic,
+                Eigen::ColMajor>
+      field2points{};
+  field2points.resize(4, objectTrls.size());
+  for (size_t i = 0; i < objectTrls.size(); i++) {
+    field2points(0, i) = objectTrls[i].X().value();
+    field2points(1, i) = objectTrls[i].Y().value();
+    field2points(2, i) = objectTrls[i].Z().value();
+    field2points(3, i) = 1;
+  }
+
+  frc::Pose2d guess2 = robotPoseSeed.ToPose2d();
+
+  constrained_solvepnp::CameraCalibration cameraCal{
+      cameraMatrix(0, 0),
+      cameraMatrix(1, 1),
+      cameraMatrix(0, 2),
+      cameraMatrix(1, 2),
+  };
+
+  Eigen::Matrix<constrained_solvepnp::casadi_real, 3, 1> guessMat{
+      guess2.X().value(), guess2.Y().value(),
+      guess2.Rotation().Radians().value()};
+
+  wpi::expected<constrained_solvepnp::RobotStateMat,
+                sleipnir::SolverExitCondition>
+      result = constrained_solvepnp::do_optimization(
+          headingFree, knownTags.size(), cameraCal, robotToCamera, guessMat,
+          field2points, pointObservations, gyroTheta.Radians().value(),
+          gyroErrorScaleFac);
+
+  if (!result.has_value()) {
+    return {};
+  } else {
+    photon::PnpResult res{};
+
+    res.best = frc::Transform3d{frc::Transform2d{
+        units::meter_t{result.value()[0]}, units::meter_t{result.value()[1]},
+        frc::Rotation2d{units::radian_t{result.value()[2]}}}};
+
+    return res;
+  }
+}
+
+}  // namespace VisionEstimation
+}  // namespace photon

photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h

@@ -42,7 +42,7 @@ static frc::Rotation3d NWU_TO_EDN{
 static frc::Rotation3d EDN_TO_NWU{
     (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()};
 
-static std::vector<cv::Point2f> GetConvexHull(
+[[maybe_unused]] static std::vector<cv::Point2f> GetConvexHull(
     const std::vector<cv::Point2f>& points) {
   std::vector<int> outputHull{};
   cv::convexHull(points, outputHull);

photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h

@@ -17,102 +17,36 @@
 
 #pragma once
 
-#include <utility>
 #include <vector>
 
 #include <Eigen/Core>
 #include <frc/apriltag/AprilTag.h>
 #include <frc/apriltag/AprilTagFieldLayout.h>
 
-#include "OpenCVHelp.h"
 #include "TargetModel.h"
-#include "photon/targeting/MultiTargetPNPResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
+#include "photon/targeting/PnpResult.h"
 
 namespace photon {
 namespace VisionEstimation {
 
-[[maybe_unused]] static std::vector<frc::AprilTag> GetVisibleLayoutTags(
+std::vector<frc::AprilTag> GetVisibleLayoutTags(
     const std::vector<PhotonTrackedTarget>& visTags,
-    const frc::AprilTagFieldLayout& layout) {
-  std::vector<frc::AprilTag> retVal{};
-  for (const auto& tag : visTags) {
-    int id = tag.GetFiducialId();
-    auto maybePose = layout.GetTagPose(id);
-    if (maybePose) {
-      retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
-    }
-  }
-  return retVal;
-}
+    const frc::AprilTagFieldLayout& layout);
 
-#include <iostream>
-
-[[maybe_unused]] static std::optional<PnpResult> EstimateCamPosePNP(
+std::optional<photon::PnpResult> EstimateCamPosePNP(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,
     const std::vector<PhotonTrackedTarget>& visTags,
-    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
-  if (visTags.size() == 0) {
-    return PnpResult();
-  }
+    const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel);
 
-  std::vector<photon::TargetCorner> corners{};
-  std::vector<frc::AprilTag> knownTags{};
-
-  for (const auto& tgt : visTags) {
-    int id = tgt.GetFiducialId();
-    auto maybePose = layout.GetTagPose(id);
-    if (maybePose) {
-      knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
-      auto currentCorners = tgt.GetDetectedCorners();
-      corners.insert(corners.end(), currentCorners.begin(),
-                     currentCorners.end());
-    }
-  }
-  if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
-    return PnpResult{};
-  }
-
-  std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
-
-  if (knownTags.size() == 1) {
-    auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs,
-                                                tagModel.GetVertices(), points);
-    if (!camToTag) {
-      return PnpResult{};
-    }
-    frc::Pose3d bestPose =
-        knownTags[0].pose.TransformBy(camToTag->best.Inverse());
-    frc::Pose3d altPose{};
-    if (camToTag->ambiguity != 0) {
-      altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse());
-    }
-    frc::Pose3d o{};
-    PnpResult result{};
-    result.best = frc::Transform3d{o, bestPose};
-    result.alt = frc::Transform3d{o, altPose};
-    result.ambiguity = camToTag->ambiguity;
-    result.bestReprojErr = camToTag->bestReprojErr;
-    result.altReprojErr = camToTag->altReprojErr;
-    return result;
-  } else {
-    std::vector<frc::Translation3d> objectTrls{};
-    for (const auto& tag : knownTags) {
-      auto verts = tagModel.GetFieldVertices(tag.pose);
-      objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
-    }
-    auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
-                                          points);
-    if (ret) {
-      // Invert best/alt transforms
-      ret->best = ret->best.Inverse();
-      ret->alt = ret->alt.Inverse();
-    }
-
-    return ret;
-  }
-}
+std::optional<photon::PnpResult> EstimateRobotPoseConstrainedSolvePNP(
+    const Eigen::Matrix<double, 3, 3>& cameraMatrix,
+    const Eigen::Matrix<double, 8, 1>& distCoeffs,
+    const std::vector<photon::PhotonTrackedTarget>& visTags,
+    const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed,
+    const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel,
+    bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac);
 
 }  // namespace VisionEstimation
 }  // namespace photon