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

[photonlib-py] Begin implementing PhotonPoseEstimator in Python (#1178)

Browse Source 
* [photonlib-py] Initial impl of PhotonPoseEstimator

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Ethan Wall
2024-01-21 07:57:32 -05:00
committed by GitHub
parent 57f02f31a5
commit 90773e0e4a
4 changed files with 591 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
photon-lib/py/photonlibpy/estimatedRobotPose.py Normal file
View File

@@ -0,0 +1,26 @@
from dataclasses import dataclass
from typing import TYPE_CHECKING
from wpimath.geometry import Pose3d
from .photonTrackedTarget import PhotonTrackedTarget
if TYPE_CHECKING:
from .photonPoseEstimator import PoseStrategy
@dataclass
class EstimatedRobotPose:
"""An estimated pose based on pipeline result"""
estimatedPose: Pose3d
"""The estimated pose"""
timestampSeconds: float
"""The estimated time the frame used to derive the robot pose was taken"""
targetsUsed: [PhotonTrackedTarget]
"""A list of the targets used to compute this pose"""
strategy: "PoseStrategy"
"""The strategy actually used to produce this pose"""

321
photon-lib/py/photonlibpy/photonPoseEstimator.py Normal file
View File

@@ -0,0 +1,321 @@
import enum
from typing import Optional
import wpilib
from robotpy_apriltag import AprilTagFieldLayout
from wpimath.geometry import Transform3d, Pose3d, Pose2d
from .photonPipelineResult import PhotonPipelineResult
from .photonCamera import PhotonCamera
from .estimatedRobotPose import EstimatedRobotPose
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.
"""
class PhotonPoseEstimator:
"""
The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
given timestamp on the field to produce a single robot in field pose, using the strategy set
below. Example usage can be found in our apriltagExample example project.
"""
def __init__(
self,
fieldTags: AprilTagFieldLayout,
strategy: PoseStrategy,
camera: PhotonCamera,
robotToCamera: Transform3d,
):
"""Create a new PhotonPoseEstimator.
:param fieldTags: A WPILib AprilTagFieldLayout linking AprilTag IDs to Pose3d objects
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
self._lastPose: Optional[Pose3d] = None
self._referencePose: Optional[Pose3d] = None
# TODO: Implement HAL reporting
@property
def fieldTags(self) -> AprilTagFieldLayout:
"""Get the AprilTagFieldLayout being used by the PositionEstimator.
Note: Setting the origin of this layout will affect the results from this class.
:returns: the AprilTagFieldLayout
"""
return self._fieldTags
@fieldTags.setter
def fieldTags(self, fieldTags: AprilTagFieldLayout):
"""Set the AprilTagFieldLayout being used by the PositionEstimator.
Note: Setting the origin of this layout will affect the results from this class.
: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) -> 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) -> 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):
self._poseCacheTimestampSeconds = -1
def _checkUpdate(self, oldObj, newObj):
if oldObj != newObj and oldObj is not None and oldObj is not newObj:
self._invalidatePoseCache()
def update(
self, cameraResult: Optional[PhotonPipelineResult] = None
) -> Optional[EstimatedRobotPose]:
"""
Updates the estimated position of the robot. Returns empty if:
- 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.
"""
if not cameraResult:
if not self._camera:
wpilib.reportError("[PhotonPoseEstimator] Missing camera!", False)
return
cameraResult = self._camera.getLatestResult()
if cameraResult.timestampSec < 0:
return
# If the pose cache timestamp was set, and the result is from the same
# timestamp, return an
# empty result
if (
self._poseCacheTimestampSeconds > 0
and abs(self._poseCacheTimestampSeconds - cameraResult.timestampSec) < 1e-6
):
return
# Remember the timestamp of the current result used
self._poseCacheTimestampSeconds = cameraResult.timestampSec
# If no targets seen, trivial case -- return empty result
if not cameraResult.targets:
return
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)
else:
wpilib.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
)
return
if not estimatedPose:
self._lastPose = None
return estimatedPose
def _multiTagOnCoprocStrategy(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
if result.multiTagResult.estimatedPose.isPresent:
best_tf = result.multiTagResult.estimatedPose.best
best = (
Pose3d()
.transformBy(best_tf) # field-to-camera
.relativeTo(self._fieldTags.getOrigin())
.transformBy(self.robotToCamera.inverse()) # field-to-robot
)
return EstimatedRobotPose(
best,
result.timestampSec,
result.targets,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
)
else:
return self._update(result, self._multiTagFallbackStrategy)
def _lowestAmbiguityStrategy(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
"""
Return the estimated position of the robot with the lowest position ambiguity from a List of
pipeline results.
:param result: pipeline result
:returns: the estimated position of the robot in the FCS and the estimated timestamp of this
estimation.
"""
lowestAmbiguityTarget = None
lowestAmbiguityScore = 10
for target in result.targets:
targetPoseAmbiguity = target.poseAmbiguity
# Make sure the target is a Fiducial target.
if targetPoseAmbiguity != -1 and targetPoseAmbiguity < lowestAmbiguityScore:
lowestAmbiguityScore = targetPoseAmbiguity
lowestAmbiguityTarget = target
# Although there are confirmed to be targets, none of them may be fiducial
# targets.
if not lowestAmbiguityTarget:
return
targetFiducialId = lowestAmbiguityTarget.fiducialId
targetPosition = self._fieldTags.getTagPose(targetFiducialId)
if not targetPosition:
self._reportFiducialPoseError(targetFiducialId)
return
return EstimatedRobotPose(
targetPosition.transformBy(
lowestAmbiguityTarget.getBestCameraToTarget().inverse()
).transformBy(self.robotToCamera.inverse()),
result.timestampSec,
result.targets,
PoseStrategy.LOWEST_AMBIGUITY,
)
def _reportFiducialPoseError(self, fiducialId: int) -> None:
if fiducialId not in self._reportedErrors:
wpilib.reportError(
f"[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: {fiducialId}",
False,
)
self._reportedErrors.add(fiducialId)

1
photon-lib/py/setup.py
View File

@@ -60,6 +60,7 @@ setup(
install_requires=[
"wpilib<2025,>=2024.0.0b2",
"robotpy-wpimath<2025,>=2024.0.0b2",
"robotpy-apriltag<2025,>=2024.0.0b2",
"pyntcore<2025,>=2024.0.0b2",
],
description=descriptionStr,

243
photon-lib/py/test/photonPoseEstimator_test.py Normal file
View File

@@ -0,0 +1,243 @@
from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult, PNPResult
from photonlibpy.photonPipelineResult import PhotonPipelineResult
from photonlibpy.photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
from photonlibpy.photonTrackedTarget import PhotonTrackedTarget, TargetCorner
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
class PhotonCameraInjector:
result: PhotonPipelineResult
def getLatestResult(self) -> PhotonPipelineResult:
return self.result
def setupCommon() -> AprilTagFieldLayout:
tagList = []
tagPoses = (
Pose3d(3, 3, 3, Rotation3d()),
Pose3d(5, 5, 5, Rotation3d()),
)
for id_, pose in enumerate(tagPoses):
aprilTag = AprilTag()
aprilTag.ID = id_
aprilTag.pose = pose
tagList.append(aprilTag)
fieldLength = 54 / 3.281 # 54 ft -> meters
fieldWidth = 27 / 3.281 # 24 ft -> meters
return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
def test_lowestAmbiguityStrategy():
aprilTags = setupCommon()
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
2,
11,
[
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,
),
PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 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.3,
),
PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.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.4,
),
],
)
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
estimatedPose = estimator.update()
pose = estimatedPose.estimatedPose
assertEquals(11, estimatedPose.timestampSeconds)
assertEquals(1, pose.x, 0.01)
assertEquals(3, pose.y, 0.01)
assertEquals(2, pose.z, 0.01)
def test_multiTagOnCoprocStrategy():
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
2,
11,
# There needs to be at least one target present for pose estimation to work
# Doesn't matter which/how many targets for this test
[
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,
)
],
multiTagResult=MultiTargetPNPResult(
PNPResult(True, Transform3d(1, 3, 2, Rotation3d()))
),
)
estimator = PhotonPoseEstimator(
AprilTagFieldLayout(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cameraOne,
Transform3d(),
)
estimatedPose = estimator.update()
pose = estimatedPose.estimatedPose
assertEquals(11, estimatedPose.timestampSeconds)
assertEquals(1, pose.x, 0.01)
assertEquals(3, pose.y, 0.01)
assertEquals(2, pose.z, 0.01)
def test_cacheIsInvalidated():
aprilTags = setupCommon()
cameraOne = PhotonCameraInjector()
result = PhotonPipelineResult(
2,
20,
[
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,
)
],
)
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# Empty result, expect empty result
cameraOne.result = PhotonPipelineResult(timestampSec=1)
estimatedPose = estimator.update()
assert estimatedPose is None
# Set actual result
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is not None
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20, estimator._poseCacheTimestampSeconds)
# 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(20, estimator._poseCacheTimestampSeconds)
# 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()
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20, estimator._poseCacheTimestampSeconds)
def assertEquals(expected, actual, epsilon=0.0):
assert abs(expected - actual) <= epsilon