Adds Trig Solve and Constrained PNP Pose Strategies to C++ photonlib

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

@@ -48,6 +48,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"
 
@@ -97,7 +99,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,
@@ -120,13 +123,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;
 
@@ -159,6 +164,13 @@ 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;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                       "");
@@ -475,4 +487,123 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
                             result.GetTimestamp(), result.GetTargets(),
                             AVERAGE_BEST_TARGETS};
 }
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(
+    photon::PhotonPipelineResult result) {
+  photon::PhotonTrackedTarget bestTarget = result.GetBestTarget();
+
+  if (bestTarget.fiducialId == -1) {
+    return {};
+  }
+
+  if (!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
+    return {};
+  }
+
+  frc::Rotation2d headingSample{
+      headingBuffer.Sample(result.GetTimestamp()).value()};
+
+  frc::Translation2d camToTagTranslation{
+      frc::Translation3d{
+          bestTarget.GetBestCameraToTarget().Translation().Norm(),
+          frc::Rotation3d{0_deg, -units::degree_t{bestTarget.GetPitch()},
+                          -units::degree_t{bestTarget.GetYaw()}}}
+          .RotateBy(m_robotToCamera.Rotation())
+          .ToTranslation2d()
+          .RotateBy(headingSample)};
+
+  std::optional<frc::Pose3d> tagPose =
+      aprilTags.GetTagPose(bestTarget.GetFiducialId());
+
+  if (!tagPose.has_value()) {
+    return {};
+  }
+
+  frc::Pose2d tagPose2d{tagPose->ToPose2d()};
+
+  frc::Translation2d fieldToCameraTranslation{tagPose2d.Translation() +
+                                              (-camToTagTranslation)};
+
+  frc::Translation2d camToRobotTranslation{
+      (-m_robotToCamera.Translation().ToTranslation2d())
+          .RotateBy(headingSample)};
+
+  frc::Pose2d robotPose{fieldToCameraTranslation + camToRobotTranslation,
+                        headingSample};
+
+  return EstimatedRobotPose{frc::Pose3d{robotPose}, result.GetTimestamp(),
+                            result.GetTargets(),
+                            PoseStrategy::PNP_DISTANCE_TRIG_SOLVE};
+}
+
+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,7 +28,9 @@
 
 #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>
 
 #include "photon/PhotonCamera.h"
@@ -47,6 +49,13 @@ enum PoseStrategy {
   AVERAGE_BEST_TARGETS,
   MULTI_TAG_PNP_ON_COPROCESSOR,
   MULTI_TAG_PNP_ON_RIO,
+  CONSTRAINED_SOLVEPNP,
+  PNP_DISTANCE_TRIG_SOLVE
+};
+
+struct ConstrainedSolvepnpParams {
+  bool headingFree{false};
+  double headingScalingFactor{0.0};
 };
 
 struct EstimatedRobotPose {
@@ -182,11 +191,24 @@ 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);
+
+  void AddHeadingData(units::second_t timestamp, frc::Rotation3d heading) {
+    AddHeadingData(timestamp, heading.ToRotation2d());
+  }
+
+  void AddHeadingData(units::second_t timestamp, frc::Rotation2d heading) {
+    headingBuffer.AddSample(timestamp, heading.Radians());
+  }
 
  private:
   frc::AprilTagFieldLayout aprilTags;
@@ -198,6 +220,8 @@ class PhotonPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
+  frc::TimeInterpolatableBuffer<units::radian_t> headingBuffer{1_s};
+
   units::second_t poseCacheTimestamp;
 
   inline static int InstanceCount = 0;
@@ -216,13 +240,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);
 
   /**
@@ -286,6 +311,15 @@ class PhotonPoseEstimator {
    */
   std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
       PhotonPipelineResult result);
+
+  std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
+      photon::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