@@ -33,7 +33,8 @@ def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
33
33
"""Creates a new :class:`np.array` with these 3d translations. The opencv tvec is a vector with
34
34
three elements representing {x, y, z} in the EDN coordinate system.
35
35
36
- :param translations: The translations to convert into a np.array
36
+ Args:
37
+ translations: The translations to convert into a np.array
37
38
"""
38
39
39
40
retVal : list [list ] = []
@@ -51,7 +52,8 @@ def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
51
52
three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
52
53
norm, and axis = rvec / norm)
53
54
54
- :param rotation: The rotation to convert into a np.array
55
+ Args:
56
+ rotation: The rotation to convert into a np.array
55
57
"""
56
58
57
59
retVal : list [np .ndarray ] = []
@@ -115,12 +117,16 @@ def reorderCircular(
115
117
116
118
({1,2,3}, true, 1) == {3,2,1}
117
119
118
- :param elements: list elements
119
- :param backwards: If indexing should happen in reverse (0, size-1, size-2, ...)
120
- :param shiftStart: How much the initial index should be shifted (instead of starting at index 0,
121
- start at shiftStart, negated if backwards)
120
+ Args:
121
+ elements: list elements
122
+ backwards: If indexing should happen in reverse (0, size-1,
123
+ size-2, ...)
124
+ shiftStart: How much the initial index should be shifted
125
+ (instead of starting at index 0, start at shiftStart,
126
+ negated if backwards)
122
127
123
- :returns: Reordered list
128
+ Returns:
129
+ Reordered list
124
130
"""
125
131
126
132
size = len (elements )
@@ -154,7 +160,8 @@ def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
154
160
"""Returns a new 3d translation from this :class:`.Mat`. The opencv tvec is a vector with three
155
161
elements representing {x, y, z} in the EDN coordinate system.
156
162
157
- :param tvecInput: The tvec to create a Translation3d from
163
+ Args:
164
+ tvecInput: The tvec to create a Translation3d from
158
165
"""
159
166
160
167
return OpenCVHelp .translationEDNToNWU (Translation3d (tvecInput ))
@@ -165,7 +172,8 @@ def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
165
172
elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
166
173
and axis = rvec / norm)
167
174
168
- :param rvecInput: The rvec to create a Rotation3d from
175
+ Args:
176
+ rvecInput: The rvec to create a Rotation3d from
169
177
"""
170
178
171
179
return OpenCVHelp .rotationEDNToNWU (Rotation3d (rvecInput ))
@@ -189,20 +197,27 @@ def solvePNP_Square(
189
197
This method is intended for use with individual AprilTags, and will not work unless 4 points
190
198
are provided.
191
199
192
- :param cameraMatrix: The camera intrinsics matrix in standard opencv form
193
- :param distCoeffs: The camera distortion matrix in standard opencv form
194
- :param modelTrls: The translations of the object corners. These should have the object pose as
195
- their origin. These must come in a specific, pose-relative order (in NWU):
196
-
197
- - Point 0: [0, -squareLength / 2, squareLength / 2]
198
- - Point 1: [0, squareLength / 2, squareLength / 2]
199
- - Point 2: [0, squareLength / 2, -squareLength / 2]
200
- - Point 3: [0, -squareLength / 2, -squareLength / 2]
201
- :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
202
- should match the given object point translations.
203
-
204
- :returns: The resulting transformation that maps the camera pose to the target pose and the
205
- ambiguity if an alternate solution is available.
200
+ Args:
201
+ cameraMatrix: The camera intrinsics matrix in standard
202
+ opencv form
203
+ distCoeffs: The camera distortion matrix in standard opencv
204
+ form
205
+ modelTrls: The translations of the object corners. These
206
+ should have the object pose as their origin. These must
207
+ come in a specific, pose-relative order (in NWU):
208
+
209
+ - Point 0: [0, -squareLength / 2, squareLength / 2]
210
+ - Point 1: [0, squareLength / 2, squareLength / 2]
211
+ - Point 2: [0, squareLength / 2, -squareLength / 2]
212
+ - Point 3: [0, -squareLength / 2, -squareLength / 2]
213
+ imagePoints: The projection of these 3d object points into
214
+ the 2d camera image. The order should match the given
215
+ object point translations.
216
+
217
+ Returns:
218
+ The resulting transformation that maps the camera pose to
219
+ the target pose and the ambiguity if an alternate solution
220
+ is available.
206
221
"""
207
222
modelTrls = OpenCVHelp .reorderCircular (modelTrls , True , - 1 )
208
223
imagePoints = np .array (OpenCVHelp .reorderCircular (imagePoints , True , - 1 ))
@@ -283,15 +298,22 @@ def solvePNP_SQPNP(
283
298
solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
284
299
#solvePNP_SQUARE} instead.
285
300
286
- :param cameraMatrix: The camera intrinsics matrix in standard opencv form
287
- :param distCoeffs: The camera distortion matrix in standard opencv form
288
- :param objectTrls: The translations of the object corners, relative to the field.
289
- :param imagePoints: The projection of these 3d object points into the 2d camera image. The order
290
- should match the given object point translations.
291
-
292
- :returns: The resulting transformation that maps the camera pose to the target pose. If the 3d
293
- model points are supplied relative to the origin, this transformation brings the camera to
294
- the origin.
301
+ Args:
302
+ cameraMatrix: The camera intrinsics matrix in standard
303
+ opencv form
304
+ distCoeffs: The camera distortion matrix in standard opencv
305
+ form
306
+ objectTrls: The translations of the object corners, relative
307
+ to the field.
308
+ imagePoints: The projection of these 3d object points into
309
+ the 2d camera image. The order should match the given
310
+ object point translations.
311
+
312
+ Returns:
313
+ The resulting transformation that maps the camera pose to
314
+ the target pose. If the 3d model points are supplied
315
+ relative to the origin, this transformation brings the
316
+ camera to the origin.
295
317
"""
296
318
297
319
objectMat = np .array (OpenCVHelp .translationToTVec (modelTrls ))
0 commit comments