Add PNP_DISTANCE_TRIG_SOLVE strategy to C++ (#2021)

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

@@ -44,6 +44,7 @@
 #include <opencv2/calib3d.hpp>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/core/types.hpp>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/time.h>
 
@@ -73,7 +74,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
-      poseCacheTimestamp(-1_s) {
+      poseCacheTimestamp(-1_s),
+      headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
   HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
              InstanceCount);
   InstanceCount++;
@@ -159,6 +161,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
                         "");
       }
       break;
+    case PNP_DISTANCE_TRIG_SOLVE:
+      ret = PnpDistanceTrigSolveStrategy(result);
+      break;
     default:
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                       "");
@@ -429,6 +434,53 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
                                     MULTI_TAG_PNP_ON_RIO);
 }
 
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
+  PhotonTrackedTarget bestTarget = result.GetBestTarget();
+  std::optional<frc::Rotation2d> headingSampleOpt =
+      headingBuffer.Sample(result.GetTimestamp());
+  if (!headingSampleOpt) {
+    FRC_ReportError(frc::warn::Warning,
+                    "There was no heading data! Use AddHeadingData to add it!");
+    return std::nullopt;
+  }
+
+  frc::Rotation2d headingSample = headingSampleOpt.value();
+
+  frc::Translation2d camToTagTranslation =
+      frc::Translation3d(
+          bestTarget.GetBestCameraToTarget().Translation().Norm(),
+          frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()),
+                          -units::degree_t(bestTarget.GetYaw())))
+          .RotateBy(m_robotToCamera.Rotation())
+          .ToTranslation2d()
+          .RotateBy(headingSample);
+
+  std::optional<frc::Pose3d> fiducialPose =
+      aprilTags.GetTagPose(bestTarget.GetFiducialId());
+  if (!fiducialPose) {
+    FRC_ReportError(frc::warn::Warning,
+                    "Tried to get pose of unknown April Tag: {}",
+                    bestTarget.GetFiducialId());
+    return std::nullopt;
+  }
+
+  frc::Pose2d tagPose = fiducialPose.value().ToPose2d();
+
+  frc::Translation2d fieldToCameraTranslation =
+      tagPose.Translation() - camToTagTranslation;
+
+  frc::Translation2d camToRobotTranslation =
+      (-m_robotToCamera.Translation().ToTranslation2d())
+          .RotateBy(headingSample);
+
+  frc::Pose2d robotPose = frc::Pose2d(
+      fieldToCameraTranslation + camToRobotTranslation, headingSample);
+
+  return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
+                            result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
+}
+
 std::optional<EstimatedRobotPose>
 PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
   std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>

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

@@ -29,6 +29,7 @@
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Transform3d.h>
+#include <frc/interpolation/TimeInterpolatableBuffer.h>
 #include <opencv2/core/mat.hpp>
 
 #include "photon/PhotonCamera.h"
@@ -47,6 +48,7 @@ enum PoseStrategy {
   AVERAGE_BEST_TARGETS,
   MULTI_TAG_PNP_ON_COPROCESSOR,
   MULTI_TAG_PNP_ON_RIO,
+  PNP_DISTANCE_TRIG_SOLVE,
 };
 
 struct EstimatedRobotPose {
@@ -172,6 +174,61 @@ class PhotonPoseEstimator {
    */
   inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
 
+  /**
+   * Add robot heading data to the buffer. Must be called periodically for the
+   * PNP_DISTANCE_TRIG_SOLVE strategy.
+   *
+   * @param timestamp Timestamp of the robot heading data.
+   * @param heading Field-relative heading at the given timestamp. Standard
+   * WPILIB field coordinates.
+   */
+  inline void AddHeadingData(units::second_t timestamp,
+                             frc::Rotation2d heading) {
+    this->headingBuffer.AddSample(timestamp, heading);
+  }
+
+  /**
+   * Add robot heading data to the buffer. Must be called periodically for the
+   * PNP_DISTANCE_TRIG_SOLVE strategy.
+   *
+   * @param timestamp Timestamp of the robot heading data.
+   * @param heading Field-relative heading at the given timestamp. Standard
+   * WPILIB coordinates.
+   */
+  inline void AddHeadingData(units::second_t timestamp,
+                             frc::Rotation3d heading) {
+    AddHeadingData(timestamp, heading.ToRotation2d());
+  }
+
+  /**
+   * Clears all heading data in the buffer, and adds a new seed. Useful for
+   * preventing estimates from utilizing heading data provided prior to a pose
+   * or rotation reset.
+   *
+   * @param timestamp Timestamp of the robot heading data.
+   * @param heading Field-relative robot heading at given timestamp. Standard
+   * WPILIB field coordinates.
+   */
+  inline void ResetHeadingData(units::second_t timestamp,
+                               frc::Rotation2d heading) {
+    headingBuffer.Clear();
+    AddHeadingData(timestamp, heading);
+  }
+
+  /**
+   * Clears all heading data in the buffer, and adds a new seed. Useful for
+   * preventing estimates from utilizing heading data provided prior to a pose
+   * or rotation reset.
+   *
+   * @param timestamp Timestamp of the robot heading data.
+   * @param heading Field-relative robot heading at given timestamp. Standard
+   * WPILIB field coordinates.
+   */
+  inline void ResetHeadingData(units::second_t timestamp,
+                               frc::Rotation3d heading) {
+    ResetHeadingData(timestamp, heading.ToRotation2d());
+  }
+
   /**
    * Update the pose estimator. If updating multiple times per loop, you should
    * call this exactly once per new result, in order of increasing result
@@ -200,6 +257,8 @@ class PhotonPoseEstimator {
 
   units::second_t poseCacheTimestamp;
 
+  frc::TimeInterpolatableBuffer<frc::Rotation2d> headingBuffer;
+
   inline static int InstanceCount = 1;
 
   inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
@@ -278,6 +337,16 @@ class PhotonPoseEstimator {
       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.