RobotPoseEstimator
- class photonvision.RobotPoseEstimator(aprilTags: robotpy_apriltag._apriltag.AprilTagFieldLayout, strategy: photonvision._photonvision.PoseStrategy, cameras: List[Tuple[photonvision._photonvision.PhotonCamera, wpimath.geometry._geometry.Transform3d]])
Bases:
pybind11_object
The RobotPoseEstimator 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.
Create a new RobotPoseEstimator.
Example: {@code <code> Map<Integer, Pose3d> map = new HashMap<>(); map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) </code> }
- 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.
cameras – An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from the center of the robot to the cameras.
- getFieldLayout() robotpy_apriltag._apriltag.AprilTagFieldLayout
Get the AprilTagFieldLayout being used by the PositionEstimator.
- Returns:
the AprilTagFieldLayout
- getPoseStrategy() photonvision._photonvision.PoseStrategy
Get the Position Estimation Strategy being used by the Position Estimator.
- Returns:
the strategy
- getReferencePose() wpimath.geometry._geometry.Pose3d
Return the reference position that is being used by the estimator.
- Returns:
the referencePose
- setCameras(cameras: List[Tuple[photonvision._photonvision.PhotonCamera, wpimath.geometry._geometry.Transform3d]]) None
Set the cameras to be used by the PoseEstimator.
- Parameters:
cameras – cameras to set.
- setLastPose(lastPose: wpimath.geometry._geometry.Pose3d) None
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
- setPoseStrategy(strat: photonvision._photonvision.PoseStrategy) None
Set the Position Estimation Strategy used by the Position Estimator.
- Parameters:
strategy – the strategy to set
- setReferencePose(referencePose: wpimath.geometry._geometry.Pose3d) None
Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
- Parameters:
referencePose – the referencePose to set
- update() Tuple[wpimath.geometry._geometry.Pose3d, seconds]