The PhotonPoseEstimator class filters or combines readings from all the fiducials visible at a given timestamp on the field to produce a single robot in field pose, using the strategy set below.
More...
#include <photon/PhotonPoseEstimator.h>
The PhotonPoseEstimator class filters or combines readings from all the fiducials visible at a given timestamp on the field to produce a single robot in field pose, using the strategy set below.
Example usage can be found in our apriltagExample example project.
◆ PhotonPoseEstimator()
photon::PhotonPoseEstimator::PhotonPoseEstimator |
( |
frc::AprilTagFieldLayout | aprilTags, |
|
|
PoseStrategy | strategy, |
|
|
frc::Transform3d | robotToCamera ) |
|
explicit |
Create a new PhotonPoseEstimator.
- Parameters
-
aprilTags | A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the FIRST field. |
strategy | The strategy it should use to determine the best pose. |
robotToCamera | Transform3d from the center of the robot to the camera mount positions (ie, robot ➔ camera). |
◆ GetFieldLayout()
frc::AprilTagFieldLayout photon::PhotonPoseEstimator::GetFieldLayout |
( |
| ) |
const |
|
inline |
Get the AprilTagFieldLayout being used by the PositionEstimator.
- Returns
- the AprilTagFieldLayout
◆ GetPoseStrategy()
PoseStrategy photon::PhotonPoseEstimator::GetPoseStrategy |
( |
| ) |
const |
|
inline |
Get the Position Estimation Strategy being used by the Position Estimator.
- Returns
- the strategy
◆ GetReferencePose()
frc::Pose3d photon::PhotonPoseEstimator::GetReferencePose |
( |
| ) |
const |
|
inline |
Return the reference position that is being used by the estimator.
- Returns
- the referencePose
◆ GetRobotToCameraTransform()
frc::Transform3d photon::PhotonPoseEstimator::GetRobotToCameraTransform |
( |
| ) |
|
|
inline |
- Returns
- The current transform from the center of the robot to the camera mount position.
◆ SetLastPose()
void photon::PhotonPoseEstimator::SetLastPose |
( |
frc::Pose3d | lastPose | ) |
|
|
inline |
Update the stored last pose.
Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy.
- Parameters
-
lastPose | the lastPose to set |
◆ SetMultiTagFallbackStrategy()
void photon::PhotonPoseEstimator::SetMultiTagFallbackStrategy |
( |
PoseStrategy | strategy | ) |
|
Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen.
Must NOT be MULTI_TAG_PNP
- Parameters
-
strategy | the strategy to set |
◆ SetPoseStrategy()
void photon::PhotonPoseEstimator::SetPoseStrategy |
( |
PoseStrategy | strat | ) |
|
|
inline |
Set the Position Estimation Strategy used by the Position Estimator.
- Parameters
-
strategy | the strategy to set |
◆ SetReferencePose()
void photon::PhotonPoseEstimator::SetReferencePose |
( |
frc::Pose3d | referencePose | ) |
|
|
inline |
Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
- Parameters
-
referencePose | the referencePose to set |
◆ SetRobotToCameraTransform()
void photon::PhotonPoseEstimator::SetRobotToCameraTransform |
( |
frc::Transform3d | robotToCamera | ) |
|
|
inline |
Useful for pan and tilt mechanisms, or cameras on turrets.
- Parameters
-
robotToCamera | The current transform from the center of the robot to the camera mount position. |
◆ Update()
Update the pose estimator.
If updating multiple times per loop, you should call this exactly once per new result, in order of increasing result timestamp.
- Parameters
-
result | The vision targeting result to process |
cameraIntrinsics | The camera calibration pinhole coefficients matrix. Only required if doing multitag-on-rio, and may be nullopt otherwise. |
distCoeffsData | The camera calibration distortion coefficients. Only required if doing multitag-on-rio, and may be nullopt otherwise. |
The documentation for this class was generated from the following file: