Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +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

15
photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java
View File

@@ -38,7 +38,6 @@ import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
@@ -61,10 +60,7 @@ public class Vision {
public Vision(EstimateConsumer estConsumer) {
this.estConsumer = estConsumer;
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
photonEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam);
// ----- Simulation
if (Robot.isSimulation()) {
@@ -91,9 +87,12 @@ public class Vision {
public void periodic() {
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : camera.getAllUnreadResults()) {
visionEst = photonEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
for (var result : camera.getAllUnreadResults()) {
visionEst = photonEstimator.estimateCoprocMultiTagPose(result);
if (visionEst.isEmpty()) {
visionEst = photonEstimator.estimateLowestAmbiguityPose(result);
}
updateEstimationStdDevs(visionEst, result.getTargets());
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(