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>

Public Member Functions
	PhotonPoseEstimator (frc::AprilTagFieldLayout aprilTags, frc::Transform3d robotToCamera)
	Create a new PhotonPoseEstimator.

	PhotonPoseEstimator (frc::AprilTagFieldLayout aprilTags, PoseStrategy strategy, frc::Transform3d robotToCamera)
	Create a new PhotonPoseEstimator.

frc::AprilTagFieldLayout	GetFieldLayout () const
	Get the AprilTagFieldLayout being used by the PositionEstimator.

PoseStrategy	GetPoseStrategy () const
	Get the Position Estimation Strategy being used by the Position Estimator.

void	SetPoseStrategy (PoseStrategy strat)
	Set the Position Estimation Strategy used by the Position Estimator.

void	SetMultiTagFallbackStrategy (PoseStrategy strategy)
	Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen.

frc::Pose3d	GetReferencePose () const
	Return the reference position that is being used by the estimator.

void	SetReferencePose (frc::Pose3d referencePose)
	Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.

frc::Transform3d	GetRobotToCameraTransform ()

void	SetRobotToCameraTransform (frc::Transform3d robotToCamera)
	Useful for pan and tilt mechanisms, or cameras on turrets.

void	SetLastPose (frc::Pose3d lastPose)
	Update the stored last pose.

void	AddHeadingData (units::second_t timestamp, frc::Rotation2d heading)
	Add robot heading data to the buffer.

void	AddHeadingData (units::second_t timestamp, frc::Rotation3d heading)
	Add robot heading data to the buffer.

void	ResetHeadingData (units::second_t timestamp, frc::Rotation2d heading)
	Clears all heading data in the buffer, and adds a new seed.

void	ResetHeadingData (units::second_t timestamp, frc::Rotation3d heading)
	Clears all heading data in the buffer, and adds a new seed.

std::optional< EstimatedRobotPose >	Update (const photon::PhotonPipelineResult &result, std::optional< photon::PhotonCamera::CameraMatrix > cameraMatrixData=std::nullopt, std::optional< photon::PhotonCamera::DistortionMatrix > coeffsData=std::nullopt, std::optional< ConstrainedSolvepnpParams > constrainedPnpParams=std::nullopt)
	Update the pose estimator.

std::optional< EstimatedRobotPose >	EstimateLowestAmbiguityPose (PhotonPipelineResult cameraResult)
	Return the estimated position of the robot with the lowest position ambiguity from a List of pipeline results.

std::optional< EstimatedRobotPose >	EstimateClosestToCameraHeightPose (PhotonPipelineResult cameraResult)
	Return the estimated position of the robot using the target with the lowest delta height difference between the estimated and actual height of the camera.

std::optional< EstimatedRobotPose >	EstimateClosestToReferencePose (PhotonPipelineResult cameraResult, frc::Pose3d referencePose)
	Return the estimated position of the robot using the target with the lowest delta in the vector magnitude between it and the reference pose.

std::optional< EstimatedRobotPose >	EstimateCoprocMultiTagPose (PhotonPipelineResult cameraResult)
	Return the estimated position of the robot by using all visible tags to compute a single pose estimate on coprocessor.

std::optional< EstimatedRobotPose >	EstimateRioMultiTagPose (PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat, PhotonCamera::DistortionMatrix distCoeffs)
	Return the estimated position of the robot by using all visible tags to compute a single pose estimate on the RoboRIO.

std::optional< EstimatedRobotPose >	EstimatePnpDistanceTrigSolvePose (PhotonPipelineResult cameraResult)
	Return the estimated position of the robot by using distance data from best visible tag to compute a Pose.

std::optional< EstimatedRobotPose >	EstimateAverageBestTargetsPose (PhotonPipelineResult cameraResult)
	Return the average of the best target poses using ambiguity as weight.

std::optional< EstimatedRobotPose >	EstimateConstrainedSolvepnpPose (photon::PhotonPipelineResult cameraResult, photon::PhotonCamera::CameraMatrix cameraMatrix, photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose, bool headingFree, double headingScaleFactor)
	Return the estimated position of the robot by solving a constrained version of the Perspective-n-Point problem with the robot's drivebase flat on the floor.

Detailed Description

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.

Constructor & Destructor Documentation

◆ PhotonPoseEstimator() [1/2]

photon::PhotonPoseEstimator::PhotonPoseEstimator	(	frc::AprilTagFieldLayout	aprilTags,
		frc::Transform3d	robotToCamera )

explicit

Create a new PhotonPoseEstimator.

Parameters

aprilTags	A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the FIRST field.
robotToCamera	Transform3d from the center of the robot to the camera mount positions (ie, robot ➔ camera).

◆ PhotonPoseEstimator() [2/2]

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

Deprecated: Use individual estimation methods with the 2 argument constructor instead.

Member Function Documentation

◆ AddHeadingData() [1/2]

void photon::PhotonPoseEstimator::AddHeadingData	(	units::second_t	timestamp,
		frc::Rotation2d	heading )

inline

Add robot heading data to the buffer.

Must be called periodically for the PNP_DISTANCE_TRIG_SOLVE strategy.

Parameters

timestamp	Timestamp of the robot heading data.
heading	Field-relative heading at the given timestamp. Standard WPILIB field coordinates.

◆ AddHeadingData() [2/2]

void photon::PhotonPoseEstimator::AddHeadingData	(	units::second_t	timestamp,
		frc::Rotation3d	heading )

inline

Add robot heading data to the buffer.

Must be called periodically for the PNP_DISTANCE_TRIG_SOLVE strategy.

Parameters

timestamp	Timestamp of the robot heading data.
heading	Field-relative heading at the given timestamp. Standard WPILIB coordinates.

◆ EstimateAverageBestTargetsPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateAverageBestTargetsPose ( PhotonPipelineResult cameraResult )

Return the average of the best target poses using ambiguity as weight.

Parameters

cameraResult A pipeline result from the camera.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's no targets.

◆ EstimateClosestToCameraHeightPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateClosestToCameraHeightPose ( PhotonPipelineResult cameraResult )

Return the estimated position of the robot using the target with the lowest delta height difference between the estimated and actual height of the camera.

Parameters

cameraResult A pipeline result from the camera.

Returns: An EstimatedRobotPose with an estimated pose, timestamp and targets used to create the estimate, or std::nullopt if there's no targets.

◆ EstimateClosestToReferencePose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateClosestToReferencePose	(	PhotonPipelineResult	cameraResult,
		frc::Pose3d	referencePose )

Return the estimated position of the robot using the target with the lowest delta in the vector magnitude between it and the reference pose.

Parameters

cameraResult	A pipeline result from the camera.
referencePose	reference pose to check vector magnitude difference against.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's no targets.

◆ EstimateConstrainedSolvepnpPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateConstrainedSolvepnpPose	(	photon::PhotonPipelineResult	cameraResult,
		photon::PhotonCamera::CameraMatrix	cameraMatrix,
		photon::PhotonCamera::DistortionMatrix	distCoeffs,
		frc::Pose3d	seedPose,
		bool	headingFree,
		double	headingScaleFactor )

Return the estimated position of the robot by solving a constrained version of the Perspective-n-Point problem with the robot's drivebase flat on the floor.

This computation takes place on the RoboRIO, and typically takes not more than 2ms. See photon::VisionEstimation::EstimateRobotPoseConstrainedSolvePNP for tuning handles this strategy exposes. Internally, the cost function is a sum-squared of pixel reprojection error + (optionally) heading error * heading scale factor. This strategy needs addHeadingData called every frame so heading data is up-to-date.

Parameters

cameraResult	A pipeline result from the camera.
cameraMatrix	Camera intrinsics from camera calibration data.
distCoeffs	Distortion coefficients from camera calibration data.
seedPose	An initial guess at robot pose, refined via optimization. Better guesses will converge faster. Can come from any pose source, but some battle-tested sources are estimateCoprocMultiTagPose, or estimateLowestAmbiguityPose if MultiTag results aren't available.
headingFree	If true, heading is completely free to vary. If false, heading excursions from the provided heading measurement will be penalized
headingScaleFactor	If headingFree is false, this weights the cost of changing our robot heading estimate against the tag corner reprojection error cost.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's no targets or heading data, or if the solver fails to solve the problem.

◆ EstimateCoprocMultiTagPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateCoprocMultiTagPose ( PhotonPipelineResult cameraResult )

Return the estimated position of the robot by using all visible tags to compute a single pose estimate on coprocessor.

This option needs to be enabled on the PhotonVision web UI as well.

Parameters

cameraResult A pipeline result from the camera.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate or std::nullopt if there's no targets, no multi-tag results, or multi-tag is disabled in the web UI.

◆ EstimateLowestAmbiguityPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateLowestAmbiguityPose ( PhotonPipelineResult cameraResult )

Return the estimated position of the robot with the lowest position ambiguity from a List of pipeline results.

Parameters

cameraResult A pipeline result from the camera.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's no targets.

◆ EstimatePnpDistanceTrigSolvePose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose ( PhotonPipelineResult cameraResult )

Return the estimated position of the robot by using distance data from best visible tag to compute a Pose.

This runs on the RoboRIO in order to access the robot's yaw heading, and MUST have AddHeadingData called every frame so heading data is up-to-date.

Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)

https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98

Parameters

cameraResult A pipeline result from the camera.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's no targets or heading data.

◆ EstimateRioMultiTagPose()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::EstimateRioMultiTagPose	(	PhotonPipelineResult	cameraResult,
		PhotonCamera::CameraMatrix	camMat,
		PhotonCamera::DistortionMatrix	distCoeffs )

Return the estimated position of the robot by using all visible tags to compute a single pose estimate on the RoboRIO.

This can take a lot of time due to the RIO's weak computing power.

Parameters

cameraResult	A pipeline result from the camera.
cameraMatrix	Camera intrinsics from camera calibration data.
distCoeffs	Distortion coefficients from camera calibration data.

Returns: An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or std::nullopt if there's less than 2 targets visible or SolvePnP fails.

◆ 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

Deprecated: Use individual estimation methods instead.

◆ GetReferencePose()

frc::Pose3d photon::PhotonPoseEstimator::GetReferencePose ( ) const

inline

Return the reference position that is being used by the estimator.

Returns: the referencePose

Deprecated: Use individual estimation methods instead.

◆ GetRobotToCameraTransform()

frc::Transform3d photon::PhotonPoseEstimator::GetRobotToCameraTransform ( )

inline

Returns: The current transform from the center of the robot to the camera mount position.

◆ ResetHeadingData() [1/2]

void photon::PhotonPoseEstimator::ResetHeadingData	(	units::second_t	timestamp,
		frc::Rotation2d	heading )

inline

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.

Parameters

timestamp	Timestamp of the robot heading data.
heading	Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

◆ ResetHeadingData() [2/2]

void photon::PhotonPoseEstimator::ResetHeadingData	(	units::second_t	timestamp,
		frc::Rotation3d	heading )

inline

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.

Parameters

timestamp	Timestamp of the robot heading data.
heading	Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

◆ 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

Deprecated: Use individual estimation methods instead.

◆ 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

Deprecated: Use individual estimation methods instead.

◆ SetPoseStrategy()

void photon::PhotonPoseEstimator::SetPoseStrategy ( PoseStrategy strat )

inline

Set the Position Estimation Strategy used by the Position Estimator.

Parameters

strategy the strategy to set

Deprecated: Use individual estimation methods instead.

◆ 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

Deprecated: Use individual estimation methods instead.

◆ 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()

std::optional< EstimatedRobotPose > photon::PhotonPoseEstimator::Update	(	const photon::PhotonPipelineResult &	result,
		std::optional< photon::PhotonCamera::CameraMatrix >	cameraMatrixData = std::nullopt,
		std::optional< photon::PhotonCamera::DistortionMatrix >	coeffsData = std::nullopt,
		std::optional< ConstrainedSolvepnpParams >	constrainedPnpParams = std::nullopt )

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.
constrainedPnpParams	Constrained SolvePNP params, if needed.

Deprecated: Use individual estimation methods instead.

The documentation for this class was generated from the following file:

photon/PhotonPoseEstimator.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ PhotonPoseEstimator() [1/2]

◆ PhotonPoseEstimator() [2/2]

Member Function Documentation

◆ AddHeadingData() [1/2]

◆ AddHeadingData() [2/2]

◆ EstimateAverageBestTargetsPose()

◆ EstimateClosestToCameraHeightPose()

◆ EstimateClosestToReferencePose()

◆ EstimateConstrainedSolvepnpPose()

◆ EstimateCoprocMultiTagPose()

◆ EstimateLowestAmbiguityPose()

◆ EstimatePnpDistanceTrigSolvePose()

◆ EstimateRioMultiTagPose()

◆ GetFieldLayout()

◆ GetPoseStrategy()

◆ GetReferencePose()

◆ GetRobotToCameraTransform()

◆ ResetHeadingData() [1/2]

◆ ResetHeadingData() [2/2]

◆ SetLastPose()

◆ SetMultiTagFallbackStrategy()

◆ SetPoseStrategy()

◆ SetReferencePose()

◆ SetRobotToCameraTransform()

◆ Update()