Package org.photonvision.estimation

Class VisionEstimation

java.lang.Object

org.photonvision.estimation.VisionEstimation

public class VisionEstimation
extends Object

Constructor Summary

Constructors

Constructor	Description
VisionEstimation()

Method Summary

Modifier and Type	Method	Description
static Optional<PnpResult>	estimateCamPosePNP(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout, TargetModel tagModel)	Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.
static Optional<PnpResult>	estimateRobotPoseConstrainedSolvepnp(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, List<PhotonTrackedTarget> visTags, Transform3d robot2camera, Pose3d robotPoseSeed, AprilTagFieldLayout tagLayout, TargetModel tagModel, boolean headingFree, Rotation2d gyroθ, double gyroErrorScaleFac)	Performs constrained solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.
static List<AprilTag>	getVisibleLayoutTags(List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout)	Get the visible AprilTags which are in the tag layout using the visible tag IDs.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Details

VisionEstimation

public VisionEstimation()

Method Details

getVisibleLayoutTags

public static List<AprilTag> getVisibleLayoutTags(List<PhotonTrackedTarget> visTags,
 AprilTagFieldLayout tagLayout)

Get the visible AprilTags which are in the tag layout using the visible tag IDs.

estimateCamPosePNP

public static Optional<PnpResult> estimateCamPosePNP(Matrix<N3,N3> cameraMatrix,
 Matrix<N8,N1> distCoeffs,
 List<PhotonTrackedTarget> visTags,
 AprilTagFieldLayout tagLayout,
 TargetModel tagModel)

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: OpenCVHelp.solvePNP_SQUARE(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N3>, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8, edu.wpi.first.math.numbers.N1>, java.util.List<edu.wpi.first.math.geometry.Translation3d>, org.opencv.core.Point[])

With multiple tags: OpenCVHelp.solvePNP_SQPNP(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N3>, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8, edu.wpi.first.math.numbers.N1>, java.util.List<edu.wpi.first.math.geometry.Translation3d>, org.opencv.core.Point[])

Parameters:

cameraMatrix - The camera intrinsics matrix in standard opencv form

distCoeffs - The camera distortion matrix in standard opencv form

visTags - The visible tags reported by PV. Non-tag targets are automatically excluded.

tagLayout - The known tag layout on the field

Returns:

The transformation that maps the field origin to the camera pose. Ensure the PnpResult are present before utilizing them.

estimateRobotPoseConstrainedSolvepnp

public static Optional<PnpResult> estimateRobotPoseConstrainedSolvepnp(Matrix<N3,N3> cameraMatrix,
 Matrix<N8,N1> distCoeffs,
 List<PhotonTrackedTarget> visTags,
 Transform3d robot2camera,
 Pose3d robotPoseSeed,
 AprilTagFieldLayout tagLayout,
 TargetModel tagModel,
 boolean headingFree,
 Rotation2d gyroθ,
 double gyroErrorScaleFac)

Performs constrained solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.

Note: The returned transformation is from the field origin to the robot drivebase

Parameters:

cameraMatrix - The camera intrinsics matrix in standard opencv form

distCoeffs - The camera distortion matrix in standard opencv form

visTags - The visible tags reported by PV. Non-tag targets are automatically excluded.

robot2camera - The Transform3d from the robot odometry frame to the camera optical frame

robotPoseSeed - An initial guess at robot pose, refined via optimizaiton. Better guesses will converge faster.

tagLayout - The known tag layout on the field

tagModel - The physical size of the AprilTags

headingFree - If heading is completely free, or if our measured gyroθ is taken into account

gyro\u03b8 - If headingFree is false, the best estimate at robot yaw. Excursions from this are penalized in our cost function.

gyroErrorScaleFac - A relative weight for gyro heading excursions against tag corner reprojection error.

Returns:

The transformation that maps the field origin to the camera pose. Ensure the PnpResult are present before utilizing them.