photonlibpy 2025.0.0b2__py3-none-any.whl → 2025.0.0b4__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -1,5 +1,5 @@
1
1
  import math
2
- from typing import Any, Tuple
2
+ from typing import Any
3
3
 
4
4
  import cv2 as cv
5
5
  import numpy as np
@@ -27,6 +27,12 @@ class OpenCVHelp:
27
27
 
28
28
  @staticmethod
29
29
  def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
30
+ """Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
31
+ three elements representing {x, y, z} in the EDN coordinate system.
32
+
33
+ :param translations: The translations to convert into a np.array
34
+ """
35
+
30
36
  retVal: list[list] = []
31
37
  for translation in translations:
32
38
  trl = OpenCVHelp.translationNWUtoEDN(translation)
@@ -38,6 +44,13 @@ class OpenCVHelp:
38
44
 
39
45
  @staticmethod
40
46
  def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
47
+ """Creates a new :class:`.np.array` with this 3d rotation. The opencv rvec Mat is a vector with
48
+ three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
49
+ norm, and axis = rvec / norm)
50
+
51
+ :param rotation: The rotation to convert into a np.array
52
+ """
53
+
41
54
  retVal: list[np.ndarray] = []
42
55
  rot = OpenCVHelp.rotationNWUtoEDN(rotation)
43
56
  rotVec = rot.getQuaternion().toRotationVector()
@@ -48,13 +61,13 @@ class OpenCVHelp:
48
61
  )
49
62
 
50
63
  @staticmethod
51
- def avgPoint(points: list[Tuple[float, float]]) -> Tuple[float, float]:
64
+ def avgPoint(points: np.ndarray) -> np.ndarray:
52
65
  x = 0.0
53
66
  y = 0.0
54
67
  for p in points:
55
- x += p[0]
56
- y += p[1]
57
- return (x / len(points), y / len(points))
68
+ x += p[0, 0]
69
+ y += p[0, 1]
70
+ return np.array([[x / len(points), y / len(points)]])
58
71
 
59
72
  @staticmethod
60
73
  def pointsToTargetCorners(points: np.ndarray) -> list[TargetCorner]:
@@ -88,6 +101,25 @@ class OpenCVHelp:
88
101
  def reorderCircular(
89
102
  elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
90
103
  ) -> list[Any]:
104
+ """Reorders the list, optionally indexing backwards and wrapping around to the last element after
105
+ the first, and shifting all indices in the direction of indexing.
106
+
107
+ e.g.
108
+
109
+ ({1,2,3}, false, 1) == {2,3,1}
110
+
111
+ ({1,2,3}, true, 0) == {1,3,2}
112
+
113
+ ({1,2,3}, true, 1) == {3,2,1}
114
+
115
+ :param elements: list elements
116
+ :param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
117
+ :param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
118
+ start at shiftStart, negated if backwards)
119
+
120
+ :returns: Reordered list
121
+ """
122
+
91
123
  size = len(elements)
92
124
  reordered = []
93
125
  dir = -1 if backwards else 1
@@ -100,18 +132,39 @@ class OpenCVHelp:
100
132
 
101
133
  @staticmethod
102
134
  def translationEDNToNWU(trl: Translation3d) -> Translation3d:
135
+ """Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
136
+ in EDN, this would be {0, -1, 0} in NWU.
137
+ """
138
+
103
139
  return trl.rotateBy(EDN_TO_NWU)
104
140
 
105
141
  @staticmethod
106
142
  def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
143
+ """Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
144
+ in NWU, this would be {0, 0, 1} in EDN.
145
+ """
146
+
107
147
  return -EDN_TO_NWU + (rot + EDN_TO_NWU)
108
148
 
109
149
  @staticmethod
110
150
  def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
151
+ """Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
152
+ elements representing {x, y, z} in the EDN coordinate system.
153
+
154
+ :param tvecInput: The tvec to create a Translation3d from
155
+ """
156
+
111
157
  return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
112
158
 
113
159
  @staticmethod
114
160
  def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
161
+ """Returns a 3d rotation from this :class:`.Mat`. The opencv rvec Mat is a vector with three
162
+ elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
163
+ and axis = rvec / norm)
164
+
165
+ :param rvecInput: The rvec to create a Rotation3d from
166
+ """
167
+
115
168
  return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
116
169
 
117
170
  @staticmethod
@@ -121,12 +174,43 @@ class OpenCVHelp:
121
174
  modelTrls: list[Translation3d],
122
175
  imagePoints: np.ndarray,
123
176
  ) -> PnpResult | None:
177
+ """Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
178
+ relative to the target is determined by the supplied 3d points of the target's model and their
179
+ associated 2d points imaged by the camera. The supplied model translations must be relative to
180
+ the target's pose.
181
+
182
+ For planar targets, there may be an alternate solution which is plausible given the 2d image
183
+ points. This has an associated "ambiguity" which describes the ratio of reprojection error
184
+ between the "best" and "alternate" solution.
185
+
186
+ This method is intended for use with individual AprilTags, and will not work unless 4 points
187
+ are provided.
188
+
189
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
190
+ :param distCoeffs: The camera distortion matrix in standard opencv form
191
+ :param modelTrls: The translations of the object corners. These should have the object pose as
192
+ their origin. These must come in a specific, pose-relative order (in NWU):
193
+
194
+ - Point 0: [0, -squareLength / 2, squareLength / 2]
195
+ - Point 1: [0, squareLength / 2, squareLength / 2]
196
+ - Point 2: [0, squareLength / 2, -squareLength / 2]
197
+ - Point 3: [0, -squareLength / 2, -squareLength / 2]
198
+ :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
199
+ should match the given object point translations.
200
+
201
+ :returns: The resulting transformation that maps the camera pose to the target pose and the
202
+ ambiguity if an alternate solution is available.
203
+ """
124
204
  modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
125
205
  imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
126
206
  objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
127
207
 
128
208
  alt: Transform3d | None = None
209
+ reprojectionError: cv.typing.MatLike | None = None
210
+ best: Transform3d = Transform3d()
211
+
129
212
  for tries in range(2):
213
+ # calc rvecs/tvecs and associated reprojection error from image points
130
214
  retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
131
215
  objectMat,
132
216
  imagePoints,
@@ -135,6 +219,7 @@ class OpenCVHelp:
135
219
  flags=cv.SOLVEPNP_IPPE_SQUARE,
136
220
  )
137
221
 
222
+ # convert to wpilib coordinates
138
223
  best = Transform3d(
139
224
  OpenCVHelp.tVecToTranslation(tvecs[0]),
140
225
  OpenCVHelp.rVecToRotation(rvecs[0]),
@@ -145,7 +230,10 @@ class OpenCVHelp:
145
230
  OpenCVHelp.rVecToRotation(rvecs[1]),
146
231
  )
147
232
 
148
- if not math.isnan(reprojectionError[0, 0]):
233
+ # check if we got a NaN result
234
+ if reprojectionError is not None and not math.isnan(
235
+ reprojectionError[0, 0]
236
+ ):
149
237
  break
150
238
  else:
151
239
  pt = imagePoints[0]
@@ -153,7 +241,8 @@ class OpenCVHelp:
153
241
  pt[0, 1] -= 0.001
154
242
  imagePoints[0] = pt
155
243
 
156
- if math.isnan(reprojectionError[0, 0]):
244
+ # solvePnP failed
245
+ if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
157
246
  print("SolvePNP_Square failed!")
158
247
  return None
159
248
 
@@ -181,6 +270,27 @@ class OpenCVHelp:
181
270
  modelTrls: list[Translation3d],
182
271
  imagePoints: np.ndarray,
183
272
  ) -> PnpResult | None:
273
+ """Finds the transformation that maps the camera's pose to the origin of the supplied object. An
274
+ "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
275
+ for example, the object translations are given relative to close-right corner of the blue
276
+ alliance(the default origin), a camera-to-origin transformation is returned. If the
277
+ translations are relative to a target's pose, a camera-to-target transformation is returned.
278
+
279
+ There must be at least 3 points to use this method. This does not return an alternate
280
+ solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
281
+ #solvePNP_SQUARE} instead.
282
+
283
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
284
+ :param distCoeffs: The camera distortion matrix in standard opencv form
285
+ :param objectTrls: The translations of the object corners, relative to the field.
286
+ :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
287
+ should match the given object point translations.
288
+
289
+ :returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
290
+ model points are supplied relative to the origin, this transformation brings the camera to
291
+ the origin.
292
+ """
293
+
184
294
  objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
185
295
 
186
296
  retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
@@ -4,29 +4,71 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
4
4
 
5
5
 
6
6
  class RotTrlTransform3d:
7
+ """Represents a transformation that first rotates a pose around the origin, and then translates it."""
8
+
7
9
  def __init__(
8
10
  self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
9
11
  ):
12
+ """A rotation-translation transformation.
13
+
14
+ Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
15
+ transform as if the origin was transformed by these components instead.
16
+
17
+ :param rot: The rotation component
18
+ :param trl: The translation component
19
+ """
10
20
  self.rot = rot
11
21
  self.trl = trl
12
22
 
13
23
  def inverse(self) -> Self:
24
+ """The inverse of this transformation. Applying the inverse will "undo" this transformation."""
14
25
  invRot = -self.rot
15
26
  invTrl = -(self.trl.rotateBy(invRot))
16
27
  return type(self)(invRot, invTrl)
17
28
 
18
29
  def getTransform(self) -> Transform3d:
30
+ """This transformation as a Transform3d (as if of the origin)"""
19
31
  return Transform3d(self.trl, self.rot)
20
32
 
21
33
  def getTranslation(self) -> Translation3d:
34
+ """The translation component of this transformation"""
22
35
  return self.trl
23
36
 
24
37
  def getRotation(self) -> Rotation3d:
38
+ """The rotation component of this transformation"""
25
39
  return self.rot
26
40
 
27
- def apply(self, trlToApply: Translation3d) -> Translation3d:
41
+ def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
28
42
  return trlToApply.rotateBy(self.rot) + self.trl
29
43
 
44
+ def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
45
+ return rotToApply + self.rot
46
+
47
+ def applyPose(self, poseToApply: Pose3d) -> Pose3d:
48
+ return Pose3d(
49
+ self.applyTranslation(poseToApply.translation()),
50
+ self.applyRotation(poseToApply.rotation()),
51
+ )
52
+
53
+ def applyTrls(self, rots: list[Rotation3d]) -> list[Rotation3d]:
54
+ retVal: list[Rotation3d] = []
55
+ for rot in rots:
56
+ retVal.append(self.applyRotation(rot))
57
+ return retVal
58
+
30
59
  @classmethod
31
60
  def makeRelativeTo(cls, pose: Pose3d) -> Self:
61
+ """The rotation-translation transformation that makes poses in the world consider this pose as the
62
+ new origin, or change the basis to this pose.
63
+
64
+ :param pose: The new origin
65
+ """
32
66
  return cls(pose.rotation(), pose.translation()).inverse()
67
+
68
+ @classmethod
69
+ def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
70
+ return cls(
71
+ last.rotation() - initial.rotation(),
72
+ last.translation()
73
+ - initial.translation().rotateBy(last.rotation() - initial.rotation()),
74
+ )
@@ -8,87 +8,117 @@ from . import RotTrlTransform3d
8
8
 
9
9
 
10
10
  class TargetModel:
11
- def __init__(
12
- self,
13
- *,
14
- width: meters | None = None,
15
- height: meters | None = None,
16
- length: meters | None = None,
17
- diameter: meters | None = None,
18
- verts: List[Translation3d] | None = None
19
- ):
20
-
21
- if (
22
- width is not None
23
- and height is not None
24
- and length is None
25
- and diameter is None
26
- and verts is None
27
- ):
28
- self.isPlanar = True
29
- self.isSpherical = False
30
- self.vertices = [
31
- Translation3d(0.0, -width / 2.0, -height / 2.0),
32
- Translation3d(0.0, width / 2.0, -height / 2.0),
33
- Translation3d(0.0, width / 2.0, height / 2.0),
34
- Translation3d(0.0, -width / 2.0, height / 2.0),
35
- ]
36
-
37
- return
38
-
39
- elif (
40
- length is not None
41
- and width is not None
42
- and height is not None
43
- and diameter is None
44
- and verts is None
45
- ):
46
- verts = [
47
- Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
48
- Translation3d(length / 2.0, width / 2.0, -height / 2.0),
49
- Translation3d(length / 2.0, width / 2.0, height / 2.0),
50
- Translation3d(length / 2.0, -width / 2.0, height / 2.0),
51
- Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
52
- Translation3d(-length / 2.0, width / 2.0, height / 2.0),
53
- Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
54
- Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
55
- ]
56
- # Handle the rest of this in the "default" case
57
- elif (
58
- diameter is not None
59
- and width is None
60
- and height is None
61
- and length is None
62
- and verts is None
63
- ):
64
- self.isPlanar = False
65
- self.isSpherical = True
66
- self.vertices = [
67
- Translation3d(0.0, -diameter / 2.0, 0.0),
68
- Translation3d(0.0, 0.0, -diameter / 2.0),
69
- Translation3d(0.0, diameter / 2.0, 0.0),
70
- Translation3d(0.0, 0.0, diameter / 2.0),
71
- ]
72
- return
73
- elif (
74
- verts is not None
75
- and width is None
76
- and height is None
77
- and length is None
78
- and diameter is None
79
- ):
80
- # Handle this in the "default" case
81
- pass
82
- else:
83
- raise Exception("Not a valid overload")
11
+ """Describes the 3d model of a target."""
12
+
13
+ def __init__(self):
14
+ """Default constructor for initialising internal class members. DO NOT USE THIS!!! USE THE createPlanar,
15
+ createCuboid, createSpheroid or create Arbitrary
16
+ """
17
+ self.vertices: List[Translation3d] = []
18
+ self.isPlanar = False
19
+ self.isSpherical = False
20
+
21
+ @classmethod
22
+ def createPlanar(cls, width: meters, height: meters) -> Self:
23
+ """Creates a rectangular, planar target model given the width and height. The model has four
24
+ vertices:
25
+
26
+ - Point 0: [0, -width/2, -height/2]
27
+ - Point 1: [0, width/2, -height/2]
28
+ - Point 2: [0, width/2, height/2]
29
+ - Point 3: [0, -width/2, height/2]
30
+ """
31
+
32
+ tm = cls()
33
+
34
+ tm.isPlanar = True
35
+ tm.isSpherical = False
36
+ tm.vertices = [
37
+ Translation3d(0.0, -width / 2.0, -height / 2.0),
38
+ Translation3d(0.0, width / 2.0, -height / 2.0),
39
+ Translation3d(0.0, width / 2.0, height / 2.0),
40
+ Translation3d(0.0, -width / 2.0, height / 2.0),
41
+ ]
42
+ return tm
43
+
44
+ @classmethod
45
+ def createCuboid(cls, length: meters, width: meters, height: meters) -> Self:
46
+ """Creates a cuboid target model given the length, width, height. The model has eight vertices:
47
+
48
+ - Point 0: [length/2, -width/2, -height/2]
49
+ - Point 1: [length/2, width/2, -height/2]
50
+ - Point 2: [length/2, width/2, height/2]
51
+ - Point 3: [length/2, -width/2, height/2]
52
+ - Point 4: [-length/2, -width/2, height/2]
53
+ - Point 5: [-length/2, width/2, height/2]
54
+ - Point 6: [-length/2, width/2, -height/2]
55
+ - Point 7: [-length/2, -width/2, -height/2]
56
+ """
57
+
58
+ tm = cls()
59
+ verts = [
60
+ Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
61
+ Translation3d(length / 2.0, width / 2.0, -height / 2.0),
62
+ Translation3d(length / 2.0, width / 2.0, height / 2.0),
63
+ Translation3d(length / 2.0, -width / 2.0, height / 2.0),
64
+ Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
65
+ Translation3d(-length / 2.0, width / 2.0, height / 2.0),
66
+ Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
67
+ Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
68
+ ]
69
+
70
+ tm._common_construction(verts)
71
+
72
+ return tm
73
+
74
+ @classmethod
75
+ def createSpheroid(cls, diameter: meters) -> Self:
76
+ """Creates a spherical target model which has similar dimensions regardless of its rotation. This
77
+ model has four vertices:
78
+
79
+ - Point 0: [0, -radius, 0]
80
+ - Point 1: [0, 0, -radius]
81
+ - Point 2: [0, radius, 0]
82
+ - Point 3: [0, 0, radius]
83
+
84
+ *Q: Why these vertices?* A: This target should be oriented to the camera every frame, much
85
+ like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
86
+ are used for drawing the image of this sphere, but do not match the corners that will be
87
+ published by photonvision.
88
+ """
89
+
90
+ tm = cls()
91
+
92
+ tm.isPlanar = False
93
+ tm.isSpherical = True
94
+ tm.vertices = [
95
+ Translation3d(0.0, -diameter / 2.0, 0.0),
96
+ Translation3d(0.0, 0.0, -diameter / 2.0),
97
+ Translation3d(0.0, diameter / 2.0, 0.0),
98
+ Translation3d(0.0, 0.0, diameter / 2.0),
99
+ ]
100
+
101
+ return tm
84
102
 
85
- # TODO maybe remove this if there is a better/preferred way
86
- # make the python type checking gods happy
87
- assert verts is not None
103
+ @classmethod
104
+ def createArbitrary(cls, verts: List[Translation3d]) -> Self:
105
+ """Creates a target model from arbitrary 3d vertices. Automatically determines if the given
106
+ vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
107
+ vertices should define a non-intersecting contour.
108
+
109
+ :param vertices: Translations representing the vertices of this target model relative to its
110
+ pose.
111
+ """
112
+
113
+ tm = cls()
114
+ tm._common_construction(verts)
88
115
 
116
+ return tm
117
+
118
+ def _common_construction(self, verts: List[Translation3d]) -> None:
89
119
  self.isSpherical = False
90
120
  if len(verts) <= 2:
91
- self.vertices: List[Translation3d] = []
121
+ self.vertices = []
92
122
  self.isPlanar = False
93
123
  else:
94
124
  cornersPlaner = True
@@ -100,17 +130,33 @@ class TargetModel:
100
130
  self.vertices = verts
101
131
 
102
132
  def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
133
+ """This target's vertices offset from its field pose.
134
+
135
+ Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
136
+ Translation3d)} with this method.
137
+ """
138
+
103
139
  basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
104
140
 
105
141
  retVal = []
106
142
 
107
143
  for vert in self.vertices:
108
- retVal.append(basisChange.apply(vert))
144
+ retVal.append(basisChange.applyTranslation(vert))
109
145
 
110
146
  return retVal
111
147
 
112
148
  @classmethod
113
149
  def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
150
+ """Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
151
+ to the camera translation. This is used for spherical targets which should not have their
152
+ projection change regardless of their own rotation.
153
+
154
+ :param tgtTrl: This target's translation
155
+ :param cameraTrl: Camera's translation
156
+
157
+ :returns: This target's pose oriented to the camera
158
+ """
159
+
114
160
  relCam = cameraTrl - tgtTrl
115
161
  orientToCam = Rotation3d(
116
162
  0.0,
@@ -130,8 +176,8 @@ class TargetModel:
130
176
 
131
177
  @classmethod
132
178
  def AprilTag36h11(cls) -> Self:
133
- return cls(width=6.5 * 0.0254, height=6.5 * 0.0254)
179
+ return cls.createPlanar(width=6.5 * 0.0254, height=6.5 * 0.0254)
134
180
 
135
181
  @classmethod
136
182
  def AprilTag16h5(cls) -> Self:
137
- return cls(width=6.0 * 0.0254, height=6.0 * 0.0254)
183
+ return cls.createPlanar(width=6.0 * 0.0254, height=6.0 * 0.0254)
@@ -11,15 +11,16 @@ class VisionEstimation:
11
11
  def getVisibleLayoutTags(
12
12
  visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
13
13
  ) -> list[AprilTag]:
14
+ """Get the visible :class:`.AprilTag`s which are in the tag layout using the visible tag IDs."""
14
15
  retVal: list[AprilTag] = []
15
16
  for tag in visTags:
16
17
  id = tag.getFiducialId()
17
18
  maybePose = layout.getTagPose(id)
18
19
  if maybePose:
19
- tag = AprilTag()
20
- tag.ID = id
21
- tag.pose = maybePose
22
- retVal.append(tag)
20
+ aprilTag = AprilTag()
21
+ aprilTag.ID = id
22
+ aprilTag.pose = maybePose
23
+ retVal.append(aprilTag)
23
24
  return retVal
24
25
 
25
26
  @staticmethod
@@ -30,12 +31,31 @@ class VisionEstimation:
30
31
  layout: AprilTagFieldLayout,
31
32
  tagModel: TargetModel,
32
33
  ) -> PnpResult | None:
34
+ """Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
35
+ field-to-camera transformation. If only one tag is visible, the result may have an alternate
36
+ solution.
37
+
38
+ **Note:** The returned transformation is from the field origin to the camera pose!
39
+
40
+ With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
41
+
42
+ With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
43
+
44
+ :param cameraMatrix: The camera intrinsics matrix in standard opencv form
45
+ :param distCoeffs: The camera distortion matrix in standard opencv form
46
+ :param visTags: The visible tags reported by PV. Non-tag targets are automatically excluded.
47
+ :param tagLayout: The known tag layout on the field
48
+
49
+ :returns: The transformation that maps the field origin to the camera pose. Ensure the {@link
50
+ PnpResult} are present before utilizing them.
51
+ """
33
52
  if len(visTags) == 0:
34
53
  return None
35
54
 
36
55
  corners: list[TargetCorner] = []
37
56
  knownTags: list[AprilTag] = []
38
57
 
58
+ # ensure these are AprilTags in our layout
39
59
  for tgt in visTags:
40
60
  id = tgt.getFiducialId()
41
61
  maybePose = layout.getTagPose(id)
@@ -53,6 +73,7 @@ class VisionEstimation:
53
73
 
54
74
  points = OpenCVHelp.cornersToPoints(corners)
55
75
 
76
+ # single-tag pnp
56
77
  if len(knownTags) == 1:
57
78
  camToTag = OpenCVHelp.solvePNP_Square(
58
79
  cameraMatrix, distCoeffs, tagModel.getVertices(), points
@@ -74,6 +95,7 @@ class VisionEstimation:
74
95
  altReprojErr=camToTag.altReprojErr,
75
96
  )
76
97
  return result
98
+ # multi-tag pnp
77
99
  else:
78
100
  objectTrls: list[Translation3d] = []
79
101
  for tag in knownTags:
@@ -20,8 +20,14 @@
20
20
  ## --> DO NOT MODIFY <--
21
21
  ###############################################################################
22
22
 
23
+ from typing import TYPE_CHECKING
24
+
23
25
  from ..packet import Packet
24
- from ..targeting import *
26
+ from ..targeting import * # noqa
27
+
28
+ if TYPE_CHECKING:
29
+ from ..targeting import MultiTargetPNPResult # noqa
30
+ from ..targeting import PnpResult # noqa
25
31
 
26
32
 
27
33
  class MultiTargetPNPResultSerde:
@@ -20,8 +20,13 @@
20
20
  ## --> DO NOT MODIFY <--
21
21
  ###############################################################################
22
22
 
23
+ from typing import TYPE_CHECKING
24
+
23
25
  from ..packet import Packet
24
- from ..targeting import *
26
+ from ..targeting import * # noqa
27
+
28
+ if TYPE_CHECKING:
29
+ from ..targeting import PhotonPipelineMetadata # noqa
25
30
 
26
31
 
27
32
  class PhotonPipelineMetadataSerde:
@@ -20,8 +20,16 @@
20
20
  ## --> DO NOT MODIFY <--
21
21
  ###############################################################################
22
22
 
23
+ from typing import TYPE_CHECKING
24
+
23
25
  from ..packet import Packet
24
- from ..targeting import *
26
+ from ..targeting import * # noqa
27
+
28
+ if TYPE_CHECKING:
29
+ from ..targeting import MultiTargetPNPResult # noqa
30
+ from ..targeting import PhotonPipelineMetadata # noqa
31
+ from ..targeting import PhotonPipelineResult # noqa
32
+ from ..targeting import PhotonTrackedTarget # noqa
25
33
 
26
34
 
27
35
  class PhotonPipelineResultSerde:
@@ -20,8 +20,14 @@
20
20
  ## --> DO NOT MODIFY <--
21
21
  ###############################################################################
22
22
 
23
+ from typing import TYPE_CHECKING
24
+
23
25
  from ..packet import Packet
24
- from ..targeting import *
26
+ from ..targeting import * # noqa
27
+
28
+ if TYPE_CHECKING:
29
+ from ..targeting import PhotonTrackedTarget # noqa
30
+ from ..targeting import TargetCorner # noqa
25
31
 
26
32
 
27
33
  class PhotonTrackedTargetSerde: