vision V1 #7

New issue

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

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

Already on GitHub? Sign in to your account

Closed

pix-xl wants to merge 12 commits into CrevolutionRoboticsProgramming:main from pix-xl:main

src/main/java/frc/robot/vision/Vision.java

-Original file line number
+Diff line change
@@ -0,0 +1,124 @@
+    package frc.robot.vision;
+    import static frc.robot.vision.VisionConfig.multiTagStdDevs;
+    import static frc.robot.vision.VisionConfig.singleTagStdDevs;
+    import static frc.robot.vision.VisionConfig.tagLayout;
+    import java.util.List;
+    import java.util.Optional;
+    import org.photonvision.EstimatedRobotPose;
+    import org.photonvision.PhotonCamera;
+    import org.photonvision.PhotonPoseEstimator;
+    import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+    import org.photonvision.targeting.PhotonTrackedTarget;
+    import edu.wpi.first.math.Matrix;
+    import edu.wpi.first.math.VecBuilder;
+    import edu.wpi.first.math.geometry.Transform3d;
+    import edu.wpi.first.math.numbers.N1;
+    import edu.wpi.first.math.numbers.N3;
+    /*
+     * This code is a modified version of Photonvision's own example and 7028's vision code:
+     * https://github.com/PhotonVision/photonvision/blob/main/photonlib-java-examples/poseest
+     * https://github.com/STMARobotics/frc-7028-2025
+     */
+    public class Vision {
+        private final PhotonCamera cam;
+        private final PhotonPoseEstimator photonEstimator;
+        private Matrix<N3, N1> curStdDevs;
+        //Construct Vision Instance
+        public Vision(String cameraName, Transform3d camTransform){
+            cam = new PhotonCamera(cameraName);
+            photonEstimator = new PhotonPoseEstimator(
+                tagLayout,
+                PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                camTransform);
+            photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        }
+        /**
+        * The latest estimated robot pose on the field from vision data. This may be empty. This should
+        * only be called once per loop.
+        *
+        * <p>
+        * Also includes updates for the standard deviations, which can (optionally) be retrieved with
+        * {@link getEstimationStdDevs}
+        *
+        * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+        *         used for estimation.
+        */
+        public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+            Optional<EstimatedRobotPose> visionEst = Optional.empty();
+            for (var change : cam.getAllUnreadResults()) {
+              visionEst = photonEstimator.update(change);
+              updateEstimationStdDevs(visionEst, change.getTargets());
+            }
+            return visionEst;
+        }
+        /**
+        * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+        * deviations based on number of tags, estimation strategy, and distance from the tags.
+        *
+        * @param estimatedPose The estimated pose to guess standard deviations for.
+        * @param targets All targets in this camera frame
+        */
+        private void updateEstimationStdDevs(Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets){
+            if (estimatedPose.isEmpty()){
+                //No pose input. Default to single-tag std devs
+                curStdDevs = singleTagStdDevs;
+            } else {
+                //Pose present, start Heuristic
+                var estStdDevs = singleTagStdDevs;
+                int numTags = 0;
+                double avgDist = 0;
+                //Precalculation - see how many tags we found, and calculate an average-distance metric
+                for (var tgt : targets) {
+                    var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+                    if (tagPose.isEmpty()){
+                        continue;
+                    }
+                    numTags++;
+                    avgDist += tagPose.get()
+                        .toPose2d()
+                        .getTranslation()
+                        .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+                }
+                if (numTags == 0){
+                    //No tags visible. Default to single-tag std devs
+                    curStdDevs = singleTagStdDevs;
+                } else {
+                    // One or more tags visible, run the full heuristic.
+                    avgDist /= numTags;
+                    // Decrease std devs if multiple targets are visible
+                    if (numTags > 1)
+                      estStdDevs = multiTagStdDevs;
+                    // Increase std devs based on (average) distance
+                    if (numTags == 1 && avgDist > 4) {
+                      estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+                    } else {
+                      estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+                    }
+                    curStdDevs = estStdDevs;
+                }
+            }
+        }
+        /**
+        * Returns the latest standard deviations of the estimated pose from {@link
+        * #getEstimatedGlobalPose()}, for use with {@link
+        * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
+        * only be used when there are targets visible.
+        */
+        public Matrix<N3, N1> getEstimationStdDevs() {
+            return curStdDevs;
+        }
+    }

src/main/java/frc/robot/vision/VisionConfig.java

-Original file line number
+Diff line change
@@ Expand Up / @@ -2,55 +2,38 @@ @@
     import org.photonvision.PhotonCamera;
+    import edu.wpi.first.apriltag.AprilTagFieldLayout;
     import edu.wpi.first.apriltag.AprilTagFields;
+    import edu.wpi.first.math.Matrix;
+    import edu.wpi.first.math.VecBuilder;
     import edu.wpi.first.math.geometry.Rotation3d;
     import edu.wpi.first.math.geometry.Transform3d;
     import edu.wpi.first.math.geometry.Translation3d;
+    import edu.wpi.first.math.numbers.N1;
+    import edu.wpi.first.math.numbers.N3;
     import edu.wpi.first.math.util.Units;
     public class VisionConfig {
-        //Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
-        public static final String centerPoseCamName = "Center_Pose_Cam";
-        public static final String leftPoseCamName = "Left_Pose_Cam";
-        public static final String rightPoseCamName = "Right_Pose_Cam";
-        public static final String driveCamName = "Drive_Cam";
-        //Creates all Photoncameras that will be used in pose estimator and commands
-        public static PhotonCamera centerCam = new PhotonCamera(centerPoseCamName);
-        public static PhotonCamera leftCam = new PhotonCamera(leftPoseCamName);
-        public static PhotonCamera rightCam = new PhotonCamera(rightPoseCamName);
-        public static PhotonCamera driveCam = new PhotonCamera(driveCamName);
-        //Creates field layout for AprilTags
-        public static AprilTagFields aprilTagField = AprilTagFields.k2025Reefscape;
-        //TODO: config
-        public static final Transform3d robotToCenterCam =
-            new Transform3d(
-                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
-                new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
-            );
-        public static final Transform3d robotToLeftCam =
-            new Transform3d(
-                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
-                new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
-            );
-        public static final Transform3d robotToRightCam =
-            new Transform3d(
-                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
-                new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
-            );
-        public static final Transform3d robotToDriveCam =
-            new Transform3d(
-                new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
-                new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
-            );
+        // Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
+        public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"};
+        //Camera Positions
+        // TODO: config camera transforms
+        public static final Transform3d[] robotToCamTransforms = new Transform3d[] {
+            new Transform3d(new Translation3d(), new Rotation3d()),
+            new Transform3d(new Translation3d(), new Rotation3d()),
+            new Transform3d(new Translation3d(), new Rotation3d()),
+            new Transform3d(new Translation3d(), new Rotation3d())
+        };
+        // Creates field layout for AprilTags
+        public static final AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
+        // Standard deviation of vision poses, this helps with correction or something idk thats what photon said
+        // TODO: experiment with standard deviation values and set them to whatever gives the most correct pose
+        public static final Matrix<N3, N1> singleTagStdDevs = VecBuilder.fill(4, 4, 8); // TODO: example values, change when testing
+        public static final Matrix<N3, N1> multiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); //TODO: change values when testing
-    }
+    }

src/main/java/frc/robot/vision/VisionConsumer.java

-Original file line number
+Diff line change
@@ -0,0 +1,25 @@
+    package frc.robot.vision;
+    import edu.wpi.first.math.Matrix;
+    import edu.wpi.first.math.geometry.Pose2d;
+    import edu.wpi.first.math.numbers.N1;
+    import edu.wpi.first.math.numbers.N3;
+    @FunctionalInterface
+    public interface VisionConsumer {
+        /**
+        * Adds a vision measurement.
+        * <p>
+        * Note that the vision measurement standard deviations passed into this method
+        * will continue to apply to future measurements.
+        *
+        * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+        * @param timestampSeconds The timestamp of the vision measurement in seconds.
+        * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, with
+        *          units in meters and radians.
+        */
+        public void addVisionMeasurement(
+            Pose2d visionRobotPoseMeters,
+            double timestampSeconds,
+            Matrix<N3, N1> visionMeasurementStdDevs);
+    }

src/main/java/frc/robot/vision/VisionPoseEstimator.java

This file was deleted.

src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java

-Original file line number
+Diff line change
@@ -0,0 +1,42 @@
+    package frc.robot.vision.commands;
+    import static frc.robot.vision.VisionConfig.camNames;
+    import static frc.robot.vision.VisionConfig.robotToCamTransforms;
+    import java.util.Arrays;
+    import edu.wpi.first.wpilibj2.command.Command;
+    import frc.robot.vision.Vision;
+    import frc.robot.vision.VisionConfig.*;
+    import frc.robot.vision.VisionConsumer;
+    public class PhotonEstimatorCommand extends Command{
+        private final Vision[] visions;
+        private final VisionConsumer visionConsumer;
+        public PhotonEstimatorCommand(VisionConsumer consumer){
+            visions = new Vision[camNames.length];
+            for (int i = 0; i < camNames.length; i++){
+                visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
+            }
+            this.visionConsumer = consumer;
+        }
+        @Override
+        public void execute(){
+            Arrays.stream(visions).forEach(vision -> {
+                var visionEst = vision.getEstimatedGlobalPose();
+                visionEst.ifPresent(est -> {
+                    var estStdDevs = vision.getEstimationStdDevs();
+                    visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+                });
+            });
+        }
+        @Override
+        public boolean runsWhenDisabled(){
+            return true;
+        }
+    }

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

vision V1 #7

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!