diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java new file mode 100644 index 000000000..b2dd5ada7 --- /dev/null +++ b/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 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 camera0Rays = getFuelRays(0); + List camera1Rays = getFuelRays(1); + List candidates = new ArrayList<>(); + for (RayObservation ray0 : camera0Rays) { + for (RayObservation ray1 : camera1Rays) { + triangulate(ray0, ray1).ifPresent(candidates::add); + } + } + candidates.sort(Comparator.comparingDouble(TriangulationCandidate::raySeparation)); + + Set usedCamera0Targets = new HashSet<>(); + Set usedCamera1Targets = new HashSet<>(); + List 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 getTargetPositionRobotRelative() { + return getFuelPositionsRobotRelative().stream().findFirst(); + } + + private List getFuelRays(int cameraIndex) { + StereoVisionIOInputsAutoLogged cameraInputs = inputs[cameraIndex]; + Transform3d robotToCamera = io[cameraIndex].getRobotToCamera(); + List 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 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 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); + } +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java new file mode 100644 index 000000000..28cda3a73 --- /dev/null +++ b/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))); +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java new file mode 100644 index 000000000..03a5f2219 --- /dev/null +++ b/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) {} +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java new file mode 100644 index 000000000..e72900f43 --- /dev/null +++ b/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 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(); + } + } + } +}