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

sphinxify java docs for python code (#1575)

Browse Source
This commit is contained in:
Lucien Morey
2024-11-16 13:01:33 +11:00
committed by GitHub
parent 9bbf49bc6b
commit 04191efc51
10 changed files with 628 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
photon-lib/py/photonlibpy/estimation/openCVHelp.py
View File

@@ -27,6 +27,12 @@ class OpenCVHelp:
@staticmethod
def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
"""Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
three elements representing {x, y, z} in the EDN coordinate system.
:param translations: The translations to convert into a np.array
"""
retVal: list[list] = []
for translation in translations:
trl = OpenCVHelp.translationNWUtoEDN(translation)
@@ -38,6 +44,13 @@ class OpenCVHelp:
@staticmethod
def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
"""Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
norm, and axis = rvec / norm)
:param rotation: The rotation to convert into a np.array
"""
retVal: list[np.ndarray] = []
rot = OpenCVHelp.rotationNWUtoEDN(rotation)
rotVec = rot.getQuaternion().toRotationVector()
@@ -88,6 +101,25 @@ class OpenCVHelp:
def reorderCircular(
elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
) -> list[Any]:
"""Reorders the list, optionally indexing backwards and wrapping around to the last element after
the first, and shifting all indices in the direction of indexing.
e.g.
({1,2,3}, false, 1) == {2,3,1}
({1,2,3}, true, 0) == {1,3,2}
({1,2,3}, true, 1) == {3,2,1}
:param elements: list elements
:param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
:param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
start at shiftStart, negated if backwards)
:returns: Reordered list
"""
size = len(elements)
reordered = []
dir = -1 if backwards else 1
@@ -100,18 +132,39 @@ class OpenCVHelp:
@staticmethod
def translationEDNToNWU(trl: Translation3d) -> Translation3d:
"""Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
in EDN, this would be {0, -1, 0} in NWU.
"""
return trl.rotateBy(EDN_TO_NWU)
@staticmethod
def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
"""Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
in NWU, this would be {0, 0, 1} in EDN.
"""
return -EDN_TO_NWU + (rot + EDN_TO_NWU)
@staticmethod
def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
"""Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
elements representing {x, y, z} in the EDN coordinate system.
:param tvecInput: The tvec to create a Translation3d from
"""
return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
@staticmethod
def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
"""Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
and axis = rvec / norm)
:param rvecInput: The rvec to create a Rotation3d from
"""
return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
@staticmethod
@@ -121,6 +174,33 @@ class OpenCVHelp:
modelTrls: list[Translation3d],
imagePoints: np.ndarray,
) -> PnpResult | None:
"""Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
relative to the target is determined by the supplied 3d points of the target's model and their
associated 2d points imaged by the camera. The supplied model translations must be relative to
the target's pose.
For planar targets, there may be an alternate solution which is plausible given the 2d image
points. This has an associated "ambiguity" which describes the ratio of reprojection error
between the "best" and "alternate" solution.
This method is intended for use with individual AprilTags, and will not work unless 4 points
are provided.
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param modelTrls: The translations of the object corners. These should have the object pose as
their origin. These must come in a specific, pose-relative order (in NWU):
- Point 0: [0, -squareLength / 2, squareLength / 2]
- Point 1: [0, squareLength / 2, squareLength / 2]
- Point 2: [0, squareLength / 2, -squareLength / 2]
- Point 3: [0, -squareLength / 2, -squareLength / 2]
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
should match the given object point translations.
:returns: The resulting transformation that maps the camera pose to the target pose and the
ambiguity if an alternate solution is available.
"""
modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
@@ -130,6 +210,7 @@ class OpenCVHelp:
best: Transform3d = Transform3d()
for tries in range(2):
# calc rvecs/tvecs and associated reprojection error from image points
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
objectMat,
imagePoints,
@@ -138,6 +219,7 @@ class OpenCVHelp:
flags=cv.SOLVEPNP_IPPE_SQUARE,
)
# convert to wpilib coordinates
best = Transform3d(
OpenCVHelp.tVecToTranslation(tvecs[0]),
OpenCVHelp.rVecToRotation(rvecs[0]),
@@ -148,6 +230,7 @@ class OpenCVHelp:
OpenCVHelp.rVecToRotation(rvecs[1]),
)
# check if we got a NaN result
if reprojectionError is not None and not math.isnan(
reprojectionError[0, 0]
):
@@ -158,6 +241,7 @@ class OpenCVHelp:
pt[0, 1] -= 0.001
imagePoints[0] = pt
# solvePnP failed
if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
print("SolvePNP_Square failed!")
return None
@@ -186,6 +270,27 @@ class OpenCVHelp:
modelTrls: list[Translation3d],
imagePoints: np.ndarray,
) -> PnpResult | None:
"""Finds the transformation that maps the camera's pose to the origin of the supplied object. An
"object" is simply a set of known 3d translations that correspond to the given 2d points. If,
for example, the object translations are given relative to close-right corner of the blue
alliance(the default origin), a camera-to-origin transformation is returned. If the
translations are relative to a target's pose, a camera-to-target transformation is returned.
There must be at least 3 points to use this method. This does not return an alternate
solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
#solvePNP_SQUARE} instead.
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param objectTrls: The translations of the object corners, relative to the field.
:param imagePoints: The projection of these 3d object points into the 2d camera image. The order
should match the given object point translations.
:returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
model points are supplied relative to the origin, this transformation brings the camera to
the origin.
"""
objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(

19
photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py
View File

@@ -4,24 +4,38 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
class RotTrlTransform3d:
"""Represents a transformation that first rotates a pose around the origin, and then translates it."""
def __init__(
self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
):
"""A rotation-translation transformation.
Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
transform as if the origin was transformed by these components instead.
:param rot: The rotation component
:param trl: The translation component
"""
self.rot = rot
self.trl = trl
def inverse(self) -> Self:
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
invRot = -self.rot
invTrl = -(self.trl.rotateBy(invRot))
return type(self)(invRot, invTrl)
def getTransform(self) -> Transform3d:
"""This transformation as a Transform3d (as if of the origin)"""
return Transform3d(self.trl, self.rot)
def getTranslation(self) -> Translation3d:
"""The translation component of this transformation"""
return self.trl
def getRotation(self) -> Rotation3d:
"""The rotation component of this transformation"""
return self.rot
def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
@classmethod
def makeRelativeTo(cls, pose: Pose3d) -> Self:
"""The rotation-translation transformation that makes poses in the world consider this pose as the
new origin, or change the basis to this pose.
:param pose: The new origin
"""
return cls(pose.rotation(), pose.translation()).inverse()
@classmethod

63
photon-lib/py/photonlibpy/estimation/targetModel.py
View File

@@ -8,14 +8,27 @@ from . import RotTrlTransform3d
class TargetModel:
"""Describes the 3d model of a target."""
def __init__(self):
"""Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
createCuboid, createSpheroid or create Arbitrary
"""
self.vertices: List[Translation3d] = []
self.isPlanar = False
self.isSpherical = False
@classmethod
def createPlanar(cls, width: meters, height: meters) -> Self:
"""Creates a rectangular, planar target model given the width and height. The model has four
vertices:
- Point 0: [0, -width/2, -height/2]
- Point 1: [0, width/2, -height/2]
- Point 2: [0, width/2, height/2]
- Point 3: [0, -width/2, height/2]
"""
tm = cls()
tm.isPlanar = True
@@ -30,6 +43,18 @@ class TargetModel:
@classmethod
def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
"""Creates a cuboid target model given the length, width, height. The model has eight vertices:
- Point 0: [length/2, -width/2, -height/2]
- Point 1: [length/2, width/2, -height/2]
- Point 2: [length/2, width/2, height/2]
- Point 3: [length/2, -width/2, height/2]
- Point 4: [-length/2, -width/2, height/2]
- Point 5: [-length/2, width/2, height/2]
- Point 6: [-length/2, width/2, -height/2]
- Point 7: [-length/2, -width/2, -height/2]
"""
tm = cls()
verts = [
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
@@ -48,6 +73,20 @@ class TargetModel:
@classmethod
def createSpheroid(cls, diameter: meters) -> Self:
"""Creates a spherical target model which has similar dimensions regardless of its rotation. This
model has four vertices:
- Point 0: [0, -radius, 0]
- Point 1: [0, 0, -radius]
- Point 2: [0, radius, 0]
- Point 3: [0, 0, radius]
*Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
are used for drawing the image of this sphere, but do not match the corners that will be
published by photonvision.
"""
tm = cls()
tm.isPlanar = False
@@ -63,6 +102,14 @@ class TargetModel:
@classmethod
def createArbitrary(cls, verts: List[Translation3d]) -> Self:
"""Creates a target model from arbitrary 3d vertices. Automatically determines if the given
vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
vertices should define a non-intersecting contour.
:param vertices: Translations representing the vertices of this target model relative to its
pose.
"""
tm = cls()
tm._common_construction(verts)
@@ -83,6 +130,12 @@ class TargetModel:
self.vertices = verts
def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
"""This target's vertices offset from its field pose.
Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
Translation3d)} with this method.
"""
basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
retVal = []
@@ -94,6 +147,16 @@ class TargetModel:
@classmethod
def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
"""Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
to the camera translation. This is used for spherical targets which should not have their
projection change regardless of their own rotation.
:param tgtTrl: This target's translation
:param cameraTrl: Camera's translation
:returns: This target's pose oriented to the camera
"""
relCam = cameraTrl - tgtTrl
orientToCam = Rotation3d(
0.0,

22
photon-lib/py/photonlibpy/estimation/visionEstimation.py
View File

@@ -11,6 +11,7 @@ class VisionEstimation:
def getVisibleLayoutTags(
visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
) -> list[AprilTag]:
"""Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
retVal: list[AprilTag] = []
for tag in visTags:
id = tag.getFiducialId()
@@ -30,12 +31,31 @@ class VisionEstimation:
layout: AprilTagFieldLayout,
tagModel: TargetModel,
) -> PnpResult | None:
"""Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
field-to-camera transformation. If only one tag is visible, the result may have an alternate
solution.
**Note:** The returned transformation is from the field origin to the camera pose!
With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
:param cameraMatrix: The camera intrinsics matrix in standard opencv form
:param distCoeffs: The camera distortion matrix in standard opencv form
:param visTags: The visible tags reported by PV. Non-tag targets are automatically excluded.
:param tagLayout: The known tag layout on the field
:returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
PnpResult} are present before utilizing them.
"""
if len(visTags) == 0:
return None
corners: list[TargetCorner] = []
knownTags: list[AprilTag] = []
# ensure these are AprilTags in our layout
for tgt in visTags:
id = tgt.getFiducialId()
maybePose = layout.getTagPose(id)
@@ -53,6 +73,7 @@ class VisionEstimation:
points = OpenCVHelp.cornersToPoints(corners)
# single-tag pnp
if len(knownTags) == 1:
camToTag = OpenCVHelp.solvePNP_Square(
cameraMatrix, distCoeffs, tagModel.getVertices(), points
@@ -74,6 +95,7 @@ class VisionEstimation:
altReprojErr=camToTag.altReprojErr,
)
return result
# multi-tag pnp
else:
objectTrls: list[Translation3d] = []
for tag in knownTags: