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.
 

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

@@ -38,6 +38,8 @@
 #include "photon/PhotonCamera.h"
 #include "photon/PhotonPoseEstimator.h"
 #include "photon/dataflow/structures/Packet.h"
+#include "photon/simulation/PhotonCameraSim.h"
+#include "photon/simulation/SimCameraProperties.h"
 #include "photon/targeting/MultiTargetPNPResult.h"
 #include "photon/targeting/PhotonPipelineResult.h"
 #include "photon/targeting/PhotonTrackedTarget.h"
@@ -306,6 +308,84 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
   EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
 }
 
+TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
+  photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
+  cameraOne.test = true;
+
+  std::vector<photon::VisionTargetSim> targets;
+  targets.reserve(tags.size());
+  for (const auto& tag : tags) {
+    targets.push_back(
+        photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
+  }
+  photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
+      &cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
+
+  /* Compound Rolled + Pitched + Yaw */
+  frc::Transform3d compoundTestTransform = frc::Transform3d(
+      -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
+
+  photon::PhotonPoseEstimator estimator(
+      aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
+
+  /* real pose of the robot base to test against */
+  frc::Pose3d realPose =
+      frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
+
+  photon::PhotonPipelineResult result = cameraOneSim.Process(
+      1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
+      targets);
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(17_s);
+
+  estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
+
+  std::optional<photon::EstimatedRobotPose> estimatedPose;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
+              units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
+              units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
+              units::unit_cast<double>(pose.Z()), .01);
+
+  /* Straight on */
+  frc::Transform3d straightOnTestTransform =
+      frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
+
+  estimator.SetRobotToCameraTransform(straightOnTestTransform);
+  realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
+                         frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
+  result = cameraOneSim.Process(
+      1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
+      targets);
+  cameraOne.testResult = {result};
+  cameraOne.testResult[0].SetReceiveTimestamp(18_s);
+
+  estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
+
+  estimatedPose = std::nullopt;
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
+              units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
+              units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
+              units::unit_cast<double>(pose.Z()), .01);
+}
+
 TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
   photon::PhotonCamera cameraOne = photon::PhotonCamera("test");