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