Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-02 02:51:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Open up pose estimator strategy methods (#2252)

Browse Source
This commit is contained in:
Gold856
2026-01-11 13:58:53 -05:00
committed by GitHub
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -43,6 +43,20 @@ public class EstimatedRobotPose {
/** The strategy actually used to produce this pose */
public final PoseStrategy strategy;
/**
* Constructs an EstimatedRobotPose
*
* @param estimatedPose estimated pose
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = null;
}
/**
* Constructs an EstimatedRobotPose
*

476
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -117,7 +117,7 @@ public class PhotonPoseEstimator {
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error const.
* heading estimate against the tag corner reprojection error cost.
*/
public static final record ConstrainedSolvepnpParams(
boolean headingFree, double headingScaleFactor) {}
@@ -144,12 +144,35 @@ public class PhotonPoseEstimator {
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
InstanceCount++;
}
/**
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
* @deprecated Use individual estimation methods with the 2 argument constructor instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
@@ -221,7 +244,9 @@ public class PhotonPoseEstimator {
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
@@ -230,7 +255,9 @@ public class PhotonPoseEstimator {
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setPrimaryStrategy(PoseStrategy strategy) {
checkUpdate(this.primaryStrategy, strategy);
@@ -246,7 +273,9 @@ public class PhotonPoseEstimator {
* NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
@@ -262,7 +291,9 @@ public class PhotonPoseEstimator {
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Pose3d getReferencePose() {
return referencePose;
}
@@ -272,7 +303,9 @@ public class PhotonPoseEstimator {
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose3d referencePose) {
checkUpdate(this.referencePose, referencePose);
this.referencePose = referencePose;
@@ -283,7 +316,9 @@ public class PhotonPoseEstimator {
* strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
@@ -293,7 +328,9 @@ public class PhotonPoseEstimator {
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
@@ -303,7 +340,9 @@ public class PhotonPoseEstimator {
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public void setLastPose(Pose2d lastPose) {
setLastPose(new Pose3d(lastPose));
}
@@ -389,9 +428,11 @@ public class PhotonPoseEstimator {
* provided in this overload.
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
return update(cameraResult, Optional.empty(), Optional.empty());
}
@@ -411,9 +452,11 @@ public class PhotonPoseEstimator {
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
@@ -436,9 +479,11 @@ public class PhotonPoseEstimator {
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
* @deprecated Use individual estimation methods instead.
*/
@Deprecated(forRemoval = true, since = "2026")
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
@@ -475,7 +520,7 @@ public class PhotonPoseEstimator {
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
@@ -491,21 +536,122 @@ public class PhotonPoseEstimator {
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
case LOWEST_AMBIGUITY -> lowestAmbiguityStrategy(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> closestToCameraHeightStrategy(cameraResult);
case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult);
case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult);
case CLOSEST_TO_REFERENCE_POSE ->
closestToReferencePoseStrategy(cameraResult, referencePose);
estimateClosestToReferencePose(cameraResult, referencePose);
case CLOSEST_TO_LAST_POSE -> {
setReferencePose(lastPose);
yield closestToReferencePoseStrategy(cameraResult, referencePose);
yield estimateClosestToReferencePose(cameraResult, referencePose);
}
case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult);
case MULTI_TAG_PNP_ON_RIO -> {
if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
yield update(cameraResult, this.multiTagFallbackStrategy);
}
var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get());
if (res.isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
constrainedPnpParams,
this.multiTagFallbackStrategy);
}
yield res;
}
case MULTI_TAG_PNP_ON_COPROCESSOR -> {
if (cameraResult.getMultiTagResult().isEmpty()) {
yield update(cameraResult, this.multiTagFallbackStrategy);
}
yield estimateCoprocMultiTagPose(cameraResult);
}
case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult);
case CONSTRAINED_SOLVEPNP -> {
boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
yield Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (cameraResult.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
cameraResult
.getMultiTagResult()
.get()
.estimatedPose
.best
.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
yield Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
}
var pnpResult =
estimateConstrainedSolvepnpPose(
cameraResult,
cameraMatrix.get(),
distCoeffs.get(),
fieldToRobotSeed,
constrainedPnpParams.get().headingFree,
constrainedPnpParams.get().headingScaleFactor);
if (!pnpResult.isPresent()) {
yield update(
cameraResult,
cameraMatrix,
distCoeffs,
Optional.empty(),
this.multiTagFallbackStrategy);
}
yield pnpResult;
}
case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
case MULTI_TAG_PNP_ON_RIO ->
multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
case CONSTRAINED_SOLVEPNP ->
constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
};
if (estimatedPose.isPresent()) {
@@ -515,12 +661,43 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.getBestTarget();
/**
* @param cameraResult A pipeline result from the camera.
* @return Whether or not pose estimation should be performed.
*/
private boolean shouldEstimate(PhotonPipelineResult cameraResult) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return false;
}
// If no targets seen, trivial case -- can't do estimation
return cameraResult.hasTargets();
}
/**
* Return the estimated position of the robot by using distance data from best visible tag to
* compute a Pose. This runs on the RoboRIO in order to access the robot's yaw heading, and MUST
* have addHeadingData called every frame so heading data is up-to-date.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget bestTarget = cameraResult.getBestTarget();
if (bestTarget == null) return Optional.empty();
var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds());
var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds());
if (headingSampleOpt.isEmpty()) {
return Optional.empty();
}
@@ -555,98 +732,82 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(robotPose),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
}
private Optional<EstimatedRobotPose> constrainedPnpStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
/**
* Return the estimated position of the robot by solving a constrained version of the
* Perspective-n-Point problem with the robot's drivebase flat on the floor. This computation
* takes place on the RoboRIO, and typically takes not more than 2ms. See {@link
* org.photonvision.jni.ConstrainedSolvepnpJni} for tuning handles this strategy exposes.
* Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
* heading error * heading scale factor. This strategy needs addHeadingData called every frame so
* heading data is up-to-date.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param seedPose An initial guess at robot pose, refined via optimization. Better guesses will
* converge faster. Can come from any pose source, but some battle-tested sources are {@link
* #estimateCoprocMultiTagPose(PhotonPipelineResult)}, or {@link
* #estimateLowestAmbiguityPose(PhotonPipelineResult)} if MultiTag results aren't available.
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error cont.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
PhotonPipelineResult cameraResult,
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
Pose3d seedPose,
boolean headingFree,
double headingScaleFactor) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (result.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
result.getMultiTagResult().get().estimatedPose.best.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
result,
cameraMatrixOpt,
distCoeffsOpt,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
return Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(headingBuffer.getSample(result.getTimestampSeconds()).get()));
}
var pnpResult =
VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
cameraMatrixOpt.get(),
distCoeffsOpt.get(),
result.getTargets(),
cameraMatrix,
distCoeffs,
cameraResult.getTargets(),
robotToCamera,
fieldToRobotSeed,
seedPose,
fieldTags,
tagModel,
constrainedPnpParams.get().headingFree,
headingBuffer.getSample(result.getTimestampSeconds()).get(),
constrainedPnpParams.get().headingScaleFactor);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
headingFree,
headingBuffer.getSample(cameraResult.getTimestampSeconds()).get(),
headingScaleFactor);
if (!pnpResult.isPresent()) return Optional.empty();
var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CONSTRAINED_SOLVEPNP));
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().isEmpty()) {
return update(result, this.multiTagFallbackStrategy);
/**
* Return the estimated position of the robot by using all visible tags to compute a single pose
* estimate on coprocessor. This option needs to be enabled on the PhotonVision web UI as well.
*
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (cameraResult.getMultiTagResult().isEmpty() || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best_tf = cameraResult.getMultiTagResult().get().estimatedPose.best;
var best =
Pose3d.kZero
.plus(best_tf) // field-to-camera
@@ -655,33 +816,31 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
DriverStation.reportWarning(
"No camera calibration data provided for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
return update(result, this.multiTagFallbackStrategy);
}
if (result.getTargets().size() < 2) {
return update(result, this.multiTagFallbackStrategy);
/**
* Return the estimated position of the robot by using all visible tags to compute a single pose
* estimate on the RoboRIO. This can take a lot of time due to the RIO's weak computing power.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
if (cameraResult.getTargets().size() < 2 || !shouldEstimate(cameraResult)) {
return Optional.empty();
}
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
cameraMatrix, distCoeffs, cameraResult.getTargets(), fieldTags, tagModel);
if (!pnpResult.isPresent()) return Optional.empty();
var best =
Pose3d.kZero
@@ -691,25 +850,29 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
* Return the estimated position of the robot with the lowest position ambiguity from a pipeline
* result.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
PhotonTrackedTarget lowestAmbiguityTarget = null;
double lowestAmbiguityScore = 10;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
double targetPoseAmbiguity = target.getPoseAmbiguity();
// Make sure the target is a Fiducial target.
if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
@@ -737,8 +900,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}
@@ -746,15 +909,19 @@ public class PhotonPoseEstimator {
* Return the estimated position of the robot using the target with the lowest delta height
* difference between the estimated and actual height of the camera.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
double smallestHeightDifference = 10e9;
EstimatedRobotPose closestHeightTarget = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -792,8 +959,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
@@ -805,8 +972,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}
@@ -819,13 +986,16 @@ public class PhotonPoseEstimator {
* Return the estimated position of the robot using the target with the lowest delta in the vector
* magnitude between it and the reference pose.
*
* @param result pipeline result
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference against.
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
PhotonPipelineResult result, Pose3d referencePose) {
public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
PhotonPipelineResult cameraResult, Pose3d referencePose) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
if (referencePose == null) {
DriverStation.reportError(
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
@@ -836,7 +1006,7 @@ public class PhotonPoseEstimator {
double smallestPoseDelta = 10e9;
EstimatedRobotPose lowestDeltaPose = null;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -870,8 +1040,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
@@ -879,8 +1049,8 @@ public class PhotonPoseEstimator {
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
@@ -890,15 +1060,19 @@ public class PhotonPoseEstimator {
/**
* Return the average of the best target poses using ambiguity as weight.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
* @param cameraResult A pipeline result from the camera.
* @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!shouldEstimate(cameraResult)) {
return Optional.empty();
}
List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (PhotonTrackedTarget target : result.targets) {
for (PhotonTrackedTarget target : cameraResult.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
@@ -923,8 +1097,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
@@ -958,8 +1132,8 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
cameraResult.getTimestampSeconds(),
cameraResult.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}

325
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -24,13 +24,9 @@
#include "photon/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <optional>
#include <span>
#include <string>
#include <utility>
#include <vector>
@@ -46,6 +42,7 @@
#include <units/angle.h>
#include <units/math.h>
#include <units/time.h>
#include <wpi/deprecated.h>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -55,7 +52,7 @@
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
#include <opencv2/core/eigen.hpp>
WPI_IGNORE_DEPRECATED
namespace photon {
namespace detail {
@@ -67,6 +64,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat,
frc::Transform3d robotToCamera)
@@ -138,39 +148,94 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
switch (strategy) {
case LOWEST_AMBIGUITY:
ret = LowestAmbiguityStrategy(result);
ret = EstimateLowestAmbiguityPose(result);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
ret = ClosestToCameraHeightStrategy(result);
ret = EstimateClosestToCameraHeightPose(result);
break;
case CLOSEST_TO_REFERENCE_POSE:
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
ret = ClosestToReferencePoseStrategy(result);
ret = EstimateClosestToReferencePose(result, this->referencePose);
break;
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
ret = EstimateAverageBestTargetsPose(result);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret = MultiTagOnCoprocStrategy(result);
if (!result.MultiTagResult()) {
ret = Update(result, this->multiTagFallbackStrategy);
} else {
ret = EstimateCoprocMultiTagPose(result);
}
break;
case MULTI_TAG_PNP_ON_RIO:
if (cameraMatrixData && cameraDistCoeffs) {
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
} else {
if (!cameraMatrixData && !cameraDistCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!",
"");
ret = Update(result, this->multiTagFallbackStrategy);
}
ret =
EstimateRioMultiTagPose(result, *cameraMatrixData, *cameraDistCoeffs);
if (!ret) {
ret = Update(result, this->multiTagFallbackStrategy);
}
break;
case CONSTRAINED_SOLVEPNP:
ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams);
case CONSTRAINED_SOLVEPNP: {
using namespace frc;
if (!cameraMatrixData || !cameraDistCoeffs) {
FRC_ReportError(
frc::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
break;
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
ret = EstimateConstrainedSolvepnpPose(
result, *cameraMatrixData, *cameraDistCoeffs, fieldToRobotSeed,
constrainedPnpParams->headingFree,
constrainedPnpParams->headingScalingFactor);
if (!ret) {
ret = Update(result, cameraMatrixData, cameraDistCoeffs, {},
this->multiTagFallbackStrategy);
}
break;
}
case PNP_DISTANCE_TRIG_SOLVE:
ret = PnpDistanceTrigSolveStrategy(result);
ret = EstimatePnpDistanceTrigSolvePose(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -184,10 +249,26 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return ret;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) {
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return false;
}
// If no targets seen, trivial case -- can't do estimation
return result.HasTargets();
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
auto foundIt = targets.end();
for (auto it = targets.begin(); it != targets.end(); ++it) {
if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
@@ -214,18 +295,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
return EstimatedRobotPose{
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToCameraHeightStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
for (auto& target : result.GetTargets()) {
for (auto& target : cameraResult.GetTargets()) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
@@ -250,14 +334,16 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -265,14 +351,17 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToReferencePoseStrategy(
PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -298,17 +387,17 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = altPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = bestPose;
stateTimestamp = result.GetTimestamp();
stateTimestamp = cameraResult.GetTimestamp();
}
}
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
return EstimatedRobotPose{pose, stateTimestamp, cameraResult.GetTargets(),
CLOSEST_TO_REFERENCE_POSE};
}
@@ -361,41 +450,31 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (!result.MultiTagResult()) {
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult) {
if (!cameraResult.MultiTagResult() || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"PhotonPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix cameraMatrix,
PhotonCamera::DistortionMatrix distCoeffs) {
// Need at least 2 targets
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
if (cameraResult.GetTargets().size() < 2 || !ShouldEstimate(cameraResult)) {
return std::nullopt;
}
auto const targets = result.GetTargets();
const auto targets = cameraResult.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
// (pixels).
@@ -418,7 +497,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
// We should only do multi-tag if at least 2 tags (* 4 corners/tag)
if (imagePoints.size() < 8) {
return Update(result, this->multiTagFallbackStrategy);
return std::nullopt;
}
// Output mats for results
@@ -426,27 +505,31 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
{
cv::Mat cameraMatCV(camMat->rows(), camMat->cols(), CV_64F);
cv::eigen2cv(*camMat, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs->rows(), distCoeffs->cols(), CV_64F);
cv::eigen2cv(*distCoeffs, distCoeffsMatCV);
cv::Mat cameraMatCV(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F);
cv::eigen2cv(cameraMatrix, cameraMatCV);
cv::Mat distCoeffsMatCV(distCoeffs.rows(), distCoeffs.cols(), CV_64F);
cv::eigen2cv(distCoeffs, distCoeffsMatCV);
cv::solvePnP(objectPoints, imagePoints, cameraMatCV, distCoeffsMatCV, rvec,
tvec, false, cv::SOLVEPNP_SQPNP);
}
const Pose3d pose = detail::ToPose3d(tvec, rvec);
const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(),
MULTI_TAG_PNP_ON_RIO);
return photon::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
std::optional<frc::Rotation2d> headingSampleOpt =
headingBuffer.Sample(result.GetTimestamp());
headingBuffer.Sample(cameraResult.GetTimestamp());
if (!headingSampleOpt) {
FRC_ReportError(frc::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
@@ -485,17 +568,21 @@ PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
frc::Pose2d robotPose = frc::Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample);
return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
PhotonPoseEstimator::EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
auto targets = result.GetTargets();
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
@@ -512,13 +599,15 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(), cameraResult.GetTargets(),
AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
std::make_pair(target.GetPoseAmbiguity(),
cameraResult.GetTimestamp())));
}
frc::Translation3d transform = frc::Translation3d();
@@ -532,77 +621,45 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
cameraResult.GetTimestamp(),
cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"StrPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
if (!headingFree) {
seedPose = frc::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
if (!constrainedPnpParams.value().headingFree) {
fieldToRobotSeed = frc::Pose3d{
fieldToRobotSeed.Translation(),
frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
}
std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
result.GetTargets().end()};
std::vector<photon::PhotonTrackedTarget> targets{
cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};
std::optional<photon::PnpResult> pnpResult =
VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
constrainedPnpParams->headingFree,
frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
constrainedPnpParams->headingScalingFactor);
cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
aprilTags, photon::kAprilTag36h11, headingFree,
frc::Rotation2d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
headingScaleFactor);
if (!pnpResult) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
return std::nullopt;
}
frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
PoseStrategy::CONSTRAINED_SOLVEPNP};
}
} // namespace photon

252
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -24,8 +24,6 @@
#pragma once
#include <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
@@ -34,11 +32,8 @@
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
enum PoseStrategy {
@@ -93,10 +88,26 @@ class PhotonPoseEstimator {
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
* @deprecated Use individual estimation methods with the 2 argument
* constructor instead.
*/
[[deprecated(
"Use individual estimation methods with the 2 argument constructor "
"instead.")]]
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
@@ -112,14 +123,20 @@ class PhotonPoseEstimator {
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
* @deprecated Use individual estimation methods instead.
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
[[deprecated("Use individual estimation methods instead.")]]
PoseStrategy GetPoseStrategy() const {
return strategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
@@ -132,22 +149,30 @@ class PhotonPoseEstimator {
* only one tag can be seen. Must NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
* @deprecated Use individual estimation methods instead.
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
[[deprecated("Use individual estimation methods instead.")]]
frc::Pose3d GetReferencePose() const {
return referencePose;
}
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetReferencePose(frc::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
@@ -178,8 +203,12 @@ class PhotonPoseEstimator {
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
* @deprecated Use individual estimation methods instead.
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(frc::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
* Add robot heading data to the buffer. Must be called periodically for the
@@ -247,7 +276,9 @@ class PhotonPoseEstimator {
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
std::optional<EstimatedRobotPose> Update(
const photon::PhotonPipelineResult& result,
std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
@@ -257,6 +288,127 @@ class PhotonPoseEstimator {
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
std::nullopt);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param cameraResult A pipeline result from the camera.
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
/**
* Return the estimated position of the robot by using all visible tags to
* compute a single pose estimate on coprocessor. This option needs to be
* enabled on the PhotonVision web UI as well.
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot by using all visible tags to
* compute a single pose estimate on the RoboRIO. This can take a lot of time
* due to the RIO's weak computing power.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
PhotonCamera::DistortionMatrix distCoeffs);
/**
* Return the estimated position of the robot by using distance data from best
* visible tag to compute a Pose. This runs on the RoboRIO in order to access
* the robot's yaw heading, and MUST have AddHeadingData called every frame so
* heading data is up-to-date.
*
* <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
PhotonPipelineResult cameraResult);
/**
* Return the average of the best target poses using ambiguity as weight.
* @param cameraResult A pipeline result from the camera.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
PhotonPipelineResult cameraResult);
/**
* Return the estimated position of the robot by solving a constrained version
* of the Perspective-n-Point problem with the robot's drivebase flat on the
* floor. This computation takes place on the RoboRIO, and typically takes not
* more than 2ms. See
* photon::VisionEstimation::EstimateRobotPoseConstrainedSolvePNP for tuning
* handles this strategy exposes. Internally, the cost function is a
* sum-squared of pixel reprojection error + (optionally) heading error *
* heading scale factor. This strategy needs addHeadingData called every frame
* so heading data is up-to-date.
*
* @param cameraResult A pipeline result from the camera.
* @param cameraMatrix Camera intrinsics from camera calibration data.
* @param distCoeffs Distortion coefficients from camera calibration data.
* @param seedPose An initial guess at robot pose, refined via optimization.
* Better guesses will converge faster. Can come from any pose source, but
* some battle-tested sources are estimateCoprocMultiTagPose, or
* estimateLowestAmbiguityPose if MultiTag results aren't available.
* @param headingFree If true, heading is completely free to vary. If false,
* heading excursions from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of
* changing our robot heading estimate against the tag corner reprojection
* error cost.
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor);
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
@@ -280,9 +432,9 @@ class PhotonPoseEstimator {
* This should only be called after timestamp checks have been done by another
* update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param cameraResult A pipeline result from the camera.
* @param strategy The pose strategy to use
* @return an EstimatedRobotPose with an estimated pose, timestamp, and
* @return An EstimatedRobotPose with an estimated pose, timestamp, and
* targets used to create the estimate.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
@@ -296,84 +448,6 @@ class PhotonPoseEstimator {
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
/**
* Return the pose calculation using the best visible tag and the robot's
* heading
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation
*/
std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
PhotonPipelineResult result);
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
};
} // namespace photon

936
photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java Normal file
View File

@@ -0,0 +1,936 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
class LegacyPhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@BeforeAll
public static void init() throws IOException {
if (!LibraryLoader.loadWpiLibraries()) {
fail();
}
RuntimeLoader.loadLibrary("photontargetingJNI");
HAL.initialize(1000, 0);
List<AprilTag> tagList = new ArrayList<>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
double fieldLength = Units.feetToMeters(54.0);
double fieldWidth = Units.feetToMeters(27.0);
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
void testLowestAmbiguityStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
}
@Test
void testClosestToCameraHeightStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
4000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
}
@Test
void closestToReferencePoseStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
17000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
}
@Test
void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
new PhotonPipelineResult(
0,
7000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
1,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
0,
-1,
-1,
new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
}
@Test
void pnpDistanceTrigSolve() {
List<VisionTargetSim> simTargets =
aprilTags.getTags().stream()
.map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
.toList();
try (PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
/* Compound Rolled + Pitched + Yaw */
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37),
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
PhotonPipelineResult result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
var bestTarget = result.getBestTarget();
assertNotNull(bestTarget);
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
/* Straight on */
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
estimator.setRobotToCameraTransform(straightOnTestTransform);
/* Pose to compare with */
realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
new PhotonPipelineResult(
0,
20 * 1000000,
1100000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))), // 1 1 1 ambig: .7
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))), // 2 2 2 ambig .3
new PhotonTrackedTarget(
9.0,
-2.0,
19.0,
3.0,
0,
-1,
-1,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
0.4,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
}
@Test
public void testConstrainedPnpOneTag() {
var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
var cameraMat =
MatBuilder.fill(
Nat.N3(),
Nat.N3(),
399.37500000000006,
0,
319.5,
0,
399.16666666666674,
239.5,
0,
0,
1);
/*
* Ground truth:
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
* 31064262190452635
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
* 24478552235412665
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
* 0836470779150917
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
* 914649865171567
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
* 191446451763934
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
* 44396966389316
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
* 4995793771070878
*/
List<TargetCorner> corners8 =
List.of(
new TargetCorner(98.09875447066685, 331.0093220119495),
new TargetCorner(122.20226758624413, 335.50083894738486),
new TargetCorner(127.17118732489361, 313.81406314178633),
new TargetCorner(104.28543773760417, 309.6516557438994));
var result =
new PhotonPipelineResult(
new PhotonPipelineMetadata(10000, 2000, 1, 100),
List.of(
new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
Optional.of(
new MultiTargetPNPResult(
new PnpResult(
new Transform3d(
// From ground truth
new Translation3d(
3.1665557336121353, 4.430673446050584, 0.48678786477534686),
new Rotation3d(
new Quaternion(
0.3132532247418243,
0.24722671090692333,
-0.08413452932300695,
0.9130568172784148))),
0.1),
new ArrayList<Short>(8))));
final double camPitch = Units.degreesToRadians(30.0);
final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");
}
PhotonPipelineResult result;
@Override
public List<PhotonPipelineResult> getAllUnreadResults() {
return List.of(result);
}
@Override
public PhotonPipelineResult getLatestResult() {
return result;
}
}
}

183
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -24,11 +24,8 @@
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
@@ -39,13 +36,11 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
@@ -57,8 +52,6 @@ import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
@@ -170,10 +163,10 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateLowestAmbiguityPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -257,11 +250,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToCameraHeightPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -345,12 +337,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -434,13 +425,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
@@ -514,7 +503,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
estimatedPose = estimator.update(cameraOne.result);
estimatedPose = estimator.estimateClosestToReferencePose(cameraOne.result, pose);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -542,9 +531,7 @@ class PhotonPoseEstimatorTest {
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
var estimator = new PhotonPoseEstimator(aprilTags, compoundTestTransform);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
@@ -556,7 +543,7 @@ class PhotonPoseEstimatorTest {
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -575,7 +562,7 @@ class PhotonPoseEstimatorTest {
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -584,82 +571,6 @@ class PhotonPoseEstimatorTest {
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
@@ -735,11 +646,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateAverageBestTargetsPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -749,8 +659,9 @@ class PhotonPoseEstimatorTest {
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.result =
void testMultiTagOnCoprocFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
@@ -799,18 +710,21 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, Transform3d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateCoprocMultiTagPose(camera.result);
assertTrue(estimatedPose.isEmpty());
estimatedPose = estimator.estimateLowestAmbiguityPose(camera.result);
assertTrue(estimatedPose.isPresent());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), 1e-9);
assertEquals(3, pose.getY(), 1e-9);
assertEquals(2, pose.getZ(), 1e-9);
}
@Test
@@ -888,34 +802,17 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam);
var multiTagEstimate = estimator.estimateCoprocMultiTagPose(result);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
estimator.estimateConstrainedSolvepnpPose(
result, cameraMat, distortion, multiTagEstimate.get().estimatedPose, true, 0);
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");

661
photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp Normal file
View File

@@ -0,0 +1,661 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <map>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include <wpi/deprecated.h>
#include "gtest/gtest.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/simulation/PhotonCameraSim.h"
#include "photon/simulation/SimCameraProperties.h"
#include "photon/simulation/VisionTargetSim.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
WPI_IGNORE_DEPRECATED
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
cameraOne.test = true;
std::vector<photon::VisionTargetSim> targets;
targets.reserve(tags.size());
for (const auto& tag : tags) {
targets.push_back(
photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
}
photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
frc::Transform3d straightOnTestTransform =
frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(18_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
// empty input, expect empty out
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(LegacyPhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
auto test2 = testResult;
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
{0, 399.16666666666674, 239.5},
{0, 0, 1}};
// Create corners data matching the Java test
std::vector<photon::TargetCorner> corners8{
photon::TargetCorner{98.09875447066685, 331.0093220119495},
photon::TargetCorner{122.20226758624413, 335.50083894738486},
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
frc::Transform3d poseTransform(
frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
poseTransform, 0.0, corners8, corners8}};
auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
std::vector<int16_t>{8});
photon::PhotonPipelineResult result{
photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
multiTagResult};
cameraOne.test = true;
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
frc::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
ASSERT_TRUE(estimatedPose.has_value());
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
}

169
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -93,12 +93,11 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -154,12 +153,11 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToCameraHeightPose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -203,14 +201,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
@@ -254,14 +251,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
@@ -295,9 +291,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateClosestToReferencePose(result, pose);
}
ASSERT_TRUE(estimatedPose);
@@ -327,8 +322,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
@@ -344,7 +338,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -374,7 +368,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimatePnpDistanceTrigSolvePose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -419,12 +413,11 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateAverageBestTargetsPose(result);
}
ASSERT_TRUE(estimatedPose);
@@ -437,101 +430,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
// empty input, expect empty out
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> targets{
@@ -555,12 +454,15 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
estimatedPose = estimator.EstimateCoprocMultiTagPose(result);
}
ASSERT_FALSE(estimatedPose);
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -586,16 +488,6 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
@@ -639,14 +531,17 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
kRobotToCam);
auto estimatedMultiTagPose =
estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
auto estimatedPose = estimator.EstimateConstrainedSolvepnpPose(
cameraOne.testResult[0], cameraMat, distortion,
estimatedMultiTagPose->estimatedPose, true, 0);
ASSERT_TRUE(estimatedPose.has_value());