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

Open up pose estimator strategy methods (#2252)

Browse Source
This commit is contained in:
Gold856
2026-01-11 13:58:53 -05:00
committed by GitHub
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,91 +48,88 @@ Another necessary argument for creating a `PhotonPoseEstimator` is the `Transfor
## Creating a `PhotonPoseEstimator`
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.
- LOWEST_AMBIGUITY
- Choose the Pose with the lowest ambiguity.
- CLOSEST_TO_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().
- CLOSEST_TO_LAST_POSE
- 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.
- MULTI_TAG_PNP_ON_RIO
- 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).
- 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.
The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above) and `Transform3d`.
```{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: 65-66
:lines: 63
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
:language: c++
:lines: 150-153
:lines: 149-150
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
:language: python
:lines: 45-50
:lines: 45-48
```
:::{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`
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.
To use your `PhotonPoseEstimator`, you must create a `PhotonCamera` and feed the results into your `PhotonPoseEstimator`. To do this, you must first 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/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
:language: java
:lines: 63
:lines: 62
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
:language: c++
:lines: 55
:lines: 151
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
:language: python
:lines: 44
```
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.
When taking in a result from a `PhotonCamera`, PhotonPoseEstimator offers nine possible "strategies" for calculating a pose from a pipeline result in the form of methods that you can call, following the pattern `estimate<strategy name>Pose`:
- Coprocessor MultiTag (`estimateCoprocMultiTagPose`)
- 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 (`estimateLowestAmbiguityPose`)
- Choose the Pose with the lowest ambiguity.
- Closest to Camera Height (`estimateClosestToCameraHeightPose`)
- Choose the Pose which is closest to the camera height.
- Closest to Reference Pose (`estimateClosestToReferencePose`)
- Choose the Pose which is closest to the pose that is passed into the function.
- Average Best Targets (`estimateAverageBestTargetsPose`)
- Choose the Pose which is the average of all the poses from each tag.
- roboRio MultiTag (`estimateRioMultiTagPose`)
- A slower, older version of Coprocessor MultiTag, not recommended for use.
- PnP Distance Trig Solve (`estimatePnpDistanceTrigSolvePose`)
- 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 (`estimateConstrainedSolvepnpPose`)
- 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.
Calling one of the `estimate<strategy>Pose()` methods on your `PhotonPoseEstimator` will return an `Optional<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. The recommended way to use the estimatePose methods is to
1. do estimation with one of MultiTag methods, check if the result is empty, then
2. fallback to single tag estimation using a method like `estimateLowestAmbiguityPose`.
```{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
:lines: 91-94
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
:language: c++
:lines: 80-100
:lines: 79-82
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
:language: python
:lines: 53
:lines: 52-54
```
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()`.
For Constrained SolvePnP, it's recommended to do the previously mentioned steps, and then feed the pose (if it exists) into `estimateConstrainedSolvepnpPose`, and if the Constrained SolvePnP result is empty, simply feed the seed pose into your drivetrain pose estimator.
Once you have the `Optional<EstimatedRobotPose>`, you can check to see if there's an actual pose inside, and act accordingly. 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()`. For Java and C++, the examples pass a method from the drivetrain to a `Vision` object, with the parameter being called `estConsumer`. Python calls the drivetrain directly.
```{eval-rst}
.. tab-set-code::
@@ -146,7 +143,22 @@ You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
:language: python
:lines: 54-57
:lines: 56-58
```
```{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: 89-115
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h
:language: c++
:lines: 77-100
.. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py
:language: python
:lines: 51-54
```
## Complete Examples

3
photon-lib/py/photonlibpy/__init__.py
View File

@@ -25,12 +25,11 @@
from .estimatedRobotPose import EstimatedRobotPose
from .packet import Packet
from .photonCamera import PhotonCamera
from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
from .photonPoseEstimator import PhotonPoseEstimator
__all__ = (
"EstimatedRobotPose",
"Packet",
"PhotonCamera",
"PhotonPoseEstimator",
"PoseStrategy",
)

7
photon-lib/py/photonlibpy/estimatedRobotPose.py
View File

@@ -16,15 +16,11 @@
###############################################################################
from dataclasses import dataclass
from typing import TYPE_CHECKING
from wpimath.geometry import Pose3d
from .targeting.photonTrackedTarget import PhotonTrackedTarget
if TYPE_CHECKING:
from .photonPoseEstimator import PoseStrategy
@dataclass
class EstimatedRobotPose:
@@ -38,6 +34,3 @@ class EstimatedRobotPose:
targetsUsed: list[PhotonTrackedTarget]
"""A list of the targets used to compute this pose"""
strategy: "PoseStrategy"
"""The strategy actually used to produce this pose"""

264
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -15,7 +15,6 @@
## along with this program. If not, see <https://www.gnu.org/licenses/>.
###############################################################################
import enum
from typing import Optional
import hal
@@ -34,54 +33,9 @@ from wpimath.geometry import (
from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
from .estimatedRobotPose import EstimatedRobotPose
from .photonCamera import PhotonCamera
from .targeting.photonPipelineResult import PhotonPipelineResult
class PoseStrategy(enum.Enum):
"""
Position estimation strategies that can be used by the PhotonPoseEstimator class.
"""
LOWEST_AMBIGUITY = enum.auto()
"""Choose the Pose with the lowest ambiguity."""
CLOSEST_TO_CAMERA_HEIGHT = enum.auto()
"""Choose the Pose which is closest to the camera height."""
CLOSEST_TO_REFERENCE_POSE = enum.auto()
"""Choose the Pose which is closest to a set Reference position."""
CLOSEST_TO_LAST_POSE = enum.auto()
"""Choose the Pose which is closest to the last pose calculated."""
AVERAGE_BEST_TARGETS = enum.auto()
"""Return the average of the best target poses using ambiguity as weight."""
MULTI_TAG_PNP_ON_COPROCESSOR = enum.auto()
"""
Use all visible tags to compute a single pose estimate on coprocessor.
This option needs to be enabled on the PhotonVision web UI as well.
"""
MULTI_TAG_PNP_ON_RIO = enum.auto()
"""
Use all visible tags to compute a single pose estimate.
This runs on the RoboRIO, and can take a lot of time.
"""
PNP_DISTANCE_TRIG_SOLVE = enum.auto()
"""
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.
Produces a Pose2d in estimatedRobotPose (0 for z, roll, pitch).
See https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
"""
class PhotonPoseEstimator:
instance_count = 1
@@ -94,8 +48,6 @@ class PhotonPoseEstimator:
def __init__(
self,
fieldTags: AprilTagFieldLayout,
strategy: PoseStrategy,
camera: PhotonCamera,
robotToCamera: Transform3d,
):
"""Create a new PhotonPoseEstimator.
@@ -104,21 +56,13 @@ class PhotonPoseEstimator:
with respect to the FIRST field using the Field Coordinate System.
Note that setting the origin of this layout object will affect the
results from this class.
:param strategy: The strategy it should use to determine the best pose.
:param camera: PhotonCamera
:param robotToCamera: Transform3d from the center of the robot to the camera mount position (i.e.,
robot ➔ camera) in the Robot Coordinate System.
"""
self._fieldTags = fieldTags
self._primaryStrategy = strategy
self._camera = camera
self.robotToCamera = robotToCamera
self._multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY
self._reportedErrors: set[int] = set()
self._poseCacheTimestampSeconds = -1.0
self._lastPose: Optional[Pose3d] = None
self._referencePose: Optional[Pose3d] = None
self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)
# Usage reporting
@@ -146,92 +90,8 @@ class PhotonPoseEstimator:
:param fieldTags: the AprilTagFieldLayout
"""
self._checkUpdate(self._fieldTags, fieldTags)
self._fieldTags = fieldTags
@property
def primaryStrategy(self) -> PoseStrategy:
"""Get the Position Estimation Strategy being used by the Position Estimator.
:returns: the strategy
"""
return self._primaryStrategy
@primaryStrategy.setter
def primaryStrategy(self, strategy: PoseStrategy):
"""Set the Position Estimation Strategy used by the Position Estimator.
:param strategy: the strategy to set
"""
self._checkUpdate(self._primaryStrategy, strategy)
self._primaryStrategy = strategy
@property
def multiTagFallbackStrategy(self) -> PoseStrategy:
return self._multiTagFallbackStrategy
@multiTagFallbackStrategy.setter
def multiTagFallbackStrategy(self, strategy: PoseStrategy):
"""Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
NOT be MULTI_TAG_PNP
:param strategy: the strategy to set
"""
self._checkUpdate(self._multiTagFallbackStrategy, strategy)
if (
strategy is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
or strategy is PoseStrategy.MULTI_TAG_PNP_ON_RIO
):
wpilib.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
False,
)
strategy = PoseStrategy.LOWEST_AMBIGUITY
self._multiTagFallbackStrategy = strategy
@property
def referencePose(self) -> Optional[Pose3d]:
"""Return the reference position that is being used by the estimator.
:returns: the referencePose
"""
return self._referencePose
@referencePose.setter
def referencePose(self, referencePose: Pose3d | Pose2d):
"""Update the stored reference pose for use when using the **CLOSEST_TO_REFERENCE_POSE**
strategy.
:param referencePose: the referencePose to set
"""
if isinstance(referencePose, Pose2d):
referencePose = Pose3d(referencePose)
self._checkUpdate(self._referencePose, referencePose)
self._referencePose = referencePose
@property
def lastPose(self) -> Optional[Pose3d]:
return self._lastPose
@lastPose.setter
def lastPose(self, lastPose: Pose3d | Pose2d):
"""Update the stored last pose. Useful for setting the initial estimate when using the
**CLOSEST_TO_LAST_POSE** strategy.
:param lastPose: the lastPose to set
"""
if isinstance(lastPose, Pose2d):
lastPose = Pose3d(lastPose)
self._checkUpdate(self._lastPose, lastPose)
self._lastPose = lastPose
def _invalidatePoseCache(self) -> None:
self._poseCacheTimestampSeconds = -1.0
def _checkUpdate(self, oldObj, newObj) -> None:
if oldObj != newObj:
self._invalidatePoseCache()
def addHeadingData(
self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
) -> None:
@@ -258,76 +118,40 @@ class PhotonPoseEstimator:
self._headingBuffer.clear()
self.addHeadingData(timestampSeconds, heading)
def update(
self, cameraResult: Optional[PhotonPipelineResult] = None
) -> Optional[EstimatedRobotPose]:
def _shouldEstimate(self, cameraResult: PhotonPipelineResult) -> bool:
"""
Updates the estimated position of the robot. Returns empty if:
:param cameraResult: A pipeline result from the camera.
- The timestamp of the provided pipeline result is the same as in the previous call to
``update()``.
- No targets were found in the pipeline results.
:param cameraResult: The latest pipeline result from the camera
:returns: an :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used to
create the estimate.
:returns: Whether or not estimation should be done.
"""
if not cameraResult:
if not self._camera:
wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False)
return None
cameraResult = self._camera.getLatestResult()
if cameraResult.getTimestampSeconds() < 0:
return None
return False
# If the pose cache timestamp was set, and the result is from the same
# timestamp, return an
# empty result
if (
self._poseCacheTimestampSeconds > 0.0
and abs(
self._poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()
)
< 1e-6
):
return None
# If no targets seen, trivial case -- can't do estimation
return len(cameraResult.targets) > 0
# Remember the timestamp of the current result used
self._poseCacheTimestampSeconds = cameraResult.getTimestampSeconds()
# If no targets seen, trivial case -- return empty result
if not cameraResult.targets:
return None
return self._update(cameraResult, self._primaryStrategy)
def _update(
self, cameraResult: PhotonPipelineResult, strat: PoseStrategy
) -> Optional[EstimatedRobotPose]:
if strat is PoseStrategy.LOWEST_AMBIGUITY:
estimatedPose = self._lowestAmbiguityStrategy(cameraResult)
elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = self._multiTagOnCoprocStrategy(cameraResult)
elif strat is PoseStrategy.PNP_DISTANCE_TRIG_SOLVE:
estimatedPose = self._pnpDistanceTrigSolveStrategy(cameraResult)
else:
wpilib.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
)
return None
if not estimatedPose:
self._lastPose = None
return estimatedPose
def _pnpDistanceTrigSolveStrategy(
def estimatePnpDistanceTrigSolvePose(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
if (bestTarget := result.getBestTarget()) is None:
"""
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
:param result: A pipeline result from the camera.
:returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
to create the estimate.
"""
if (
not self._shouldEstimate(result)
or (bestTarget := result.getBestTarget()) is None
):
return None
if (
@@ -364,16 +188,22 @@ class PhotonPoseEstimator:
)
return EstimatedRobotPose(
Pose3d(robotPose),
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
Pose3d(robotPose), result.getTimestampSeconds(), result.getTargets()
)
def _multiTagOnCoprocStrategy(
def estimateCoprocMultiTagPose(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
if result.multitagResult is not None:
"""
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.
:param result: A pipeline result from the camera.
:returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
to create the estimate.
"""
if result.multitagResult is not None and self._shouldEstimate(result):
best_tf = result.multitagResult.estimatedPose.best
best = (
Pose3d()
@@ -385,23 +215,22 @@ class PhotonPoseEstimator:
best,
result.getTimestampSeconds(),
result.targets,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
)
else:
return self._update(result, self._multiTagFallbackStrategy)
return None
def _lowestAmbiguityStrategy(
def estimateLowestAmbiguityPose(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
"""
Return the estimated position of the robot with the lowest position ambiguity from a List of
pipeline results.
Return the estimated position of the robot with the lowest position ambiguity from a pipeline results.
:param result: pipeline result
:returns: the estimated position of the robot in the FCS and the estimated timestamp of this
estimation.
:param result: A pipeline result from the camera.
:returns: An :class:`EstimatedRobotPose` with an estimated pose, timestamp, and targets used
to create the estimate.
"""
if not self._shouldEstimate(result):
return None
lowestAmbiguityTarget = None
lowestAmbiguityScore = 10.0
@@ -432,7 +261,6 @@ class PhotonPoseEstimator:
).transformBy(self.robotToCamera.inverse()),
result.getTimestampSeconds(),
result.targets,
PoseStrategy.LOWEST_AMBIGUITY,
)
def _reportFiducialPoseError(self, fiducialId: int) -> None:

111
photon-lib/py/test/photonPoseEstimator_test.py
View File

@@ -15,10 +15,9 @@
## along with this program. If not, see <https://www.gnu.org/licenses/>.
###############################################################################
from test import testUtil
import wpimath.units
from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
from photonlibpy import PhotonCamera, PhotonPoseEstimator
from photonlibpy.estimation import TargetModel
from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionTargetSim
from photonlibpy.targeting import (
@@ -138,10 +137,11 @@ def test_lowestAmbiguityStrategy():
)
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
aprilTags,
Transform3d(),
)
estimatedPose = estimator.update()
estimatedPose = estimator.estimateLowestAmbiguityPose(cameraOne.result)
assert estimatedPose is not None
@@ -179,8 +179,6 @@ def test_pnpDistanceTrigSolve():
estimator = PhotonPoseEstimator(
aprilTags,
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
cameraOne,
compoundTestTransform,
)
@@ -198,7 +196,7 @@ def test_pnpDistanceTrigSolve():
estimator.addHeadingData(
result.getTimestampSeconds(), realPose.rotation().toRotation2d()
)
estimatedRobotPose = estimator.update(result)
estimatedRobotPose = estimator.estimatePnpDistanceTrigSolvePose(result)
assert estimatedRobotPose is not None
pose = estimatedRobotPose.estimatedPose
@@ -224,7 +222,7 @@ def test_pnpDistanceTrigSolve():
estimator.addHeadingData(
result.getTimestampSeconds(), realPose.rotation().toRotation2d()
)
estimatedRobotPose = estimator.update(result)
estimatedRobotPose = estimator.estimatePnpDistanceTrigSolvePose(result)
assert estimatedRobotPose is not None
pose = estimatedRobotPose.estimatedPose
@@ -271,13 +269,10 @@ def test_multiTagOnCoprocStrategy():
estimator = PhotonPoseEstimator(
AprilTagFieldLayout(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cameraOne,
Transform3d(),
)
estimatedPose = estimator.update()
estimatedPose = estimator.estimateCoprocMultiTagPose(cameraOne.result)
assert estimatedPose is not None
pose = estimatedPose.estimatedPose
@@ -288,97 +283,5 @@ def test_multiTagOnCoprocStrategy():
assertEquals(2, pose.z, 0.01)
def test_cacheIsInvalidated():
aprilTags = fakeAprilTagFieldLayout()
cameraOne = PhotonCameraInjector()
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# Initial state, expect no timestamp.
assertEquals(-1, estimator._poseCacheTimestampSeconds)
# First result is 17s after epoch start.
timestamps = testUtil.PipelineTimestamps(captureTimestampMicros=17_000_000)
latencySecs = timestamps.pipelineLatencySecs()
# No targets, expect empty result
cameraOne.result = PhotonPipelineResult(
timestamps.receiveTimestampMicros(),
metadata=timestamps.toPhotonPipelineMetadata(),
)
estimatedPose = estimator.update()
assert estimatedPose is None
assertEquals(
timestamps.receiveTimestampMicros() * 1e-6 - latencySecs,
estimator._poseCacheTimestampSeconds,
1e-3,
)
# Set actual result
timestamps.incrementTimeMicros(2_500_000)
result = PhotonPipelineResult(
timestamps.receiveTimestampMicros(),
[
PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
[
TargetCorner(1, 2),
TargetCorner(3, 4),
TargetCorner(5, 6),
TargetCorner(7, 8),
],
0.7,
)
],
metadata=timestamps.toPhotonPipelineMetadata(),
)
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is not None
expectedTimestamp = timestamps.receiveTimestampMicros() * 1e-6 - latencySecs
assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# And again -- pose cache should mean this is empty
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is None
# Expect the old timestamp to still be here
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# Set new field layout -- right after, the pose cache timestamp should be -1
estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
assertEquals(-1, estimator._poseCacheTimestampSeconds)
# Update should cache the current timestamp (20) again
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is not None
assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# Setting a value from None to a non-None should invalidate the cache.
assert estimator.referencePose is None
estimator.referencePose = Pose3d(3, 3, 3, Rotation3d())
assertEquals(-1, estimator._poseCacheTimestampSeconds)
def assertEquals(expected, actual, epsilon=0.0):
assert abs(expected - actual) <= epsilon

14
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -43,6 +43,20 @@ public class EstimatedRobotPose {
/** The strategy actually used to produce this pose */
public final PoseStrategy strategy;
/**
* Constructs an EstimatedRobotPose
*
* @param estimatedPose estimated pose
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = null;
}
/**
* Constructs an EstimatedRobotPose
*

476
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -117,7 +117,7 @@ public class PhotonPoseEstimator {
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error const.
* heading estimate against the tag corner reprojection error cost.
*/
public static final record ConstrainedSolvepnpParams(
boolean headingFree, double headingScaleFactor) {}
@@ -144,12 +144,35 @@ public class PhotonPoseEstimator {
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
InstanceCount++;
}
/**
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
* @deprecated Use individual estimation methods with the 2 argument constructor instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
@@ -221,7 +244,9 @@ public class PhotonPoseEstimator {
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
@@ -230,7 +255,9 @@ public class PhotonPoseEstimator {
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
@@ -246,7 +273,9 @@ public class PhotonPoseEstimator {
* NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
@@ -262,7 +291,9 @@ public class PhotonPoseEstimator {
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Pose3d getReferencePose() {
return referencePose;
}
@@ -272,7 +303,9 @@ public class PhotonPoseEstimator {
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
@@ -283,7 +316,9 @@ public class PhotonPoseEstimator {
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
@@ -293,7 +328,9 @@ public class PhotonPoseEstimator {
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
@@ -303,7 +340,9 @@ public class PhotonPoseEstimator {
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose2d lastPose) {
setLastPose(new Pose3d(lastPose));
}
@@ -389,9 +428,11 @@ public class PhotonPoseEstimator {
* provided in this overload.
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
return update(cameraResult, Optional.empty(), Optional.empty());
}
@@ -411,9 +452,11 @@ public class PhotonPoseEstimator {
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
@@ -436,9 +479,11 @@ public class PhotonPoseEstimator {
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
@@ -475,7 +520,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
@@ -491,21 +536,122 @@ public class PhotonPoseEstimator {
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
case LOWEST_AMBIGUITY -> lowestAmbiguityStrategy(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> closestToCameraHeightStrategy(cameraResult);
case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
case CLOSEST_TO_REFERENCE_POSE ->
closestToReferencePoseStrategy(cameraResult, referencePose);
estimateClosestToReferencePose(cameraResult, referencePose);
case CLOSEST_TO_LAST_POSE -> {
setReferencePose(lastPose);
yield closestToReferencePoseStrategy(cameraResult, referencePose);
yield estimateClosestToReferencePose(cameraResult, referencePose);
}
case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult);
case MULTI_TAG_PNP_ON_RIO -> {
if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield update(cameraResult, this.multiTagFallbackStrategy);
}
var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get());
if (res.isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
constrainedPnpParams,
this.multiTagFallbackStrategy);
}
yield res;
}
case MULTI_TAG_PNP_ON_COPROCESSOR -> {
if (cameraResult.getMultiTagResult().isEmpty()) {
yield update(cameraResult, this.multiTagFallbackStrategy);
}
yield estimateCoprocMultiTagPose(cameraResult);
}
case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult);
case CONSTRAINED_SOLVEPNP -> {
boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
yield Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (cameraResult.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
cameraResult
.getMultiTagResult()
.get()
.estimatedPose
.best
.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
yield Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
}
var pnpResult =
estimateConstrainedSolvepnpPose(
cameraResult,
cameraMatrix.get(),
distCoeffs.get(),
fieldToRobotSeed,
constrainedPnpParams.get().headingFree,
constrainedPnpParams.get().headingScaleFactor);
if (!pnpResult.isPresent()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
yield pnpResult;
}
case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
case MULTI_TAG_PNP_ON_RIO ->
multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
case CONSTRAINED_SOLVEPNP ->
constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
};
if (estimatedPose.isPresent()) {
@@ -515,12 +661,43 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.getBestTarget();
/**
* @param cameraResult A pipeline result from the camera.
* @return Whether or not pose estimation should be performed.
*/
private boolean shouldEstimate(PhotonPipelineResult cameraResult) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return false;
}
// If no targets seen, trivial case -- can't do estimation
return cameraResult.hasTargets();
}
/**
* 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.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget bestTarget = cameraResult.getBestTarget();
if (bestTarget == null) return Optional.empty();
var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds());
var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds());
if (headingSampleOpt.isEmpty()) {
return Optional.empty();
}
@@ -555,98 +732,82 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(robotPose),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
}
private Optional<EstimatedRobotPose> constrainedPnpStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
/**
* 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 {@link
* org.photonvision.jni.ConstrainedSolvepnpJni} 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.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param 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 {@link
* #estimateCoprocMultiTagPose(PhotonPipelineResult)}, or {@link
* #estimateLowestAmbiguityPose(PhotonPipelineResult)} if MultiTag results aren't available.
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error cont.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
PhotonPipelineResult cameraResult,
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
Pose3d seedPose,
boolean headingFree,
double headingScaleFactor) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (result.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
result.getMultiTagResult().get().estimatedPose.best.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
result,
cameraMatrixOpt,
distCoeffsOpt,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
return Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(headingBuffer.getSample(result.getTimestampSeconds()).get()));
}
var pnpResult =
VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
cameraMatrixOpt.get(),
distCoeffsOpt.get(),
result.getTargets(),
cameraMatrix,
distCoeffs,
cameraResult.getTargets(),
robotToCamera,
fieldToRobotSeed,
seedPose,
fieldTags,
tagModel,
constrainedPnpParams.get().headingFree,
headingBuffer.getSample(result.getTimestampSeconds()).get(),
constrainedPnpParams.get().headingScaleFactor);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
headingFree,
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get(),
headingScaleFactor);
if (!pnpResult.isPresent()) return Optional.empty();
var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CONSTRAINED_SOLVEPNP));
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().isEmpty()) {
return update(result, this.multiTagFallbackStrategy);
/**
* 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.
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (cameraResult.getMultiTagResult().isEmpty() || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best_tf = cameraResult.getMultiTagResult().get().estimatedPose.best;
var best =
Pose3d.kZero
.plus(best_tf) // field-to-camera
@@ -655,33 +816,31 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
return update(result, this.multiTagFallbackStrategy);
}
if (result.getTargets().size() < 2) {
return update(result, this.multiTagFallbackStrategy);
/**
* 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.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
if (cameraResult.getTargets().size() < 2 || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
cameraMatrix, distCoeffs, cameraResult.getTargets(), fieldTags, tagModel);
if (!pnpResult.isPresent()) return Optional.empty();
var best =
Pose3d.kZero
@@ -691,25 +850,29 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
* Return the estimated position of the robot with the lowest position ambiguity from a pipeline
* result.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget lowestAmbiguityTarget = null;
double lowestAmbiguityScore = 10;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
double targetPoseAmbiguity = target.getPoseAmbiguity();
// Make sure the target is a Fiducial target.
if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
@@ -737,8 +900,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}
@@ -746,15 +909,19 @@ public class PhotonPoseEstimator {
* 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.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
double smallestHeightDifference = 10e9;
EstimatedRobotPose closestHeightTarget = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -792,8 +959,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
@@ -805,8 +972,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}
@@ -819,13 +986,16 @@ public class PhotonPoseEstimator {
* Return the estimated position of the robot using the target with the lowest delta in the vector
* magnitude between it and the reference pose.
*
* @param result pipeline result
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference against.
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
PhotonPipelineResult result, Pose3d referencePose) {
public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
PhotonPipelineResult cameraResult, Pose3d referencePose) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
if (referencePose == null) {
DriverStation.reportError(
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
@@ -836,7 +1006,7 @@ public class PhotonPoseEstimator {
double smallestPoseDelta = 10e9;
EstimatedRobotPose lowestDeltaPose = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -870,8 +1040,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
@@ -879,8 +1049,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
@@ -890,15 +1060,19 @@ public class PhotonPoseEstimator {
/**
* Return the average of the best target poses using ambiguity as weight.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -923,8 +1097,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
@@ -958,8 +1132,8 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}

325
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -24,13 +24,9 @@
#include "photon/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <optional>
#include <span>
#include <string>
#include <utility>
#include <vector>
@@ -46,6 +42,7 @@
#include <units/angle.h>
#include <units/math.h>
#include <units/time.h>
#include <wpi/deprecated.h>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -55,7 +52,7 @@
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
WPI_IGNORE_DEPRECATED
namespace photon {
namespace detail {
@@ -67,6 +64,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat,
frc::Transform3d robotToCamera)
@@ -138,39 +148,94 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
switch (strategy) {
case LOWEST_AMBIGUITY:
ret = LowestAmbiguityStrategy(result);
ret = EstimateLowestAmbiguityPose(result);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
ret = ClosestToCameraHeightStrategy(result);
ret = EstimateClosestToCameraHeightPose(result);
break;
case CLOSEST_TO_REFERENCE_POSE:
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
ret = EstimateAverageBestTargetsPose(result);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret = MultiTagOnCoprocStrategy(result);
if (!result.MultiTagResult()) {
ret = Update(result, this->multiTagFallbackStrategy);
} else {
ret = EstimateCoprocMultiTagPose(result);
}
break;
case MULTI_TAG_PNP_ON_RIO:
if (cameraMatrixData && cameraDistCoeffs) {
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
} else {
if (!cameraMatrixData && !cameraDistCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!",
"");
ret = Update(result, this->multiTagFallbackStrategy);
}
ret =
EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
if (!ret) {
ret = Update(result, this->multiTagFallbackStrategy);
}
break;
case CONSTRAINED_SOLVEPNP:
ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams);
case CONSTRAINED_SOLVEPNP: {
using namespace frc;
if (!cameraMatrixData || !cameraDistCoeffs) {
FRC_ReportError(
frc::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
break;
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
ret = EstimateConstrainedSolvepnpPose(
result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
constrainedPnpParams->headingFree,
constrainedPnpParams->headingScalingFactor);
if (!ret) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
}
break;
}
case PNP_DISTANCE_TRIG_SOLVE:
ret = PnpDistanceTrigSolveStrategy(result);
ret = EstimatePnpDistanceTrigSolvePose(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -184,10 +249,26 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return ret;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) {
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return false;
}
// If no targets seen, trivial case -- can't do estimation
return result.HasTargets();
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
auto foundIt = targets.end();
for (auto it = targets.begin(); it != targets.end(); ++it) {
if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
@@ -214,18 +295,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
return EstimatedRobotPose{
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToCameraHeightStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
for (auto& target : result.GetTargets()) {
for (auto& target : cameraResult.GetTargets()) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
@@ -250,14 +334,16 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -265,14 +351,17 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToReferencePoseStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -298,17 +387,17 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = altPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = bestPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
}
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(),
CLOSEST_TO_REFERENCE_POSE};
}
@@ -361,41 +450,31 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (!result.MultiTagResult()) {
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (!cameraResult.MultiTagResult() || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"PhotonPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix cameraMatrix,
PhotonCamera::DistortionMatrix distCoeffs) {
// Need at least 2 targets
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
if (cameraResult.GetTargets().size() < 2 || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
auto const targets = result.GetTargets();
const auto targets = cameraResult.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
// (pixels).
@@ -418,7 +497,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
// We should only do multi-tag if at least 2 tags (* 4 corners/tag)
if (imagePoints.size() < 8) {
return Update(result, this->multiTagFallbackStrategy);
return std::nullopt;
}
// Output mats for results
@@ -426,27 +505,31 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
{
cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
cv::eigen2cv(*camMat, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
cv::Mat cameraMatCV(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
cv::eigen2cv(cameraMatrix, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
cv::eigen2cv(distCoeffs, distCoeffsMatCV);
cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
tvec, false, cv::SOLVEPNP_SQPNP);
}
const Pose3d pose = detail::ToPose3d(tvec, rvec);
const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(),
MULTI_TAG_PNP_ON_RIO);
return photon::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
std::optional<frc::Rotation2d> headingSampleOpt =
headingBuffer.Sample(result.GetTimestamp());
headingBuffer.Sample(cameraResult.GetTimestamp());
if (!headingSampleOpt) {
FRC_ReportError(frc::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
@@ -485,17 +568,21 @@ PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
frc::Pose2d robotPose = frc::Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample);
return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -512,13 +599,15 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
std::make_pair(target.GetPoseAmbiguity(),
cameraResult.GetTimestamp())));
}
frc::Translation3d transform = frc::Translation3d();
@@ -532,77 +621,45 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(),
cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"StrPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
if (!headingFree) {
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
if (!constrainedPnpParams.value().headingFree) {
fieldToRobotSeed = frc::Pose3d{
fieldToRobotSeed.Translation(),
frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
}
std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
result.GetTargets().end()};
std::vector<photon::PhotonTrackedTarget> targets{
cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};
std::optional<photon::PnpResult> pnpResult =
VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
constrainedPnpParams->headingFree,
frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
constrainedPnpParams->headingScalingFactor);
cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
aprilTags, photon::kAprilTag36h11, headingFree,
frc::Rotation2d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
headingScaleFactor);
if (!pnpResult) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
return std::nullopt;
}
frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
PoseStrategy::CONSTRAINED_SOLVEPNP};
}
} // namespace photon

252
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -24,8 +24,6 @@
#pragma once
#include <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
@@ -34,11 +32,8 @@
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
enum PoseStrategy {
@@ -93,10 +88,26 @@ class PhotonPoseEstimator {
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param 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.
*/
[[deprecated(
"Use individual estimation methods with the 2 argument constructor "
"instead.")]]
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
@@ -112,14 +123,20 @@ class PhotonPoseEstimator {
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
[[deprecated("Use individual estimation methods instead.")]]
PoseStrategy GetPoseStrategy() const {
return strategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
@@ -132,22 +149,30 @@ class PhotonPoseEstimator {
* only one tag can be seen. Must NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
[[deprecated("Use individual estimation methods instead.")]]
frc::Pose3d GetReferencePose() const {
return referencePose;
}
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetReferencePose(frc::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
@@ -178,8 +203,12 @@ class PhotonPoseEstimator {
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(frc::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
* Add robot heading data to the buffer. Must be called periodically for the
@@ -247,7 +276,9 @@ class PhotonPoseEstimator {
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
std::optional<EstimatedRobotPose> Update(
const photon::PhotonPipelineResult& result,
std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
@@ -257,6 +288,127 @@ class PhotonPoseEstimator {
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
std::nullopt);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
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.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
/**
* 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.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
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 the RoboRIO. This can take a lot of time
* due to the RIO's weak computing power.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
PhotonCamera::DistortionMatrix distCoeffs);
/**
* 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.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult);
/**
* Return the average of the best target poses using ambiguity as weight.
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult);
/**
* 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.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param 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.
* @param headingFree If true, heading is completely free to vary. If false,
* heading excursions from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of
* changing our robot heading estimate against the tag corner reprojection
* error cost.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor);
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
@@ -280,9 +432,9 @@ class PhotonPoseEstimator {
* This should only be called after timestamp checks have been done by another
* update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param cameraResult A pipeline result from the camera.
* @param strategy The pose strategy to use
* @return an EstimatedRobotPose with an estimated pose, timestamp, and
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
@@ -296,84 +448,6 @@ class PhotonPoseEstimator {
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
PhotonPipelineResult result);
/**
* 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.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
/**
* Return the pose calculation using the best visible tag and the robot's
* heading
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation
*/
std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
PhotonPipelineResult result);
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
};
} // namespace photon

936
photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java Normal file
View File

@@ -0,0 +1,936 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
class LegacyPhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@BeforeAll
public static void init() throws IOException {
if (!LibraryLoader.loadWpiLibraries()) {
fail();
}
RuntimeLoader.loadLibrary("photontargetingJNI");
HAL.initialize(1000, 0);
List<AprilTag> tagList = new ArrayList<>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
double fieldLength = Units.feetToMeters(54.0);
double fieldWidth = Units.feetToMeters(27.0);
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
void testLowestAmbiguityStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
}
@Test
void testClosestToCameraHeightStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
4000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
}
@Test
void closestToReferencePoseStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
17000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
}
@Test
void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
new PhotonPipelineResult(
0,
7000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
0,
-1,
-1,
new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
}
@Test
void pnpDistanceTrigSolve() {
List<VisionTargetSim> simTargets =
aprilTags.getTags().stream()
.map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
.toList();
try (PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
/* Compound Rolled + Pitched + Yaw */
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37),
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
PhotonPipelineResult result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
var bestTarget = result.getBestTarget();
assertNotNull(bestTarget);
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
/* Straight on */
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
estimator.setRobotToCameraTransform(straightOnTestTransform);
/* Pose to compare with */
realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
new PhotonPipelineResult(
0,
20 * 1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))), // 1 1 1 ambig: .7
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))), // 2 2 2 ambig .3
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
}
@Test
public void testConstrainedPnpOneTag() {
var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
var cameraMat =
MatBuilder.fill(
Nat.N3(),
Nat.N3(),
399.37500000000006,
0,
319.5,
0,
399.16666666666674,
239.5,
0,
0,
1);
/*
* Ground truth:
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
* 31064262190452635
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
* 24478552235412665
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
* 0836470779150917
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
* 914649865171567
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
* 191446451763934
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
* 44396966389316
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
* 4995793771070878
*/
List<TargetCorner> corners8 =
List.of(
new TargetCorner(98.09875447066685, 331.0093220119495),
new TargetCorner(122.20226758624413, 335.50083894738486),
new TargetCorner(127.17118732489361, 313.81406314178633),
new TargetCorner(104.28543773760417, 309.6516557438994));
var result =
new PhotonPipelineResult(
new PhotonPipelineMetadata(10000, 2000, 1, 100),
List.of(
new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
Optional.of(
new MultiTargetPNPResult(
new PnpResult(
new Transform3d(
// From ground truth
new Translation3d(
3.1665557336121353, 4.430673446050584, 0.48678786477534686),
new Rotation3d(
new Quaternion(
0.3132532247418243,
0.24722671090692333,
-0.08413452932300695,
0.9130568172784148))),
0.1),
new ArrayList<Short>(8))));
final double camPitch = Units.degreesToRadians(30.0);
final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");
}
PhotonPipelineResult result;
@Override
public List<PhotonPipelineResult> getAllUnreadResults() {
return List.of(result);
}
@Override
public PhotonPipelineResult getLatestResult() {
return result;
}
}
}

183
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -24,11 +24,8 @@
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
@@ -39,13 +36,11 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
@@ -57,8 +52,6 @@ import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
@@ -170,10 +163,10 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateLowestAmbiguityPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -257,11 +250,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToCameraHeightPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -345,12 +337,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -434,13 +425,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
@@ -514,7 +503,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
estimatedPose = estimator.update(cameraOne.result);
estimatedPose = estimator.estimateClosestToReferencePose(cameraOne.result, pose);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -542,9 +531,7 @@ class PhotonPoseEstimatorTest {
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
var estimator = new PhotonPoseEstimator(aprilTags, compoundTestTransform);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
@@ -556,7 +543,7 @@ class PhotonPoseEstimatorTest {
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -575,7 +562,7 @@ class PhotonPoseEstimatorTest {
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -584,82 +571,6 @@ class PhotonPoseEstimatorTest {
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
@@ -735,11 +646,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateAverageBestTargetsPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -749,8 +659,9 @@ class PhotonPoseEstimatorTest {
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.result =
void testMultiTagOnCoprocFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
@@ -799,18 +710,21 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, Transform3d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateCoprocMultiTagPose(camera.result);
assertTrue(estimatedPose.isEmpty());
estimatedPose = estimator.estimateLowestAmbiguityPose(camera.result);
assertTrue(estimatedPose.isPresent());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), 1e-9);
assertEquals(3, pose.getY(), 1e-9);
assertEquals(2, pose.getZ(), 1e-9);
}
@Test
@@ -888,34 +802,17 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam);
var multiTagEstimate = estimator.estimateCoprocMultiTagPose(result);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
estimator.estimateConstrainedSolvepnpPose(
result, cameraMat, distortion, multiTagEstimate.get().estimatedPose, true, 0);
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");

661
photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp Normal file
View File

@@ -0,0 +1,661 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <map>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include <wpi/deprecated.h>
#include "gtest/gtest.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/simulation/PhotonCameraSim.h"
#include "photon/simulation/SimCameraProperties.h"
#include "photon/simulation/VisionTargetSim.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
WPI_IGNORE_DEPRECATED
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
cameraOne.test = true;
std::vector<photon::VisionTargetSim> targets;
targets.reserve(tags.size());
for (const auto& tag : tags) {
targets.push_back(
photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
}
photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
frc::Transform3d straightOnTestTransform =
frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(18_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
// empty input, expect empty out
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
auto test2 = testResult;
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
{0, 399.16666666666674, 239.5},
{0, 0, 1}};
// Create corners data matching the Java test
std::vector<photon::TargetCorner> corners8{
photon::TargetCorner{98.09875447066685, 331.0093220119495},
photon::TargetCorner{122.20226758624413, 335.50083894738486},
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
frc::Transform3d poseTransform(
frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
poseTransform, 0.0, corners8, corners8}};
auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
std::vector<int16_t>{8});
photon::PhotonPipelineResult result{
photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
multiTagResult};
cameraOne.test = true;
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
frc::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
ASSERT_TRUE(estimatedPose.has_value());
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
}

169
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -93,12 +93,11 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -154,12 +153,11 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToCameraHeightPose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -203,14 +201,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
@@ -254,14 +251,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
@@ -295,9 +291,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(result, pose);
}
ASSERT_TRUE(estimatedPose);
@@ -327,8 +322,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
@@ -344,7 +338,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -374,7 +368,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -419,12 +413,11 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateAverageBestTargetsPose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -437,101 +430,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
// empty input, expect empty out
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
@@ -555,12 +454,15 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateCoprocMultiTagPose(result);
}
ASSERT_FALSE(estimatedPose);
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -586,16 +488,6 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
@@ -639,14 +531,17 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
kRobotToCam);
auto estimatedMultiTagPose =
estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
auto estimatedPose = estimator.EstimateConstrainedSolvepnpPose(
cameraOne.testResult[0], cameraMat, distortion,
estimatedMultiTagPose->estimatedPose, true, 0);
ASSERT_TRUE(estimatedPose.has_value());

2
photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -156,7 +156,7 @@ public class VisionEstimation {
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param robot2camera The {@link Transform3d} from the robot odometry frame to the camera optical
* frame
* @param robotPoseSeed An initial guess at robot pose, refined via optimizaiton. Better guesses
* @param robotPoseSeed An initial guess at robot pose, refined via optimization. Better guesses
* will converge faster.
* @param tagLayout The known tag layout on the field
* @param tagModel The physical size of the AprilTags

14
photonlib-cpp-examples/poseest/src/main/include/Vision.h
View File

@@ -49,9 +49,6 @@ class Vision {
Eigen::Matrix<double, 3, 1>)>
estConsumer)
: estConsumer{estConsumer} {
photonEstimator.SetMultiTagFallbackStrategy(
photon::PoseStrategy::LOWEST_AMBIGUITY);
if (frc::RobotBase::IsSimulation()) {
visionSim = std::make_unique<photon::VisionSystemSim>("main");
@@ -79,7 +76,10 @@ class Vision {
// Run each new pipeline result through our pose estimator
for (const auto& result : camera.GetAllUnreadResults()) {
// cache result and update pose estimator
auto visionEst = photonEstimator.Update(result);
auto visionEst = photonEstimator.EstimateCoprocMultiTagPose(result);
if (!visionEst) {
visionEst = photonEstimator.EstimateLowestAmbiguityPose(result);
}
m_latestResult = result;
// In sim only, add our vision estimate to the sim debug field
@@ -146,10 +146,8 @@ class Vision {
frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
private:
photon::PhotonPoseEstimator photonEstimator{
constants::Vision::kTagLayout,
photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
constants::Vision::kRobotToCam};
photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout,
constants::Vision::kRobotToCam};
photon::PhotonCamera camera{constants::Vision::kCameraName};
std::unique_ptr<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;

15
photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
View File

@@ -38,7 +38,6 @@ import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
@@ -61,10 +60,7 @@ public class Vision {
public Vision(EstimateConsumer estConsumer) {
this.estConsumer = estConsumer;
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
photonEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam);
// ----- Simulation
if (Robot.isSimulation()) {
@@ -91,9 +87,12 @@ public class Vision {
public void periodic() {
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : camera.getAllUnreadResults()) {
visionEst = photonEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
for (var result : camera.getAllUnreadResults()) {
visionEst = photonEstimator.estimateCoprocMultiTagPose(result);
if (visionEst.isEmpty()) {
visionEst = photonEstimator.estimateLowestAmbiguityPose(result);
}
updateEstimationStdDevs(visionEst, result.getTargets());
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(

11
photonlib-python-examples/poseest/robot.py
View File

@@ -27,7 +27,7 @@
import drivetrain
import wpilib
import wpimath.geometry
from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
from photonlibpy import PhotonCamera, PhotonPoseEstimator
from robotpy_apriltag import AprilTagField, AprilTagFieldLayout
kRobotToCam = wpimath.geometry.Transform3d(
@@ -44,14 +44,15 @@ class MyRobot(wpilib.TimedRobot):
self.cam = PhotonCamera("YOUR CAMERA NAME")
self.camPoseEst = PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagField.kDefaultField),
PoseStrategy.LOWEST_AMBIGUITY,
self.cam,
kRobotToCam,
)
def robotPeriodic(self) -> None:
camEstPose = self.camPoseEst.update()
if camEstPose:
for result in self.cam.getAllUnreadResults():
camEstPose = self.camPoseEst.estimateCoprocMultiTagPose(result)
if camEstPose is None:
camEstPose = self.camPoseEst.estimateLowestAmbiguityPose(result)
self.swerve.addVisionPoseEstimate(
camEstPose.estimatedPose, camEstPose.timestampSeconds
)