From 65c976b23543a45f77ed8893d7785355c0d5d2f8 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 8 Oct 2025 16:40:46 -0500 Subject: [PATCH 1/4] Add section on obtaining vision measurements for pose estimation Adds comprehensive "Obtaining Vision Measurements" section explaining how to get robot pose estimates from vision systems like PhotonVision and Limelight for use with AddVisionMeasurement(). Fixes #1413 --- .../state-space-pose-estimators.rst | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst b/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst index 80b06ebff7..2b5dcf88bf 100644 --- a/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst +++ b/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst @@ -47,6 +47,33 @@ Add vision pose measurements occasionally by calling ``AddVisionMeasurement()``. :lines: 93-106 :lineno-match: +## Obtaining Vision Measurements + +To use ``AddVisionMeasurement()``, you need to obtain a robot pose estimate from your vision system. The most common approach is using AprilTags detected by vision coprocessors. The general process is: + +1. **Detect AprilTags**: Use a vision solution like PhotonVision or Limelight to detect AprilTags on the field +2. **Get Tag Pose**: Look up the known field position of the detected tag from the ``AprilTagFieldLayout`` +3. **Calculate Robot Pose**: Use the camera-to-tag transform and camera-to-robot transform to calculate where the robot is on the field +4. **Apply Measurement**: Pass the calculated pose, timestamp, and standard deviations to ``AddVisionMeasurement()`` + +Most vision libraries provide utilities to simplify this process: + +- **PhotonVision**: The PhotonLib library includes ``PhotonPoseEstimator`` which handles the transforms and provides robot poses directly. See the `PhotonVision documentation `_ for details. +- **Limelight**: Limelight provides MegaTag which outputs robot poses through NetworkTables. See the `Limelight documentation `_ for the ``botpose`` fields. + +.. important:: When using vision measurements, it's critical to: + + - Use the **timestamp** from when the image was captured, not when it was processed + - Scale the **standard deviations** based on factors like distance from tags, number of tags seen, and tag ambiguity + - Reject measurements with high ambiguity or when no tags are detected + +For teams implementing their own vision solution, you'll need to: + +1. Obtain the camera-to-tag transform using methods like ``cv::solvePnP`` (OpenCV) or similar +2. Apply the camera-to-robot transform (fixed based on camera mounting) +3. Transform from the tag's field position to get the robot's field position +4. Handle latency by using the image capture timestamp + ## Tuning Pose Estimators All pose estimators offer user-customizable standard deviations for model and measurements (defaults are used if you don't provide them). Standard deviation is a measure of how spread out the noise is for a random signal. Giving a state a smaller standard deviation means it will be trusted more during data fusion. From 746b088305fe19f81a143bbee448da422091da98 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Mon, 13 Oct 2025 20:32:11 -0500 Subject: [PATCH 2/4] Move vision measurement content to vision processing docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Per review feedback, moved the detailed vision measurement information from the pose estimator docs to a new article in the vision processing section. Changes: - Created new apriltag-pose-estimation.rst in vision-processing/apriltag/ - Comprehensive guide covering PhotonVision, Limelight, and custom solutions - Detailed sections on timestamps, standard deviations, and rejecting bad measurements - Code examples in Java, C++, and Python for both libraries - Updated apriltag/index.rst to include new article - Replaced detailed section in state-space-pose-estimators.rst with a seealso link The pose estimator docs now focus on the API usage, while the vision processing docs explain how to obtain the measurements. 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- .../state-space-pose-estimators.rst | 27 +- .../apriltag/apriltag-pose-estimation.rst | 304 ++++++++++++++++++ .../vision-processing/apriltag/index.rst | 2 + 3 files changed, 307 insertions(+), 26 deletions(-) create mode 100644 source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst diff --git a/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst b/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst index 2b5dcf88bf..a93b4cbb18 100644 --- a/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst +++ b/source/docs/software/advanced-controls/state-space/state-space-pose-estimators.rst @@ -47,32 +47,7 @@ Add vision pose measurements occasionally by calling ``AddVisionMeasurement()``. :lines: 93-106 :lineno-match: -## Obtaining Vision Measurements - -To use ``AddVisionMeasurement()``, you need to obtain a robot pose estimate from your vision system. The most common approach is using AprilTags detected by vision coprocessors. The general process is: - -1. **Detect AprilTags**: Use a vision solution like PhotonVision or Limelight to detect AprilTags on the field -2. **Get Tag Pose**: Look up the known field position of the detected tag from the ``AprilTagFieldLayout`` -3. **Calculate Robot Pose**: Use the camera-to-tag transform and camera-to-robot transform to calculate where the robot is on the field -4. **Apply Measurement**: Pass the calculated pose, timestamp, and standard deviations to ``AddVisionMeasurement()`` - -Most vision libraries provide utilities to simplify this process: - -- **PhotonVision**: The PhotonLib library includes ``PhotonPoseEstimator`` which handles the transforms and provides robot poses directly. See the `PhotonVision documentation `_ for details. -- **Limelight**: Limelight provides MegaTag which outputs robot poses through NetworkTables. See the `Limelight documentation `_ for the ``botpose`` fields. - -.. important:: When using vision measurements, it's critical to: - - - Use the **timestamp** from when the image was captured, not when it was processed - - Scale the **standard deviations** based on factors like distance from tags, number of tags seen, and tag ambiguity - - Reject measurements with high ambiguity or when no tags are detected - -For teams implementing their own vision solution, you'll need to: - -1. Obtain the camera-to-tag transform using methods like ``cv::solvePnP`` (OpenCV) or similar -2. Apply the camera-to-robot transform (fixed based on camera mounting) -3. Transform from the tag's field position to get the robot's field position -4. Handle latency by using the image capture timestamp +.. seealso:: For detailed information about obtaining vision measurements from AprilTags, see :doc:`/docs/software/vision-processing/apriltag/apriltag-pose-estimation`. ## Tuning Pose Estimators diff --git a/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst new file mode 100644 index 0000000000..1a9a9c73db --- /dev/null +++ b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst @@ -0,0 +1,304 @@ +# Using AprilTags for Pose Estimation + +AprilTag detection is most commonly used to help your robot determine its position on the field. This article explains how to obtain vision measurements from AprilTags and use them with WPILib's pose estimators. + +## Overview + +The process of using AprilTags for pose estimation involves: + +1. **Detect AprilTags**: Use a vision solution to detect AprilTags on the field +2. **Get Tag Pose**: Look up the known field position of the detected tag from the ``AprilTagFieldLayout`` +3. **Calculate Robot Pose**: Use the camera-to-tag transform and camera-to-robot transform to calculate where the robot is on the field +4. **Apply Measurement**: Pass the calculated pose to your pose estimator using ``AddVisionMeasurement()`` + +## Using Vision Libraries + +Most teams use existing vision processing libraries that handle the complex mathematics for you: + +### PhotonVision + +PhotonVision provides the ``PhotonPoseEstimator`` class which simplifies the entire process: + +.. tab-set-code:: + + ```java + import org.photonvision.PhotonPoseEstimator; + import org.photonvision.PhotonPoseEstimator.PoseStrategy; + + // Create the pose estimator + PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator( + fieldLayout, // AprilTagFieldLayout + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, // PhotonCamera + robotToCam // Transform3d from robot to camera + ); + + // In your periodic method + var result = photonPoseEstimator.update(); + if (result.isPresent()) { + var estimatedPose = result.get(); + poseEstimator.addVisionMeasurement( + estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds + ); + } + ``` + + ```c++ + #include + + // Create the pose estimator + photon::PhotonPoseEstimator photonPoseEstimator{ + fieldLayout, // frc::AprilTagFieldLayout + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + camera, // photon::PhotonCamera + robotToCam // frc::Transform3d from robot to camera + }; + + // In your periodic method + auto result = photonPoseEstimator.Update(); + if (result) { + poseEstimator.AddVisionMeasurement( + result->estimatedPose.ToPose2d(), + result->timestamp + ); + } + ``` + + ```python + from photonlibpy.photonPoseEstimator import PhotonPoseEstimator, PoseStrategy + + # Create the pose estimator + photon_pose_estimator = PhotonPoseEstimator( + field_layout, # AprilTagFieldLayout + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, # PhotonCamera + robot_to_cam # Transform3d from robot to camera + ) + + # In your periodic method + result = photon_pose_estimator.update() + if result is not None: + pose_estimator.addVisionMeasurement( + result.estimatedPose.toPose2d(), + result.timestamp + ) + ``` + +See the `PhotonVision documentation `_ for complete details. + +### Limelight + +Limelight cameras with MegaTag provide robot poses directly through NetworkTables: + +.. tab-set-code:: + + ```java + import edu.wpi.first.networktables.NetworkTable; + import edu.wpi.first.networktables.NetworkTableInstance; + + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + + // In your periodic method + double[] botpose = limelightTable.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); + if (botpose.length > 0 && botpose[0] != 0.0) { + Pose2d visionPose = new Pose2d(botpose[0], botpose[1], Rotation2d.fromDegrees(botpose[5])); + double latency = limelightTable.getEntry("tl").getDouble(0) + limelightTable.getEntry("cl").getDouble(0); + double timestamp = Timer.getFPGATimestamp() - (latency / 1000.0); + + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + ``` + + ```c++ + #include + #include + + auto limelightTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); + + // In your periodic method + auto botpose = limelightTable->GetEntry("botpose_wpiblue").GetDoubleArray({}); + if (!botpose.empty() && botpose[0] != 0.0) { + frc::Pose2d visionPose{units::meter_t{botpose[0]}, units::meter_t{botpose[1]}, + frc::Rotation2d{units::degree_t{botpose[5]}}}; + auto latency = limelightTable->GetEntry("tl").GetDouble(0) + limelightTable->GetEntry("cl").GetDouble(0); + auto timestamp = frc::Timer::GetFPGATimestamp() - units::millisecond_t{latency}; + + poseEstimator.AddVisionMeasurement(visionPose, timestamp); + } + ``` + + ```python + from ntcore import NetworkTableInstance + from wpilib import Timer + from wpimath.geometry import Pose2d, Rotation2d + + limelight_table = NetworkTableInstance.getDefault().getTable("limelight") + + # In your periodic method + botpose = limelight_table.getEntry("botpose_wpiblue").getDoubleArray([]) + if len(botpose) > 0 and botpose[0] != 0.0: + vision_pose = Pose2d(botpose[0], botpose[1], Rotation2d.fromDegrees(botpose[5])) + latency = limelight_table.getEntry("tl").getDouble(0) + limelight_table.getEntry("cl").getDouble(0) + timestamp = Timer.getFPGATimestamp() - (latency / 1000.0) + + pose_estimator.addVisionMeasurement(vision_pose, timestamp) + ``` + +.. note:: Use ``botpose_wpiblue`` or ``botpose_wpired`` based on your alliance color. These provide poses in the correct field coordinate system. + +See the `Limelight documentation `_ for the complete NetworkTables API. + +## Important Considerations + +### Timestamps + +It's critical to use the **timestamp from when the image was captured**, not when it was processed. Vision processing introduces latency (typically 20-100ms), and the pose estimator needs the actual capture time to properly fuse the measurement with odometry data. + +Most vision libraries provide this timestamp: +- PhotonVision: ``result.timestampSeconds`` +- Limelight: Calculate from ``tl`` (targeting latency) + ``cl`` (capture latency) + +### Standard Deviations + +The accuracy of vision measurements varies based on several factors: + +- **Distance from tags**: Measurements are less accurate when far from tags +- **Number of tags**: Seeing multiple tags improves accuracy +- **Tag ambiguity**: Low-resolution or angled views reduce accuracy +- **Camera quality**: Higher resolution cameras provide better accuracy + +You should scale the standard deviations passed to ``AddVisionMeasurement()`` based on these factors: + +.. tab-set-code:: + + ```java + // Example: Scale standard deviations based on distance and tag count + double distance = /* calculate distance to nearest tag */; + int tagCount = /* number of tags seen */; + + // More tags = more trust, greater distance = less trust + double xyStdDev = 0.5 * Math.pow(distance, 2) / tagCount; + double thetaStdDev = 999999.9; // Don't trust rotation from single tag + + poseEstimator.addVisionMeasurement( + visionPose, + timestamp, + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) + ); + ``` + + ```c++ + // Example: Scale standard deviations based on distance and tag count + double distance = /* calculate distance to nearest tag */; + int tagCount = /* number of tags seen */; + + // More tags = more trust, greater distance = less trust + double xyStdDev = 0.5 * std::pow(distance, 2) / tagCount; + double thetaStdDev = 999999.9; // Don't trust rotation from single tag + + poseEstimator.AddVisionMeasurement( + visionPose, + timestamp, + {xyStdDev, xyStdDev, thetaStdDev} + ); + ``` + + ```python + # Example: Scale standard deviations based on distance and tag count + distance = # calculate distance to nearest tag + tag_count = # number of tags seen + + # More tags = more trust, greater distance = less trust + xy_std_dev = 0.5 * (distance ** 2) / tag_count + theta_std_dev = 999999.9 # Don't trust rotation from single tag + + pose_estimator.addVisionMeasurement( + vision_pose, + timestamp, + (xy_std_dev, xy_std_dev, theta_std_dev) + ) + ``` + +See :doc:`/docs/software/advanced-controls/state-space/state-space-pose-estimators` for more information about tuning standard deviations. + +### Rejecting Bad Measurements + +You should reject vision measurements in certain situations: + +- **No tags detected**: Only use measurements when tags are visible +- **High ambiguity**: Reject measurements with low confidence (check tag ambiguity values) +- **Unrealistic poses**: Reject measurements that are outside the field boundaries or far from your current estimate +- **During rapid motion**: Vision measurements may be less reliable during fast turns or acceleration + +Example rejection logic: + +.. tab-set-code:: + + ```java + var result = photonPoseEstimator.update(); + if (result.isPresent()) { + var estimatedPose = result.get(); + + // Check if pose is reasonable (within field boundaries) + if (estimatedPose.estimatedPose.getX() >= 0 && + estimatedPose.estimatedPose.getX() <= fieldLayout.getFieldLength() && + estimatedPose.estimatedPose.getY() >= 0 && + estimatedPose.estimatedPose.getY() <= fieldLayout.getFieldWidth()) { + + poseEstimator.addVisionMeasurement( + estimatedPose.estimatedPose.toPose2d(), + estimatedPose.timestampSeconds + ); + } + } + ``` + + ```c++ + auto result = photonPoseEstimator.Update(); + if (result) { + // Check if pose is reasonable (within field boundaries) + if (result->estimatedPose.X() >= 0_m && + result->estimatedPose.X() <= fieldLayout.GetFieldLength() && + result->estimatedPose.Y() >= 0_m && + result->estimatedPose.Y() <= fieldLayout.GetFieldWidth()) { + + poseEstimator.AddVisionMeasurement( + result->estimatedPose.ToPose2d(), + result->timestamp + ); + } + } + ``` + + ```python + result = photon_pose_estimator.update() + if result is not None: + # Check if pose is reasonable (within field boundaries) + if (0 <= result.estimatedPose.X() <= field_layout.getFieldLength() and + 0 <= result.estimatedPose.Y() <= field_layout.getFieldWidth()): + + pose_estimator.addVisionMeasurement( + result.estimatedPose.toPose2d(), + result.timestamp + ) + ``` + +## Custom Vision Solutions + +If you're implementing your own vision processing, you'll need to: + +1. **Detect tags and get camera-to-tag transforms**: Use a library like OpenCV's ``solvePnP`` to calculate the transformation from your camera to each detected tag +2. **Transform to robot pose**: Apply your camera-to-robot transform (determined by camera mounting position) +3. **Transform to field pose**: Use the tag's known field position from ``AprilTagFieldLayout`` to calculate the robot's field position +4. **Handle latency**: Capture and use the image timestamp, accounting for processing delay + +This approach requires solid understanding of 3D geometry and coordinate transformations. Most teams are better served using existing vision libraries that handle these details. + +## See Also + +- :doc:`apriltag-field-layout` - Understanding and using AprilTag field layouts +- :doc:`/docs/software/advanced-controls/state-space/state-space-pose-estimators` - Using pose estimators with vision measurements +- :doc:`/docs/software/basic-programming/coordinate-system` - Understanding the FRC coordinate system +- `PhotonVision Documentation `_ - Complete PhotonVision documentation +- `Limelight Documentation `_ - Complete Limelight documentation diff --git a/source/docs/software/vision-processing/apriltag/index.rst b/source/docs/software/vision-processing/apriltag/index.rst index 06f7e9506b..c6ebd151fe 100644 --- a/source/docs/software/vision-processing/apriltag/index.rst +++ b/source/docs/software/vision-processing/apriltag/index.rst @@ -4,3 +4,5 @@ :maxdepth: 2 apriltag-intro + apriltag-field-layout + apriltag-pose-estimation From 0ef2dc280f5c93f4e889149e65c6d0efc6e2597b Mon Sep 17 00:00:00 2001 From: jasondaming Date: Mon, 13 Oct 2025 21:39:04 -0500 Subject: [PATCH 3/4] Fix duplicate explicit target names in apriltag-pose-estimation.rst MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Changed links in "See Also" section to anonymous hyperlinks (double underscores) to avoid conflicts with similar link text used earlier in the document. 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- .../vision-processing/apriltag/apriltag-pose-estimation.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst index 1a9a9c73db..a5c8eca441 100644 --- a/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst +++ b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst @@ -300,5 +300,5 @@ This approach requires solid understanding of 3D geometry and coordinate transfo - :doc:`apriltag-field-layout` - Understanding and using AprilTag field layouts - :doc:`/docs/software/advanced-controls/state-space/state-space-pose-estimators` - Using pose estimators with vision measurements - :doc:`/docs/software/basic-programming/coordinate-system` - Understanding the FRC coordinate system -- `PhotonVision Documentation `_ - Complete PhotonVision documentation -- `Limelight Documentation `_ - Complete Limelight documentation +- `PhotonVision Documentation `__ - Complete PhotonVision documentation +- `Limelight Documentation `__ - Complete Limelight documentation From 1c86dc1ecf1092699caadc609b8a477248dfa300 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 21 Oct 2025 21:40:45 -0500 Subject: [PATCH 4/4] Fix build errors by removing references to non-existent apriltag-field-layout file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Removed references to apriltag-field-layout.rst which doesn't exist in this PR: - Removed from toctree in index.rst - Removed from "See Also" section in apriltag-pose-estimation.rst The apriltag-field-layout article is being created in PR #3138. Once that PR merges, the references can be added back. This fixes the build errors: - "toctree contains reference to nonexisting document" - "unknown document: 'apriltag-field-layout'" 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- .../vision-processing/apriltag/apriltag-pose-estimation.rst | 1 - source/docs/software/vision-processing/apriltag/index.rst | 1 - 2 files changed, 2 deletions(-) diff --git a/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst index a5c8eca441..1a7f3b72d6 100644 --- a/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst +++ b/source/docs/software/vision-processing/apriltag/apriltag-pose-estimation.rst @@ -297,7 +297,6 @@ This approach requires solid understanding of 3D geometry and coordinate transfo ## See Also -- :doc:`apriltag-field-layout` - Understanding and using AprilTag field layouts - :doc:`/docs/software/advanced-controls/state-space/state-space-pose-estimators` - Using pose estimators with vision measurements - :doc:`/docs/software/basic-programming/coordinate-system` - Understanding the FRC coordinate system - `PhotonVision Documentation `__ - Complete PhotonVision documentation diff --git a/source/docs/software/vision-processing/apriltag/index.rst b/source/docs/software/vision-processing/apriltag/index.rst index c6ebd151fe..3902634a67 100644 --- a/source/docs/software/vision-processing/apriltag/index.rst +++ b/source/docs/software/vision-processing/apriltag/index.rst @@ -4,5 +4,4 @@ :maxdepth: 2 apriltag-intro - apriltag-field-layout apriltag-pose-estimation