Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improve docs for PhotonPoseEstimator #901

Merged
merged 1 commit into from
Aug 30, 2023
Merged

Improve docs for PhotonPoseEstimator #901

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 35 additions & 20 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -250,10 +250,16 @@ public void setRobotToCameraTransform(Transform3d robotToCamera) {

/**
* Poll data from the configured cameras and update the estimated position of the robot. Returns
* empty if there are no cameras set or no targets were found from the cameras.
* empty if:
*
* @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create
* the estimate
* <ul>
* <li>New data has not been received since the last call to {@code update()}.
* <li>No targets were found from the camera
* <li>There is no camera set
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update() {
if (camera == null) {
Expand All @@ -267,34 +273,43 @@ public Optional<EstimatedRobotPose> update() {
}

/**
* Updates the estimated position of the robot. Returns empty if there are no cameras set or no
* targets were found from the cameras.
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* </ul>
*
* @param cameraResult The latest pipeline result from the camera
* @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
* pipeline results used to create the estimate
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}

/**
* Updates the estimated position of the robot. Returns empty if there are no cameras set or no
* targets were found from the cameras.
* Updates the estimated position of the robot. Returns empty if:
*
* @param cameraResult The latest pipeline result from the camera
* @param cameraMatrixData Camera calibration data that can be used in the case of no assigned
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* </ul>
*
* @param cameraMatrix Camera calibration data that can be used in the case of no assigned
* PhotonCamera.
* @param coeffsData Camera calibration data that can be used in the case of no assigned
* @param distCoeffs Camera calibration data that can be used in the case of no assigned
* PhotonCamera
* @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
* pipeline results used to create the estimate
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrixData,
Optional<Matrix<N5, N1>> coeffsData) {
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N5, N1>> distCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
Expand All @@ -316,13 +331,13 @@ public Optional<EstimatedRobotPose> update(
return Optional.empty();
}

return update(cameraResult, cameraMatrixData, coeffsData, this.primaryStrategy);
return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
}

private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrixData,
Optional<Matrix<N5, N1>> coeffsData,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N5, N1>> distCoeffs,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
switch (strat) {
Expand All @@ -343,7 +358,7 @@ private Optional<EstimatedRobotPose> update(
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrixData, coeffsData);
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
default:
DriverStation.reportError(
Expand Down