Stereo Logic

src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java

@@ -0,0 +1,187 @@
+package frc.robot.subsystems.stereoVision;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+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.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Optional;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+
+public class StereoVision extends SubsystemBase {
+  private final StereoVisionIO[] io = new StereoVisionIO[2];
+  private final StereoVisionIOInputsAutoLogged[] inputs = new StereoVisionIOInputsAutoLogged[2];
+  private final Alert[] disconnectedAlerts = new Alert[2];
+
+  public StereoVision() {
+    this(
+        new StereoVisionIOPhotonVision(
+            StereoVisionConstants.camera0Name, StereoVisionConstants.robotToCamera0),
+        new StereoVisionIOPhotonVision(
+            StereoVisionConstants.camera1Name, StereoVisionConstants.robotToCamera1));
+  }
+
+  public StereoVision(StereoVisionIO camera0, StereoVisionIO camera1) {
+    io[0] = camera0;
+    io[1] = camera1;
+    for (int i = 0; i < inputs.length; i++) {
+      inputs[i] = new StereoVisionIOInputsAutoLogged();
+      disconnectedAlerts[i] =
+          new Alert(
+              "Stereo camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning);
+    }
+  }
+
+  public Rotation2d getTargetX(int cameraIndex) {
+    int[] targetClassIds = inputs[cameraIndex].targetClassIds;
+    double[] targetYawDegrees = inputs[cameraIndex].targetYawDegrees;
+    for (int i = 0; i < targetClassIds.length; i++) {
+      if (targetClassIds[i] == StereoVisionConstants.fuelObjectClassId) {
+        return Rotation2d.fromDegrees(targetYawDegrees[i]);
+      }
+    }
+    return new Rotation2d();
+  }
+
+  /** Returns triangulated robot-relative positions for detected fuel objects. */
+  public List<Translation3d> getFuelPositionsRobotRelative() {
+    var camera0 = inputs[0];
+    var camera1 = inputs[1];
+    if (camera0.targetClassIds.length == 0 || camera1.targetClassIds.length == 0) {
+      return List.of();
+    }
+
+    double now = Timer.getFPGATimestamp();
+    if (now - camera0.timestampSeconds > StereoVisionConstants.maxObservationAgeSeconds
+        || now - camera1.timestampSeconds > StereoVisionConstants.maxObservationAgeSeconds
+        || Math.abs(camera0.timestampSeconds - camera1.timestampSeconds)
+            > StereoVisionConstants.maxTimestampDifferenceSeconds) {
+      return List.of();
+    }
+
+    List<RayObservation> camera0Rays = getFuelRays(0);
+    List<RayObservation> camera1Rays = getFuelRays(1);
+    List<TriangulationCandidate> candidates = new ArrayList<>();
+    for (RayObservation ray0 : camera0Rays) {
+      for (RayObservation ray1 : camera1Rays) {
+        triangulate(ray0, ray1).ifPresent(candidates::add);
+      }
+    }
+    candidates.sort(Comparator.comparingDouble(TriangulationCandidate::raySeparation));
+
+    Set<Integer> usedCamera0Targets = new HashSet<>();
+    Set<Integer> usedCamera1Targets = new HashSet<>();
+    List<Translation3d> positions = new ArrayList<>();
+    for (TriangulationCandidate candidate : candidates) {
+      if (usedCamera0Targets.contains(candidate.camera0TargetIndex())
+          || usedCamera1Targets.contains(candidate.camera1TargetIndex())) {
+        continue;
+      }
+      usedCamera0Targets.add(candidate.camera0TargetIndex());
+      usedCamera1Targets.add(candidate.camera1TargetIndex());
+      positions.add(candidate.position());
+    }
+    return List.copyOf(positions);
+  }
+
+  /** Returns the first triangulated fuel position for callers that only need one target. */
+  public Optional<Translation3d> getTargetPositionRobotRelative() {
+    return getFuelPositionsRobotRelative().stream().findFirst();
+  }
+
+  private List<RayObservation> getFuelRays(int cameraIndex) {
+    StereoVisionIOInputsAutoLogged cameraInputs = inputs[cameraIndex];
+    Transform3d robotToCamera = io[cameraIndex].getRobotToCamera();
+    List<RayObservation> rays = new ArrayList<>();
+    for (int i = 0; i < cameraInputs.targetClassIds.length; i++) {
+      if (cameraInputs.targetClassIds[i] != StereoVisionConstants.fuelObjectClassId) {
+        continue;
+      }
+      Translation3d direction =
+          new Translation3d(1.0, 0.0, 0.0)
+              .rotateBy(
+                  robotToCamera
+                      .getRotation()
+                      .rotateBy(
+                          new Rotation3d(
+                              0.0,
+                              Math.toRadians(cameraInputs.targetPitchDegrees[i]),
+                              Math.toRadians(cameraInputs.targetYawDegrees[i]))));
+      rays.add(new RayObservation(i, robotToCamera.getTranslation(), direction));
+    }
+    return rays;
+  }
+
+  private Optional<TriangulationCandidate> triangulate(RayObservation ray0, RayObservation ray1) {
+    Translation3d origin0 = ray0.origin();
+    Translation3d origin1 = ray1.origin();
+    Translation3d direction0 = ray0.direction();
+    Translation3d direction1 = ray1.direction();
+
+    Translation3d betweenOrigins = origin0.minus(origin1);
+    double dot = direction0.dot(direction1);
+    double denominator = 1.0 - dot * dot;
+    if (denominator < StereoVisionConstants.minimumTriangulationDenominator) {
+      return Optional.empty();
+    }
+
+    double origin0Projection = direction0.dot(betweenOrigins);
+    double origin1Projection = direction1.dot(betweenOrigins);
+    double distance0 = (dot * origin1Projection - origin0Projection) / denominator;
+    double distance1 = (origin1Projection - dot * origin0Projection) / denominator;
+    if (distance0 <= 0.0 || distance1 <= 0.0) {
+      return Optional.empty();
+    }
+
+    Translation3d point0 = origin0.plus(direction0.times(distance0));
+    Translation3d point1 = origin1.plus(direction1.times(distance1));
+    double raySeparation = point0.getDistance(point1);
+    if (raySeparation > StereoVisionConstants.maximumRaySeparationMeters) {
+      return Optional.empty();
+    }
+    return Optional.of(
+        new TriangulationCandidate(
+            ray0.targetIndex(), ray1.targetIndex(), point0.plus(point1).div(2.0), raySeparation));
+  }
+
+  private record RayObservation(int targetIndex, Translation3d origin, Translation3d direction) {}
+
+  private record TriangulationCandidate(
+      int camera0TargetIndex,
+      int camera1TargetIndex,
+      Translation3d position,
+      double raySeparation) {}
+
+  @Override
+  public void periodic() {
+    for (int i = 0; i < io.length; i++) {
+      io[i].updateInputs(inputs[i]);
+      Logger.processInputs("StereoVision/Camera" + Integer.toString(i), inputs[i]);
+    }
+
+    // Loop over cameras
+    for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) {
+      // Update disconnected alert
+      disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected);
+    }
+
+    List<Translation3d> fuelPositions = getFuelPositionsRobotRelative();
+    double[] coordinatesMeters = new double[fuelPositions.size() * 3];
+    for (int i = 0; i < fuelPositions.size(); i++) {
+      Translation3d position = fuelPositions.get(i);
+      coordinatesMeters[i * 3] = position.getX();
+      coordinatesMeters[i * 3 + 1] = position.getY();
+      coordinatesMeters[i * 3 + 2] = position.getZ();
+    }
+    Logger.recordOutput("StereoVision/FuelPositionCount", fuelPositions.size());
+    Logger.recordOutput("StereoVision/FuelPositionsRobotRelativeMeters", coordinatesMeters);
+  }
+}

src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java

@@ -0,0 +1,48 @@
+package frc.robot.subsystems.stereoVision;
+
+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.util.Units;
+
+public class StereoVisionConstants {
+  public static final String camera0Name = "stereo_camera_0";
+  public static final String camera1Name = "stereo_camera_1";
+
+  // Configure this PhotonVision pipeline to use the built-in Fuel v11n model.
+  public static final int yoloPipelineIndex = 0;
+  // Fuel v11n has one class, so its class ID is 0.
+  public static final int fuelObjectClassId = 0;
+
+  private static final Rotation3d rotationCorrection = new Rotation3d(0, 0, Math.PI / 2);
+
+  public static final double maxObservationAgeSeconds = 0.25;
+  public static final double maxTimestampDifferenceSeconds = 0.10;
+  public static final double minimumTriangulationDenominator = 1e-4;
+  public static final double maximumRaySeparationMeters = 0.25;
+
+  // replace these sample poses with measured stereo camera mounting poses.
+  public static final Transform3d robotToCamera0 =
+      new Transform3d( // front camera
+          new Translation3d(
+                  Units.inchesToMeters(8.779),
+                  Units.inchesToMeters(10.445),
+                  Units.inchesToMeters(27.152 + 1.75))
+              .rotateBy(rotationCorrection),
+          new Rotation3d(
+              Units.degreesToRadians(0.0),
+              Units.degreesToRadians(-25.2),
+              Units.degreesToRadians(0.0)));
+
+  public static final Transform3d robotToCamera1 =
+      new Transform3d( // rear left camera
+          new Translation3d(
+                  Units.inchesToMeters(9.562), // x
+                  Units.inchesToMeters(10.974), // y
+                  Units.inchesToMeters(17.035 + 1.75)) // z
+              .rotateBy(rotationCorrection),
+          new Rotation3d(
+              Units.degreesToRadians(0.0),
+              Units.degreesToRadians(-11.32),
+              Units.degreesToRadians(155.3)));
+}

src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.stereoVision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface StereoVisionIO {
+  @AutoLog
+  public static class StereoVisionIOInputs {
+    public boolean connected = false;
+    public int[] targetClassIds = new int[0];
+    public double[] targetYawDegrees = new double[0];
+    public double[] targetPitchDegrees = new double[0];
+    public double timestampSeconds = 0.0;
+  }
+
+  Transform3d getRobotToCamera();
+
+  default void updateInputs(StereoVisionIOInputs inputs) {}
+}

src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java

@@ -0,0 +1,47 @@
+package frc.robot.subsystems.stereoVision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.List;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class StereoVisionIOPhotonVision implements StereoVisionIO {
+  protected final PhotonCamera camera;
+  protected final Transform3d robotToCamera;
+
+  /**
+   * Creates a PhotonVision camera input for object detection.
+   *
+   * @param name The configured name of the camera.
+   * @param robotToCamera The 3D position of the camera relative to the robot.
+   */
+  public StereoVisionIOPhotonVision(String name, Transform3d robotToCamera) {
+    camera = new PhotonCamera(name);
+    this.robotToCamera = robotToCamera;
+    camera.setPipelineIndex(StereoVisionConstants.yoloPipelineIndex);
+  }
+
+  @Override
+  public Transform3d getRobotToCamera() {
+    return robotToCamera;
+  }
+
+  @Override
+  public void updateInputs(StereoVisionIOInputs inputs) {
+    inputs.connected = camera.isConnected();
+
+    for (var result : camera.getAllUnreadResults()) {
+      inputs.timestampSeconds = result.getTimestampSeconds();
+      List<PhotonTrackedTarget> targets = result.getTargets();
+      inputs.targetClassIds = new int[targets.size()];
+      inputs.targetYawDegrees = new double[targets.size()];
+      inputs.targetPitchDegrees = new double[targets.size()];
+      for (int i = 0; i < targets.size(); i++) {
+        PhotonTrackedTarget target = targets.get(i);
+        inputs.targetClassIds[i] = target.getDetectedObjectClassID();
+        inputs.targetYawDegrees[i] = target.getYaw();
+        inputs.targetPitchDegrees[i] = target.getPitch();
+      }
+    }
+  }
+}