Vision Logic

build.gradle

@@ -88,7 +88,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     implementation 'com.google.code.gson:gson:2.10.1'
-    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.4'
+    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.9'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"

simgui-window.json

@@ -5,7 +5,7 @@
   "MainWindow": {
     "GLOBAL": {
       "fps": "120",
-      "height": "1001",
+      "height": "974",
       "maximized": "1",
       "style": "0",
       "userScale": "2",
@@ -71,22 +71,22 @@
     "###/SmartDashboard/AutoChooser": {
       "Collapsed": "0",
       "Pos": "-3,891",
-      "Size": "307,60"
+      "Size": "361,66"
     },
     "###/SmartDashboard/Field": {
       "Collapsed": "0",
-      "Pos": "1278,491",
-      "Size": "633,365"
+      "Pos": "350,392",
+      "Size": "1534,512"
     },
     "###FMS": {
       "Collapsed": "0",
       "Pos": "-1,712",
-      "Size": "202,214"
+      "Size": "388,176"
     },
     "###Joysticks": {
       "Collapsed": "0",
       "Pos": "320,888",
-      "Size": "976,278"
+      "Size": "1156,91"
     },
     "###NetworkTables": {
       "Collapsed": "0",
@@ -116,12 +116,12 @@
     "###System Joysticks": {
       "Collapsed": "0",
       "Pos": "1,346",
-      "Size": "232,254"
+      "Size": "273,290"
     },
     "###Timing": {
       "Collapsed": "0",
       "Pos": "0,172",
-      "Size": "169,168"
+      "Size": "197,186"
     },
     "Debug##Default": {
       "Collapsed": "0",
@@ -131,7 +131,7 @@
     "Robot State": {
       "Collapsed": "0",
       "Pos": "2,25",
-      "Size": "109,134"
+      "Size": "126,152"
     }
   }
 }

src/main/java/frc/trigon/robot/constants/CameraConstants.java

@@ -5,8 +5,9 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera;
-import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource;
-import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants;
+import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
 
 public class CameraConstants {
     public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera");
@@ -24,28 +25,34 @@ public class CameraConstants {
                     new Rotation3d(0, Units.degreesToRadians(-31.7), 0)
             ),
             REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d(
-                    new Translation3d(0, 0, 0.632),
-                    new Rotation3d(0, Units.degreesToRadians(-24), Units.degreesToRadians(180))
+                    new Translation3d(0, 0, 0.62),
+                    new Rotation3d(0, Units.degreesToRadians(-23.5), Units.degreesToRadians(180))
             );
-    public static final RobotPoseSource
-            REAR_LEFT_CAMERA = new RobotPoseSource(
-            RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA,
+    public static final AprilTagCamera
+            REAR_LEFT_CAMERA = new AprilTagCamera(
+            AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA,
             "Rear Left Camera",
-            REAR_LEFT_CENTER_TO_CAMERA
+            REAR_LEFT_CENTER_TO_CAMERA,
+            PoseEstimatorConstants.THETA_STD_EXPONENT,
+            PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT
     ),
-            REAR_RIGHT_CAMERA = new RobotPoseSource(
-                    RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA,
+            REAR_RIGHT_CAMERA = new AprilTagCamera(
+                    AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA,
                     "Rear Right Camera",
-                    REAR_RIGHT_CENTER_TO_CAMERA
+                    REAR_RIGHT_CENTER_TO_CAMERA,
+                    PoseEstimatorConstants.THETA_STD_EXPONENT,
+                    PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT
             ),
     //            FRONT_MIDDLE_CAMERA = new RobotPoseSource(
 //                    RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA,
 //                    "Front Middle Camera",
 //                    FRONT_MIDDLE_CENTER_TO_CAMERA
 //            ),
-    REAR_MIDDLE_CAMERA = new RobotPoseSource(
-            RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA,
+    REAR_MIDDLE_CAMERA = new AprilTagCamera(
+            AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA,
             "Rear Middle Camera",
-            REAR_MIDDLE_CENTER_TO_CAMERA
+            REAR_MIDDLE_CENTER_TO_CAMERA,
+            PoseEstimatorConstants.THETA_STD_EXPONENT / 10,
+            PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT
     );
 }

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java

@@ -0,0 +1,124 @@
+package frc.trigon.robot.poseestimation.apriltagcamera;
+
+import edu.wpi.first.math.Matrix;
+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.Transform3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import frc.trigon.robot.Robot;
+import frc.trigon.robot.constants.FieldConstants;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags.
+ * An april tag is like a 2D barcode used to find the robot's position on the field.
+ * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field.
+ */
+public class AprilTagCamera {
+    protected final String name;
+    private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged();
+    private final Transform3d robotCenterToCamera;
+    private final AprilTagCameraIO aprilTagCameraIO;
+    private final double
+            thetaStandardDeviationsExponent,
+            translationStandardDeviationsExponent;
+    private double lastUpdatedTimestamp;
+    private Pose2d robotPose = null;
+
+    /**
+     * Constructs a new AprilTagCamera.
+     *
+     * @param robotPoseSourceType                                      the type of camera
+     * @param name                                                     the camera's name
+     * @param robotCenterToCamera                                      the transform of the robot's origin point to the camera
+     * @param thetaStandardDeviationsExponent                  the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP
+     * @param translationStandardDeviationsExponent            the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP
+     */
+    public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationsExponent, double translationStandardDeviationsExponent) {
+        this.name = name;
+        this.robotCenterToCamera = robotCenterToCamera;
+        this.thetaStandardDeviationsExponent = thetaStandardDeviationsExponent;
+        this.translationStandardDeviationsExponent = translationStandardDeviationsExponent;
+
+        if (Robot.IS_REAL)
+            aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name);
+        else
+            aprilTagCameraIO = new AprilTagCameraIO();
+    }
+
+    public void update() {
+        aprilTagCameraIO.updateInputs(inputs);
+        Logger.processInputs("Cameras/" + name, inputs);
+        robotPose = inputs.cameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()).toPose2d();
+
+        logEstimatedRobotPose();
+        if (!FieldConstants.TAG_ID_TO_POSE.isEmpty())
+            logUsedTags();
+    }
+
+    public boolean hasNewResult() {
+        return (inputs.hasResult && inputs.averageDistanceFromAllTags != 0) && isNewTimestamp();
+    }
+
+    public Pose2d getEstimatedRobotPose() {
+        return robotPose;
+    }
+
+    public String getName() {
+        return name;
+    }
+
+    public double getLatestResultTimestampSeconds() {
+        return inputs.latestResultTimestampSeconds;
+    }
+
+    private boolean isNewTimestamp() {
+        if (lastUpdatedTimestamp == getLatestResultTimestampSeconds())
+            return false;
+
+        lastUpdatedTimestamp = getLatestResultTimestampSeconds();
+        return true;
+    }
+
+    public Matrix<N3, N1> getStandardDeviations() {
+        final int numberOfVisibleTags = inputs.visibleTagIDs.length;
+        final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags);
+        final double thetaStandardDeviation = calculateStandardDeviations(thetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags);
+
+        return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation);
+    }
+
+    /**
+     * Calculates the standard deviation of the estimated pose using a formula.
+     * As we get further from the tag(s), this will return a less trusting (higher deviation) result.
+     *
+     * @param exponent            a calibrated gain, different for each pose estimating strategy
+     * @param distance            the distance from the tag(s)
+     * @param numberOfVisibleTags the number of visible tags
+     * @return the standard deviation
+     */
+    private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) {
+        return exponent * (distance * distance) / numberOfVisibleTags;
+    }
+
+    private void logEstimatedRobotPose() {
+        if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null)
+            Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST);
+        else
+            Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose);
+    }
+
+    private void logUsedTags() {
+        if (!inputs.hasResult) {
+            Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]);
+            return;
+        }
+
+        final Pose3d[] usedTagPoses = new Pose3d[inputs.visibleTagIDs.length];
+        for (int i = 0; i < usedTagPoses.length; i++)
+            usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]);
+        Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses);
+    }
+}

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java

@@ -0,0 +1,27 @@
+package frc.trigon.robot.poseestimation.apriltagcamera;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO;
+import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO;
+
+import java.util.function.Function;
+
+public class AprilTagCameraConstants {
+    public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, -0.06, new Rotation3d(Units.degreesToRadians(-1.8), 0, 0));
+    static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0];
+    public static final double MAXIMUM_AMBIGUITY = 0.2;
+
+    public enum RobotPoseSourceType {
+        PHOTON_CAMERA(AprilTagPhotonCameraIO::new),
+        LIMELIGHT(AprilTagLimelightIO::new);
+
+        final Function<String, AprilTagCameraIO> createIOFunction;
+
+        RobotPoseSourceType(Function<String, AprilTagCameraIO> createIOFunction) {
+            this.createIOFunction = createIOFunction;
+        }
+    }
+}

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java

@@ -0,0 +1,19 @@
+package frc.trigon.robot.poseestimation.apriltagcamera;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import org.littletonrobotics.junction.AutoLog;
+
+public class AprilTagCameraIO {
+    protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) {
+    }
+
+    @AutoLog
+    public static class RobotPoseSourceInputs {
+        public boolean hasResult = false;
+        public double latestResultTimestampSeconds = 0;
+        public Pose3d cameraSolvePNPPose = new Pose3d();
+        public int[] visibleTagIDs = new int[0];
+        public double averageDistanceFromAllTags = 0;
+        public double distanceFromBestTag = 0;
+    }
+}

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java

@@ -0,0 +1,81 @@
+package frc.trigon.robot.poseestimation.apriltagcamera.io;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import frc.trigon.robot.constants.FieldConstants;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO;
+import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged;
+import org.trigon.utilities.LimelightHelpers;
+
+public class AprilTagLimelightIO extends AprilTagCameraIO {
+    private final String hostname;
+
+    public AprilTagLimelightIO(String hostname) {
+        this.hostname = hostname;
+    }
+
+    @Override
+    protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) {
+        final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname);
+
+        inputs.hasResult = results.targets_Fiducials.length > 0;
+
+        if (inputs.hasResult)
+            updateHasResultInputs(inputs, results);
+        else
+            updateNoResultInputs(inputs);
+    }
+
+    private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) {
+        inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue();
+        inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture;
+        inputs.visibleTagIDs = getVisibleTagIDs(results);
+        inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results);
+        inputs.distanceFromBestTag = getDistanceFromBestTag(results);
+    }
+
+    private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) {
+        inputs.visibleTagIDs = new int[0];
+        inputs.cameraSolvePNPPose = new Pose3d();
+    }
+
+    private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) {
+        final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials;
+        final int[] visibleTagIDs = new int[visibleTags.length];
+        visibleTagIDs[0] = (int) getBestTarget(results).fiducialID;
+        int idAddition = 1;
+
+        for (int i = 0; i < visibleTagIDs.length; i++) {
+            final int currentID = (int) visibleTags[i].fiducialID;
+
+            if (currentID == visibleTagIDs[0]) {
+                idAddition = 0;
+                continue;
+            }
+
+            visibleTagIDs[i + idAddition] = currentID;
+        }
+        return visibleTagIDs;
+    }
+
+    private double getAverageDistanceFromAllTags(LimelightHelpers.LimelightResults results) {
+        final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials;
+        double totalDistanceFromTags = 0;
+
+        for (LimelightHelpers.LimelightTarget_Fiducial targetFiducial : targetFiducials)
+            totalDistanceFromTags += getDistanceFromTag((int) targetFiducial.fiducialID, results.getBotPose3d_wpiBlue());
+
+        return totalDistanceFromTags / results.targets_Fiducials.length;
+    }
+
+    private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) {
+        return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue());
+    }
+
+    private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) {
+        return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation());
+    }
+
+    private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) {
+        return results.targets_Fiducials[0];
+    }
+}