photonlibpy.estimation.visionEstimation
VisionEstimation
estimateCamPosePNP(cameraMatrix, distCoeffs, visTags, layout, tagModel)
staticmethod
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}
Parameters:
Name | Type | Description | Default |
---|---|---|---|
cameraMatrix
|
ndarray
|
The camera intrinsics matrix in standard opencv form |
required |
distCoeffs
|
ndarray
|
The camera distortion matrix in standard opencv form |
required |
visTags
|
list[PhotonTrackedTarget]
|
The visible tags reported by PV. Non-tag targets are automatically excluded. |
required |
tagLayout
|
The known tag layout on the field |
required |
Returns:
Type | Description |
---|---|
PnpResult | None
|
The transformation that maps the field origin to the camera pose. Ensure the {@link PnpResult} are present before utilizing them. |
getVisibleLayoutTags(visTags, layout)
staticmethod
Get the visible :class:.AprilTag
s which are in the tag layout using the visible tag IDs.