diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 2b67dd2d5..28d40a41a 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -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(1_s)) { HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, InstanceCount); InstanceCount++; @@ -159,6 +161,9 @@ std::optional 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 PhotonPoseEstimator::MultiTagOnRioStrategy( MULTI_TAG_PNP_ON_RIO); } +std::optional +PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) { + PhotonTrackedTarget bestTarget = result.GetBestTarget(); + std::optional 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 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 PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { std::vector>> diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index a20ea775c..7998ab718 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #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 headingBuffer; + inline static int InstanceCount = 1; inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } @@ -278,6 +337,16 @@ class PhotonPoseEstimator { std::optional camMat, std::optional 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 PnpDistanceTrigSolveStrategy( + PhotonPipelineResult result); + /** * Return the average of the best target poses using ambiguity as weight. diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index a4922c61d..74eac6e43 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/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(pose.Z()), .01); } +TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + cameraOne.test = true; + + std::vector 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 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(realPose.X()), + units::unit_cast(pose.X()), .01); + EXPECT_NEAR(units::unit_cast(realPose.Y()), + units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(units::unit_cast(realPose.Z()), + units::unit_cast(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(realPose.X()), + units::unit_cast(pose.X()), .01); + EXPECT_NEAR(units::unit_cast(realPose.Y()), + units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(units::unit_cast(realPose.Z()), + units::unit_cast(pose.Z()), .01); +} + TEST(PhotonPoseEstimatorTest, AverageBestPoses) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test");