Skip to content

Commit

Permalink
Improve docs for PhotonPoseEstimator (#901)
Browse files Browse the repository at this point in the history
In particular, document that update() ensures that new data is only used once.
  • Loading branch information
rzblue committed Aug 30, 2023
1 parent 8751764 commit de39441
Showing 1 changed file with 35 additions and 20 deletions.
55 changes: 35 additions & 20 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
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

0 comments on commit de39441

Please sign in to comment.