Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add PNP_DISTANCE_TRIG_SOLVE strategy to C++ (#2021)

Browse Source
This commit is contained in:
cuttestkittensrule
2025-08-10 06:55:04 -07:00
committed by GitHub
parent 35dcc3ce5a
commit 8676649ebc
3 changed files with 202 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -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>>>