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