Skip to content

Commit

Permalink
Add C++ photonlib support
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed Sep 30, 2023
1 parent 50d4267 commit f65f1c9
Show file tree
Hide file tree
Showing 13 changed files with 303 additions and 1,364 deletions.
10 changes: 9 additions & 1 deletion photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;

/** An estimated pose based on pipeline result */
Expand All @@ -39,16 +40,23 @@ public class EstimatedRobotPose {
/** A list of the targets used to compute this pose */
public final List<PhotonTrackedTarget> targetsUsed;

/** 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) {
Pose3d estimatedPose,
double timestampSeconds,
List<PhotonTrackedTarget> targetsUsed,
PoseStrategy strategy) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = strategy;
}
}
71 changes: 58 additions & 13 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,10 @@ public enum PoseStrategy {
AVERAGE_BEST_TARGETS,

/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP
MULTI_TAG_PNP_ON_COPROCESSOR,

/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP_ON_RIO
}

private AprilTagFieldLayout fieldTags;
Expand Down Expand Up @@ -173,7 +176,8 @@ public void setPrimaryStrategy(PoseStrategy strategy) {
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
|| strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
Expand Down Expand Up @@ -357,8 +361,11 @@ private Optional<EstimatedRobotPose> update(
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
default:
DriverStation.reportError(
Expand All @@ -373,7 +380,28 @@ private Optional<EstimatedRobotPose> update(
return estimatedPose;
}

private Optional<EstimatedRobotPose> multiTagPNPStrategy(
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
}

private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
Expand All @@ -395,7 +423,11 @@ private Optional<EstimatedRobotPose> multiTagPNPStrategy(
.plus(robotToCamera.inverse()); // field-to-robot

return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}

/**
Expand Down Expand Up @@ -440,7 +472,8 @@ private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResul
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}

/**
Expand Down Expand Up @@ -494,7 +527,8 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}

if (bestTransformDelta < smallestHeightDifference) {
Expand All @@ -506,7 +540,8 @@ private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelin
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}

Expand Down Expand Up @@ -568,13 +603,19 @@ private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition, result.getTimestampSeconds(), result.getTargets());
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
return Optional.ofNullable(lowestDeltaPose);
Expand Down Expand Up @@ -617,7 +658,8 @@ private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineRe
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}

totalAmbiguity += 1.0 / target.getPoseAmbiguity();
Expand Down Expand Up @@ -649,7 +691,10 @@ private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineRe

return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}

/**
Expand Down
Loading

0 comments on commit f65f1c9

Please sign in to comment.