Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +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

69
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

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