Add circle fitting for doing vision tracking

Most code stolen from FRC 6328 Mechanical Advantage

Co-Authored-By: Aum-P <[email protected]>
Co-Authored-By: Jonah <[email protected]>
Co-Authored-By: Ayushk2023 <[email protected]>

Author: 4 people

src/main/java/frc/subsystem/VisionManager.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -18,9 +19,11 @@
 import frc.robot.Constants;
 import frc.subsystem.BlinkinLED.BlinkinLedMode;
 import frc.subsystem.BlinkinLED.LedStatus;
+import frc.utility.CircleFitter;
 import frc.utility.Limelight;
 import frc.utility.Limelight.LedMode;
 import frc.utility.Limelight.LimelightResolution;
+import frc.utility.geometry.GeometryUtils;
 import frc.utility.net.editing.LiveEditableValue;
 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
@@ -30,10 +33,7 @@
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
-import java.util.HashSet;
-import java.util.Objects;
-import java.util.Optional;
-import java.util.Set;
+import java.util.*;
 import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.locks.ReentrantReadWriteLock;
 
@@ -122,10 +122,11 @@ public void logData() {
     private static final MatBuilder<N3, N1> THREE_BY_ONE_MAT_BUILDER = new MatBuilder<>(Nat.N3(), Nat.N1());
 
     private final LiveEditableValue<Rotation> cameraRotation;
-
+    private final LiveEditableValue<Rotation2d> cameraRotation2d;
     private final LiveEditableValue<Double> hOffset;
     private final LiveEditableValue<Double> depthOffset;
     private final LiveEditableValue<Vector3D> centerOffset;
+    private final LiveEditableValue<Transform2d> centerOffsetTranslation2d;
 
     {
         NetworkTable guiTable = limelight.limelightGuiTable;
@@ -135,6 +136,13 @@ public void logData() {
                 guiTable.getEntry("centerOffset"),
                 (value) -> new Vector3D(0, 0, (Double) value),
                 Vector3D::getZ);
+
+        centerOffsetTranslation2d = new LiveEditableValue<>(
+                new Transform2d(new Translation2d(IS_PRACTICE ? 6.9 : 18, 0), new Rotation2d()),
+                guiTable.getEntry("centerOffsetNew"),
+                (value) -> new Transform2d(new Translation2d((Double) value, 0), new Rotation2d()),
+                Transform2d::getX);
+
         cameraRotation = new LiveEditableValue<>(
                 new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR,
                         Math.toRadians(IS_PRACTICE ? -37.5 : -34.5), 0, 0),
@@ -146,6 +154,13 @@ public void logData() {
                 (value) ->
                         Math.toDegrees(value.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[0])
         );
+
+        cameraRotation2d = new LiveEditableValue<>(
+                new Rotation2d(Math.toRadians(IS_PRACTICE ? -37.5 : -34.5)),
+                guiTable.getEntry("angle"),
+                (value) -> Rotation2d.fromDegrees((Double) value),
+                Rotation2d::getDegrees
+        );
     }
 
     private Vector3D getPositionOfTargetRelativeToRobot() {
@@ -277,6 +292,28 @@ public void update() {
         logData("Angle To Target", angleToTarget);
 
 
+        Optional<Pose2d> circleFitVisionPoseOptional = processFrame();
+
+        if (circleFitVisionPoseOptional.isPresent()) {
+            Pose2d circleFitVisionPose = circleFitVisionPoseOptional.get();
+
+            if (DriverStation.isTeleopEnabled()) {
+                robotTracker.addVisionMeasurement(circleFitVisionPose.getTranslation(), getLimelightTime(), false);
+            }
+            RobotPositionSender.addRobotPosition(new RobotState(
+                    circleFitVisionPose.getX(),
+                    circleFitVisionPose.getY(),
+                    circleFitVisionPose.getRotation().getRadians(),
+                    getLimelightTime(),
+                    "Circle Fit Vision Pose"
+            ));
+
+            logData("Circle Fit Vision Pose X", circleFitVisionPose.getX());
+            logData("Circle Fit Vision Pose Y", circleFitVisionPose.getY());
+            logData("Circle Fit Vision Pose Angle", circleFitVisionPose.getRotation().getRadians());
+            logData("Circle Fit Vision Pose Time", getLimelightTime());
+        }
+
         Optional<Translation2d> robotTranslationOptional = getVisionTranslation();
         if (robotTranslationOptional.isPresent()) {
             Translation2d robotTranslation = robotTranslationOptional.get();
@@ -306,7 +343,7 @@ public void update() {
                 if (dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation(),
                         robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) {
                     if (DriverStation.isTeleopEnabled()) {
-                        robotTracker.addVisionMeasurement(robotTranslation, getLimelightTime(), false);
+                        // robotTracker.addVisionMeasurement(robotTranslation, getLimelightTime(), false);
                     }
 
                     logData("Using Vision Info", "Using Vision Info");
@@ -351,4 +388,248 @@ public void shootBallsUntilBeamBreakHasBroken(double shootTime) throws Interrupt
     public void close() throws Exception {
 
     }
+
+    double lastCaptureTimestamp = 0;
+    private static final int MIN_TARGET_COUNT = 3; // For calculating odometry
+    private double lastTranslationsTimestamp;
+    private List<Translation2d> lastTranslations;
+
+    public static final double VISION_TARGET_HEIGHT_LOWER =
+            Units.inchesToMeters(8.0 * 12.0 + 5.625); // Bottom of tape
+    public static final double VISION_TARGET_HEIGHT_UPPER = VISION_TARGET_HEIGHT_LOWER + Units.inchesToMeters(2.0); // Top of tape
+    public static final double VISION_TARGET_DIAMETER = Units.inchesToMeters(4.0 * 12.0 + 5.375);
+    private static final double CIRCLE_FIT_PRECISION = 0.01;
+
+    public static final double FIELD_LENGTH = Units.inchesToMeters(54.0 * 12.0);
+    public static final double FIELD_WIDTH = Units.inchesToMeters(27.0 * 12.0);
+    public static final Translation2d HUB_CENTER = new Translation2d(FIELD_LENGTH / 2.0, FIELD_WIDTH / 2.0);
+
+    /**
+     * Process the current vision data
+     *
+     * @return The robot's position relative to the field
+     * @author jonahb55
+     * @author Aum-P
+     * @author V-RA
+     * @author Ayushk2023
+     * @author Mechanical Advantage 6328
+     */
+    private Optional<Pose2d> processFrame() {
+        // noinspection FloatingPointEquality
+        if (Limelight.getInstance().getTimestamp() == lastCaptureTimestamp) {
+            // Exit if no new frame
+            return Optional.empty();
+        }
+
+        Limelight.getInstance().getTimestamp();
+        lastCaptureTimestamp = Limelight.getInstance().getTimestamp();
+
+        CameraPosition cameraPosition = new CameraPosition(hOffset.get(), cameraRotation2d.get(),
+                centerOffsetTranslation2d.get());
+        Vector2d[] cornersRaw = Limelight.getInstance().getCorners();
+
+        int targetCount = cornersRaw.length / 4;
+        // Calculate camera to target translation
+        if (targetCount >= MIN_TARGET_COUNT && cornersRaw.length % 4 == 0) {
+            // Calculate individual corner translations
+            List<Translation2d> cameraToTargetTranslations = new ArrayList<>();
+            for (int targetIndex = 0; targetIndex < targetCount; targetIndex++) {
+                List<VisionPoint> corners = new ArrayList<>();
+                double totalX = 0.0, totalY = 0.0;
+                for (int i = targetIndex * 4; i < (targetIndex * 4) + 4; i++) {
+                    if (i < cornersRaw.length) {
+                        corners.add(new VisionPoint(cornersRaw[i].x, cornersRaw[i].y));
+                        totalX += cornersRaw[i].x;
+                        totalY += cornersRaw[i].y;
+                    }
+                }
+
+                VisionPoint targetAvg = new VisionPoint(totalX / 4, totalY / 4);
+                corners = sortCorners(corners, targetAvg);
+
+                for (int i = 0; i < corners.size(); i++) {
+                    Translation2d translation = solveCameraToTargetTranslation(
+                            corners.get(i), i < 2 ? VISION_TARGET_HEIGHT_LOWER : VISION_TARGET_HEIGHT_UPPER, cameraPosition);
+                    if (translation != null) {
+                        cameraToTargetTranslations.add(translation);
+                    }
+                }
+            }
+
+            // Save individual translations
+            lastTranslationsTimestamp = Timer.getFPGATimestamp();
+            lastTranslations = cameraToTargetTranslations;
+
+            // Combine corner translations to full target translation
+            if (cameraToTargetTranslations.size() >= MIN_TARGET_COUNT * 4) {
+                Translation2d cameraToTargetTranslation =
+                        CircleFitter.fit(VISION_TARGET_DIAMETER / 2.0,
+                                cameraToTargetTranslations, CIRCLE_FIT_PRECISION);
+
+                // Calculate field to robot translation
+                Rotation2d robotRotation = RobotTracker.getInstance().getGyroRotation(lastCaptureTimestamp);
+                Rotation2d cameraRotation = robotRotation
+                        .rotateBy(cameraPosition.vehicleToCamera.getRotation());
+                Transform2d fieldToTargetRotated =
+                        new Transform2d(HUB_CENTER, cameraRotation);
+                Transform2d fieldToCamera = fieldToTargetRotated.plus(
+                        GeometryUtils.transformFromTranslation(cameraToTargetTranslation.unaryMinus()));
+                Pose2d fieldToVehicle =
+                        GeometryUtils.transformToPose(fieldToCamera.plus(cameraPosition.vehicleToCamera.inverse()));
+
+                if (fieldToVehicle.getX() > FIELD_LENGTH
+                        || fieldToVehicle.getX() < 0.0
+                        || fieldToVehicle.getY() > FIELD_WIDTH
+                        || fieldToVehicle.getY() < 0.0) {
+                    return Optional.empty();
+                }
+                return Optional.of(fieldToVehicle);
+            }
+        }
+        return Optional.empty();
+    }
+
+    /**
+     * @author jonahb55
+     * @author Aum-P
+     * @author V-RA
+     * @author Ayushk2023
+     * @author Mechanical Advantage 6328
+     */
+    private List<VisionPoint> sortCorners(List<VisionPoint> corners,
+                                          VisionPoint average) {
+
+        // Find top corners
+        Integer topLeftIndex = null;
+        Integer topRightIndex = null;
+        double minPosRads = Math.PI;
+        double minNegRads = Math.PI;
+        for (int i = 0; i < corners.size(); i++) {
+            VisionPoint corner = corners.get(i);
+            double angleRad =
+                    new Rotation2d(corner.x - average.x, average.y - corner.y)
+                            .minus(Rotation2d.fromDegrees(90)).getRadians();
+            if (angleRad > 0) {
+                if (angleRad < minPosRads) {
+                    minPosRads = angleRad;
+                    topLeftIndex = i;
+                }
+            } else {
+                if (Math.abs(angleRad) < minNegRads) {
+                    minNegRads = Math.abs(angleRad);
+                    topRightIndex = i;
+                }
+            }
+        }
+
+        // Find lower corners
+        Integer lowerIndex1 = null;
+        Integer lowerIndex2 = null;
+        for (int i = 0; i < corners.size(); i++) {
+            boolean alreadySaved = false;
+            if (topLeftIndex != null) {
+                if (topLeftIndex.equals(i)) {
+                    alreadySaved = true;
+                }
+            }
+            if (topRightIndex != null) {
+                if (topRightIndex.equals(i)) {
+                    alreadySaved = true;
+                }
+            }
+            if (!alreadySaved) {
+                if (lowerIndex1 == null) {
+                    lowerIndex1 = i;
+                } else {
+                    lowerIndex2 = i;
+                }
+            }
+        }
+
+        // Combine final list
+        List<VisionPoint> newCorners = new ArrayList<>();
+        if (topLeftIndex != null) {
+            newCorners.add(corners.get(topLeftIndex));
+        }
+        if (topRightIndex != null) {
+            newCorners.add(corners.get(topRightIndex));
+        }
+        if (lowerIndex1 != null) {
+            newCorners.add(corners.get(lowerIndex1));
+        }
+        if (lowerIndex2 != null) {
+            newCorners.add(corners.get(lowerIndex2));
+        }
+        return newCorners;
+    }
+
+    public static final Rotation2d FOV_HORIZONTAL = Rotation2d.fromDegrees(59.6);
+    public static final Rotation2d FOV_VERTICAL = Rotation2d.fromDegrees(49.7);
+
+    private static final double vpw = 2.0 * Math.tan(FOV_HORIZONTAL.getRadians() / 2.0);
+    private static final double vph = 2.0 * Math.tan(FOV_VERTICAL.getRadians() / 2.0);
+
+    /**
+     * Camera offsets in pixels?
+     */
+    public static final double CROSSHAIR_X = 0;
+    public static final double CROSSHAIR_Y = 0;
+
+    /**
+     * @author jonahb55
+     * @author Aum-P
+     * @author V-RA
+     * @author Ayushk2023
+     * @author Mechanical Advantage 6328
+     */
+    private Translation2d solveCameraToTargetTranslation(VisionPoint corner, double goalHeight, CameraPosition cameraPosition) {
+
+        double halfWidthPixels = limelight.getCameraResolution().width / 2.0;
+        double halfHeightPixels = limelight.getCameraResolution().height / 2.0;
+        double nY = -((corner.x - halfWidthPixels - CROSSHAIR_X) / halfWidthPixels);
+        double nZ = -((corner.y - halfHeightPixels - CROSSHAIR_Y) / halfHeightPixels);
+
+        Translation2d xzPlaneTranslation = new Translation2d(1.0, vph / 2.0 * nZ).rotateBy(cameraPosition.verticalRotation);
+        double x = xzPlaneTranslation.getX();
+        double y = vpw / 2.0 * nY;
+        double z = xzPlaneTranslation.getY();
+
+        double differentialHeight = cameraPosition.cameraHeight - goalHeight;
+        if ((z < 0.0) == (differentialHeight > 0.0)) {
+            double scaling = differentialHeight / -z;
+            double distance = Math.hypot(x, y) * scaling;
+            Rotation2d angle = new Rotation2d(x, y);
+            return new Translation2d(distance * angle.getCos(), distance * angle.getSin());
+        }
+        return null;
+    }
+
+    /**
+     * @author jonahb55
+     */
+    public static class VisionPoint {
+        public final double x;
+        public final double y;
+
+        public VisionPoint(double x, double y) {
+            this.x = x;
+            this.y = y;
+        }
+    }
+
+    /**
+     * @author jonahb55
+     */
+    public static final class CameraPosition {
+        public final double cameraHeight;
+        public final Rotation2d verticalRotation;
+        public final Transform2d vehicleToCamera;
+
+        public CameraPosition(double cameraHeight, Rotation2d verticalRotation,
+                              Transform2d vehicleToCamera) {
+            this.cameraHeight = cameraHeight;
+            this.verticalRotation = verticalRotation;
+            this.vehicleToCamera = vehicleToCamera;
+        }
+    }
 }

src/main/java/frc/utility/CircleFitter.java

@@ -0,0 +1,71 @@
+package frc.utility;
+
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.util.List;
+
+/**
+ * @author jonahb55
+ * @author Mechanical Advantage 6328
+ */
+public final class CircleFitter {
+  private CircleFitter() {}
+
+  public static Translation2d fit(double radius, List<Translation2d> points,
+                                  double precision) {
+
+    // Find starting point
+    double xSum = 0.0;
+    double ySum = 0.0;
+    for (Translation2d point : points) {
+      xSum += point.getX();
+      ySum += point.getY();
+    }
+    Translation2d center =
+            new Translation2d(xSum / points.size() + radius, ySum / points.size());
+
+    // Iterate to find optimal center
+    double shiftDist = radius / 2.0;
+    double minResidual = calcResidual(radius, points, center);
+    while (true) {
+      List<Translation2d> translations = List.of(
+              new Translation2d(shiftDist, 0.0), new Translation2d(-shiftDist, 0.0),
+              new Translation2d(0.0, shiftDist),
+              new Translation2d(0.0, -shiftDist));
+      Translation2d bestPoint = center;
+      boolean centerIsBest = true;
+
+      // Check all adjacent positions
+      for (Translation2d translation : translations) {
+        double residual =
+                calcResidual(radius, points, center.plus(translation));
+        if (residual < minResidual) {
+          bestPoint = center.plus(translation);
+          minResidual = residual;
+          centerIsBest = false;
+          break;
+        }
+      }
+
+      // Decrease shift, exit, or continue
+      if (centerIsBest) {
+        shiftDist /= 2.0;
+        if (shiftDist < precision) {
+          return center;
+        }
+      } else {
+        center = bestPoint;
+      }
+    }
+  }
+
+  private static double calcResidual(double radius, List<Translation2d> points,
+                                     Translation2d center) {
+    double residual = 0.0;
+    for (Translation2d point : points) {
+      double diff = point.getDistance(center) - radius;
+      residual += diff * diff;
+    }
+    return residual;
+  }
+}

src/main/java/frc/utility/Limelight.java

@@ -37,16 +37,19 @@ public enum LimelightResolution {
             this.height = height;
         }
 
-        final int width;
-        final int height;
+        public final int width;
+        public final int height;
     }
 
-    public LimelightResolution cameraResolution = LimelightResolution.k320x240;
+    private LimelightResolution cameraResolution = LimelightResolution.k320x240;
 
     public void setCameraResolution(LimelightResolution resolution) {
         cameraResolution = resolution;
     }
 
+    public LimelightResolution getCameraResolution() {
+        return cameraResolution;
+    }
 
     public static @NotNull Limelight getInstance(String name) {
         LIMELIGHT_MAP_LOCK.readLock().lock();

src/main/java/frc/utility/geometry/GeometryUtils.java

@@ -1,7 +1,6 @@
 package frc.utility.geometry;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.*;
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 
@@ -14,9 +13,123 @@ private GeometryUtils() {}
      *
      * @param translation The translation2d to compute the angle of.
      * @return The angle of the translation. (in radians)
+     * @author Varun
      */
     @Contract(value = "_ -> new", pure = true)
     public static @NotNull Rotation2d angleOf(@NotNull Translation2d translation) {
         return new Rotation2d(Math.atan2(translation.getY(), translation.getX()));
     }
+
+    /**
+     * Creates a pure translating transform
+     *
+     * @param translation The translation to create the transform with
+     * @return The resulting transform
+     * @author jonahb55
+     */
+    public static Transform2d transformFromTranslation(
+            Translation2d translation) {
+        return new Transform2d(translation, new Rotation2d());
+    }
+
+    /**
+     * Creates a pure translating transform
+     *
+     * @param x The x componenet of the translation
+     * @param y The y componenet of the translation
+     * @return The resulting transform
+     * @author jonahb55
+     */
+    public static Transform2d transformFromTranslation(double x, double y) {
+        return new Transform2d(new Translation2d(x, y), new Rotation2d());
+    }
+
+    /**
+     * Creates a pure rotating transform
+     *
+     * @param rotation The rotation to create the transform with
+     * @return The resulting transform
+     * @author jonahb55
+     */
+    public static Transform2d transformFromRotation(Rotation2d rotation) {
+        return new Transform2d(new Translation2d(), rotation);
+    }
+
+    /**
+     * Creates a pure translated pose
+     *
+     * @param translation The translation to create the pose with
+     * @return The resulting pose
+     * @author jonahb55
+     */
+    public static Pose2d poseFromTranslation(Translation2d translation) {
+        return new Pose2d(translation, new Rotation2d());
+    }
+
+    /**
+     * Creates a pure rotated pose
+     *
+     * @param rotation The rotation to create the pose with
+     * @return The resulting pose
+     * @author jonahb55
+     */
+    public static Pose2d poseFromRotation(Rotation2d rotation) {
+        return new Pose2d(new Translation2d(), rotation);
+    }
+
+    /**
+     * Converts a Pose2d to a Transform2d to be used in a kinematic chain
+     *
+     * @param pose The pose that will represent the transform
+     * @return The resulting transform
+     * @author jonahb55
+     */
+    public static Transform2d poseToTransform(Pose2d pose) {
+        return new Transform2d(pose.getTranslation(), pose.getRotation());
+    }
+
+    /**
+     * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic chain
+     *
+     * @param transform The transform that will represent the pose
+     * @return The resulting pose
+     * @author jonahb55
+     */
+    public static Pose2d transformToPose(Transform2d transform) {
+        return new Pose2d(transform.getTranslation(), transform.getRotation());
+    }
+
+    /**
+     * Interpolates between two poses based on the scale factor t. For example, t=0 would result in the first pose, t=1 would
+     * result in the last pose, and t=0.5 would result in a pose which is exactly halfway between the two poses. Values of t less
+     * than zero return the first pose, and values of t greater than 1 return the last pose.
+     *
+     * @param lhs The left hand side, or first pose to use for interpolation
+     * @param rhs The right hand side, or last pose to use for interpolation
+     * @param t   The scale factor, 0 <= t <= 1
+     * @return The pose which represents the interpolation. For t <= 0, the "lhs" parameter is returned directly. For t >= 1, the
+     * "rhs" parameter is returned directly.
+     * @author jonahb55
+     */
+    public static Pose2d interpolate(Pose2d lhs, Pose2d rhs, double t) {
+        if (t <= 0) {
+            return lhs;
+        } else if (t >= 1) {
+            return rhs;
+        }
+        Twist2d twist = lhs.log(rhs);
+        Twist2d scaled = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
+        return lhs.exp(scaled);
+    }
+
+    /**
+     * Returns the direction that this translation makes with the origin as a Rotation2d
+     *
+     * @param translation The translation
+     * @return The direction of the translation
+     * @author jonahb55
+     */
+    public static Rotation2d direction(Translation2d translation) {
+        return new Rotation2d(translation.getX(), translation.getY());
+    }
 }