mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Update PhotonPoseEstimator examples (#1903)
## Description Updates the PhotonPoseEstimator programming documentation page to reflect the new PhotonPoseEstimator. and add a more comprehensive step-by-step tutorial using code from the PV Pose Estimation examples. Updates various code examples to ensure they are matching the latest documentation or example code on GitHub. This PR is a successor of, and therefore closes #1765. This PR is blocked by #1706, as the linked PR updates examples. Closes #1757 and closes #1800 and closes #1632 and closes #1773 and closes #1465. ## Meta Merge checklist: - [x] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes - [x] The description documents the _what_ and _why_ - [x] If this PR changes behavior or adds a feature, user documentation is updated - [x] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly - [x] If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1 - [x] If this PR touches pipeline settings or anything related to data exchange, the frontend typing is updated - [x] If this PR addresses a bug, a regression test for it is added --------- Co-authored-by: Kevin Reas <76408202+PaarkG@users.noreply.github.com> Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
Alan
@@ -86,7 +86,7 @@ Each pipeline result has a `hasTargets()`/`HasTargets()` (Java and C++ respectiv
|
||||
```
|
||||
|
||||
:::{warning}
|
||||
In Java/C++, You must *always* check if the result has a target via `hasTargets()`/`HasTargets()` before getting targets or else you may get a null pointer exception. Further, you must use the same result in every subsequent call in that loop.
|
||||
In Java/C++, You must _always_ check if the result has a target via `hasTargets()`/`HasTargets()` before getting targets or else you may get a null pointer exception. Further, you must use the same result in every subsequent call in that loop.
|
||||
:::
|
||||
|
||||
## Getting a List of Targets
|
||||
@@ -140,7 +140,7 @@ You can get the {ref}`best target <docs/reflectiveAndShape/contour-filtering:Con
|
||||
|
||||
## Getting Data From A Target
|
||||
|
||||
- double `getYaw()`/`GetYaw()`: The yaw of the target in degrees (positive right).
|
||||
- double `getYaw()`/`GetYaw()`: The yaw of the target in degrees (positive left).
|
||||
- double `getPitch()`/`GetPitch()`: The pitch of the target in degrees (positive up).
|
||||
- double `getArea()`/`GetArea()`: The area (how much of the camera feed the bounding box takes up) as a percent (0-100).
|
||||
- double `getSkew()`/`GetSkew()`: The skew of the target in degrees (counter-clockwise positive).
|
||||
|
||||
@@ -4,30 +4,46 @@
|
||||
For more information on how to methods to get AprilTag data, look {ref}`here <docs/programming/photonlib/getting-target-data:Getting AprilTag Data From A Target>`.
|
||||
:::
|
||||
|
||||
PhotonLib includes a `PhotonPoseEstimator` class, which allows you to combine the pose data from all tags in view in order to get a field relative pose. The `PhotonPoseEstimator` class works with one camera per object instance, but more than one instance may be created.
|
||||
PhotonLib includes a `PhotonPoseEstimator` class, which allows you to combine the pose data from all tags in view in order to get a field relative pose. For each camera, a separate instance of the `PhotonPoseEstimator` class should be created.
|
||||
|
||||
## Creating an `AprilTagFieldLayout`
|
||||
|
||||
`AprilTagFieldLayout` is used to represent a layout of AprilTags within a space (field, shop at home, classroom, etc.). WPILib provides a JSON that describes the layout of AprilTags on the field which you can then use in the AprilTagFieldLayout constructor. You can also specify a custom layout.
|
||||
|
||||
The API documentation can be found in here: [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/apriltag/AprilTagFieldLayout.html) and [C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_april_tag_field_layout.html).
|
||||
The API documentation can be found in here: [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/apriltag/AprilTagFieldLayout.html), [C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_april_tag_field_layout.html), and [Python](https://robotpy.readthedocs.io/projects/apriltag/en/stable/robotpy_apriltag/AprilTagFieldLayout.html#robotpy_apriltag.AprilTagFieldLayout).
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. code-block:: Java
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java
|
||||
:language: java
|
||||
:lines: 48-49
|
||||
|
||||
// The field from AprilTagFields will be different depending on the game.
|
||||
AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Constants.h
|
||||
:language: c++
|
||||
:lines: 46-47
|
||||
|
||||
.. code-block:: C++
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 46
|
||||
```
|
||||
|
||||
// The parameter for LoadAPrilTagLayoutField will be different depending on the game.
|
||||
frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::kDefaultField);
|
||||
## Defining the Robot to Camera `Transform3d`
|
||||
|
||||
.. code-block:: Python
|
||||
Another necessary argument for creating a `PhotonPoseEstimator` is the `Transform3d` representing the robot-relative location and orientation of the camera. A `Transform3d` contains a `Translation3d` and a `Rotation3d`. The `Translation3d` is created in meters and the `Rotation3d` is created with radians. For more information on the coordinate system, please see the {ref}`Coordinate Systems <docs/apriltag-pipelines/coordinate-systems:Coordinate Systems>` documentation.
|
||||
|
||||
# Coming Soon!
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java
|
||||
:language: java
|
||||
:lines: 44-45
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Constants.h
|
||||
:language: c++
|
||||
:lines: 43-45
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 33-36
|
||||
```
|
||||
|
||||
## Creating a `PhotonPoseEstimator`
|
||||
@@ -35,127 +51,116 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all
|
||||
The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has nine possible values:
|
||||
|
||||
- MULTI_TAG_PNP_ON_COPROCESSOR
|
||||
- Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.
|
||||
- Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here <docs/apriltag-pipelines/multitag:multitag localization>` for more information.
|
||||
- Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.
|
||||
- Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here <docs/apriltag-pipelines/multitag:multitag localization>` for more information.
|
||||
- LOWEST_AMBIGUITY
|
||||
- Choose the Pose with the lowest ambiguity.
|
||||
- Choose the Pose with the lowest ambiguity.
|
||||
- CLOSEST_TO_CAMERA_HEIGHT
|
||||
- Choose the Pose which is closest to the camera height.
|
||||
- Choose the Pose which is closest to the camera height.
|
||||
- CLOSEST_TO_REFERENCE_POSE
|
||||
- Choose the Pose which is closest to the pose from setReferencePose().
|
||||
- Choose the Pose which is closest to the pose from setReferencePose().
|
||||
- CLOSEST_TO_LAST_POSE
|
||||
- Choose the Pose which is closest to the last pose calculated.
|
||||
- Choose the Pose which is closest to the last pose calculated.
|
||||
- AVERAGE_BEST_TARGETS
|
||||
- Choose the Pose which is the average of all the poses from each tag.
|
||||
- Choose the Pose which is the average of all the poses from each tag.
|
||||
- MULTI_TAG_PNP_ON_RIO
|
||||
- A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use.
|
||||
- A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use.
|
||||
- PNP_DISTANCE_TRIG_SOLVE
|
||||
- Use 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. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98).
|
||||
- Use 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. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98).
|
||||
- CONSTRAINED_SOLVEPNP
|
||||
- Solve 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 should not take more than 2ms.
|
||||
This also requires addHeadingData to be called every frame so heading data is up to date.
|
||||
If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
|
||||
the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
|
||||
seed.
|
||||
- Solve 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 should not take more than 2ms.
|
||||
This also requires addHeadingData to be called every frame so heading data is up to date.
|
||||
If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
|
||||
the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
|
||||
seed.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. code-block:: Java
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
|
||||
:language: java
|
||||
:lines: 65-66
|
||||
|
||||
//Forward Camera
|
||||
cam = new PhotonCamera("testCamera");
|
||||
Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); //Cam mounted facing forward, half a meter forward of center, half a meter up from center.
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
|
||||
:language: c++
|
||||
:lines: 150-153
|
||||
|
||||
// Construct PhotonPoseEstimator
|
||||
PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cam, robotToCam);
|
||||
|
||||
.. code-block:: C++
|
||||
|
||||
// Forward Camera
|
||||
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
|
||||
std::make_shared<photonlib::PhotonCamera>("testCamera");
|
||||
// Camera is mounted facing forward, half a meter forward of center, half a
|
||||
// meter up from center.
|
||||
frc::Transform3d robotToCam =
|
||||
frc::Transform3d(frc::Translation3d(0.5_m, 0_m, 0.5_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad));
|
||||
|
||||
// ... Add other cameras here
|
||||
|
||||
// Assemble the list of cameras & mount locations
|
||||
std::vector<
|
||||
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
|
||||
cameras;
|
||||
cameras.push_back(std::make_pair(cameraOne, robotToCam));
|
||||
|
||||
photonlib::RobotPoseEstimator estimator(
|
||||
aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
|
||||
|
||||
.. code-block:: Python
|
||||
|
||||
kRobotToCam = wpimath.geometry.Transform3d(
|
||||
wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
|
||||
wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
|
||||
)
|
||||
|
||||
self.cam = PhotonCamera("YOUR CAMERA NAME")
|
||||
|
||||
self.camPoseEst = PhotonPoseEstimator(
|
||||
loadAprilTagLayoutField(AprilTagField.kDefaultField),
|
||||
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
|
||||
self.cam,
|
||||
kRobotToCam
|
||||
)
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 45-50
|
||||
```
|
||||
|
||||
:::{note}
|
||||
Python still takes a `PhotonCamera` in the constructor, so you must create the camera as shown in the next section and then return and use it to create the `PhotonPoseEstimator`.
|
||||
:::
|
||||
|
||||
## Using a `PhotonPoseEstimator`
|
||||
|
||||
Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotPose`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated. You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `PhotonPoseEstimator` every loop using `addVisionMeasurement()`.
|
||||
The final prerequisite to using your `PhotonPoseEstimator` is creating a `PhotonCamera`. To do this, you must set the name of your camera in Photon Client. From there you can define the camera in code.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/357d8a518a93f7a1f8084a79449249e613b605a7/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
|
||||
:language: java
|
||||
:lines: 85-88
|
||||
|
||||
.. code-block:: C++
|
||||
|
||||
std::pair<frc::Pose2d, units::millisecond_t> getEstimatedGlobalPose(
|
||||
frc::Pose3d prevEstimatedRobotPose) {
|
||||
robotPoseEstimator.SetReferencePose(prevEstimatedRobotPose);
|
||||
units::millisecond_t currentTime = frc::Timer::GetFPGATimestamp();
|
||||
auto result = robotPoseEstimator.Update();
|
||||
if (result.second) {
|
||||
return std::make_pair<>(result.first.ToPose2d(),
|
||||
currentTime - result.second);
|
||||
} else {
|
||||
return std::make_pair(frc::Pose2d(), 0_ms);
|
||||
}
|
||||
}
|
||||
|
||||
.. code-block:: Python
|
||||
|
||||
# Coming Soon!
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
|
||||
:language: java
|
||||
:lines: 63
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h
|
||||
:language: c++
|
||||
:lines: 55
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 44
|
||||
```
|
||||
|
||||
You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `RobotPoseEstimator` every loop using `addVisionMeasurement()`. TODO: add example note
|
||||
Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotPose`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
|
||||
:language: java
|
||||
:lines: 93-116
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
|
||||
:language: c++
|
||||
:lines: 80-100
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 53
|
||||
```
|
||||
|
||||
You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `PhotonPoseEstimator` every loop using `addVisionMeasurement()`.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set-code::
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java
|
||||
:language: java
|
||||
:lines: 49
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Robot.h
|
||||
:language: c++
|
||||
:lines: 54-57
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 54-57
|
||||
```
|
||||
|
||||
## Complete Examples
|
||||
|
||||
The complete examples for the `PhotonPoseEstimator` can be found in the following locations:
|
||||
|
||||
- [Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/poseest)
|
||||
- [C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/poseest)
|
||||
- [Python](https://github.com/PhotonVision/photonvision/tree/main/photonlib-python-examples/poseest)
|
||||
|
||||
## Additional `PhotonPoseEstimator` Methods
|
||||
|
||||
### `setReferencePose(Pose3d referencePose)`
|
||||
For more information on the `PhotonPoseEstimator` class, please see the API documentation.
|
||||
|
||||
Updates the stored reference pose when using the CLOSEST_TO_REFERENCE_POSE strategy.
|
||||
|
||||
### `setLastPose(Pose3d lastPose)`
|
||||
|
||||
Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy.
|
||||
|
||||
### `addHeadingData(double timestampSeconds, Rotation2d heading)`
|
||||
|
||||
Adds robot heading data to be stored in buffer. Must be called periodically with a proper timestamp for the PNP_DISTANCE_TRIG_SOLVE and CONSTRAINED_SOLVEPNP strategies
|
||||
- [Java Documentation](https://javadocs.photonvision.org/release/org/photonvision/PhotonPoseEstimator.html)
|
||||
- [C++ Documentation](https://cppdocs.photonvision.org/release/classphoton_1_1_photon_pose_estimator.html)
|
||||
- [Python Documentation](https://pydocs.photonvision.org/release/reference/photonPoseEstimator/)
|
||||
|
||||
Reference in New Issue
Block a user