Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-26 01:51:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update PhotonPoseEstimator examples (#1903)

Browse Source 
## 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
2025-04-14 23:42:25 -07:00
committed by GitHub
parent 1c42755451
commit 537cd7c564
6 changed files with 139 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
docs/source/docs/apriltag-pipelines/multitag.md
View File

@@ -23,30 +23,41 @@ Ensure that your camera is calibrated and 3D mode is enabled. Navigate to the Ou
By default, enabling multi-target will disable calculating camera-to-target transforms for each observed AprilTag target to increase performance; the X/Y/angle numbers shown in the target table of the UI are instead calculated using the tag's expected location (per the field layout JSON) and the field-to-camera transform calculated using MultiTag. If you additionally want the individual camera-to-target transform calculated using SolvePNP for each target, enable "Always Do Single-Target Estimation".
:::
This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class <docs/programming/photonlib/robot-pose-estimator:AprilTags and PhotonPoseEstimator>` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()` (Java/C++).
This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class <docs/programming/photonlib/robot-pose-estimator:AprilTags and PhotonPoseEstimator>` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python).
```{eval-rst}
.. tab-set-code::
.. code-block:: Java
var result = camera.getLatestResult();
if (result.getMultiTagResult().estimatedPose.isPresent) {
Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
var results = camera.getAllUnreadResults();
for (var result : results) {
var multiTagResult = result.getMultiTagResult();
if (multiTagResult.isPresent()) {
var fieldToCamera = multiTagResult.get().estimatedPose.best;
}
}
.. code-block:: C++
auto result = camera.GetLatestResult();
if (result.MultiTagResult().result.isPresent) {
frc::Transform3d fieldToCamera = result.MultiTagResult().result.best;
auto results = camera.GetAllUnreadResults();
for (auto &result : results)
{
auto multiTagResult = result.MultiTagResult();
if (multiTagResult.has_value()) {
frc::Transform3d fieldToCamera = multiTagResult->estimatedPose.best;
}
}
.. code-block:: Python
# Coming Soon!
results = camera.getAllUnreadResults()
for result in results:
multitagResult = result.multitagResult
if multitagResult is not None:
fieldToCamera = multitagResult.estimatedPose.best
```
:::{note}

8
docs/source/docs/examples/aimandrange.md
View File

@@ -1,6 +1,6 @@
# Combining Aiming and Getting in Range
The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/aimandrange)/[C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/aimandrange)).
The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/aimandrange)/[C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/aimandrange)/[Python](https://github.com/PhotonVision/photonvision/tree/main/photonlib-python-examples/aimandrange))
## Knowledge and Equipment Needed
@@ -10,7 +10,7 @@ The following example is from the PhotonLib example repository ([Java](https://g
Now that you know how to aim toward the AprilTag, let's also drive the correct distance from the AprilTag.
To do this, we'll use the *pitch* of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we'll use the P term of a PID controller to drive the robot to the correct distance.
To do this, we'll use the _pitch_ of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we'll use the P term of a PID controller to drive the robot to the correct distance.
```{eval-rst}
.. tab-set::
@@ -43,8 +43,8 @@ To do this, we'll use the *pitch* of the target in the camera image and trigonom
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/abe95dfaa055bbe3609f72cfcaaba0f96ee7978c/photonlib-python-examples/aimandrange/robot.py
:language: python
:lines: 44-95
:lines: 52-91
:linenos:
:lineno-start: 44
:lineno-start: 52
```

4
docs/source/docs/programming/photonlib/getting-target-data.md
View File

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

217
docs/source/docs/programming/photonlib/robot-pose-estimator.md
View File

@@ -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/)

2
docs/source/docs/troubleshooting/camera-troubleshooting.md
View File

@@ -12,7 +12,7 @@ If you haven't yet, please refer to {ref}`the Pi CSI Camera Configuration page <
If the driver isn't loaded, you may be using a non-official Pi image, or an image not new enough. Try updating to the most recent image available (one released for 2023) -- if that doesn't resolve the problem, {ref}`contact us<index:Contact Us>` with your settings ZIP file and Pi version/camera version/config.txt file used.
If the camera is not detected, the most likely cause is either a config.txt file incorrectly set-up, or a ribbon cable attached backwards. Review the {ref}`picam configuration page <docs/hardware/picamconfig:pi camera configuration>`, and verify the ribbon cable is properly oriented at both ends, and that it is \_fully\_ inserted into the FFC connector. Then, {ref}`contact us<index:Contact Us>` with your settings ZIP file and Pi version/camera version/config.txt file used.
If the camera is not detected, the most likely cause is either a config.txt file incorrectly set-up, or a ribbon cable attached backwards. Review the {ref}`picam configuration page <docs/hardware/picamconfig:pi camera configuration>`, and verify the ribbon cable is properly oriented at both ends, and that it is _fully_ inserted into the FFC connector. Then, {ref}`contact us<index:Contact Us>` with your settings ZIP file and Pi version/camera version/config.txt file used.
## USB cameras