Skip to content

multiple cameras #88

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.

Sign up for GitHub

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

Draft
Levercpu wants to merge 1 commit into
base: LS_SimulationYAY
Choose a base branch
from
Draft

multiple cameras #88

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/constants/GameConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,10 @@ public enum Mode {

public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later
public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later
public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change Later
public static final Transform3d[] ROBOT_TO_CAMERA = {new Transform3d(0,0,0, new Rotation3d(0,0,0)),
new Transform3d(0,0,0, new Rotation3d(0,0,Math.PI/2)),
new Transform3d(0,0,0, new Rotation3d(0,0,Math.PI)),
new Transform3d(0,0,0, new Rotation3d(0,0,3*Math.PI/2))}; // TODO: change Later
public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later
public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later
public static final double AVERAGE_PIR_LATENCY = 20; //ms
Expand Down
40 changes: 20 additions & 20 deletions src/main/java/frc/robot/subsystems/ApriltagSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,10 @@
package frc.robot.subsystems;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.apriltags.*;
Expand Down Expand Up @@ -74,23 +72,25 @@ public void periodic() {
}
}
public void simReadings() {
for (Apriltag tag: Apriltag.values()) {
Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA);
if (tag.canSee(cameraPos,Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) {
Pose3d adjPose = tag.getPose().relativeTo(cameraPos);
double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm());
double distance = tag.getTranslation().getDistance(cameraPos.getTranslation());
if (distance/cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) {
VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance,0);
Vector<N3> stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos);
double readingX = robotPoseSupplier.get().getX()+ random.nextGaussian()*stdDevs.get(0);
double readingY = robotPoseSupplier.get().getY()+ random.nextGaussian()*stdDevs.get(1);
double readingYaw = robotPoseSupplier.get().getRotation().getDegrees()+ random.nextGaussian()*stdDevs.get(2);
Pose2d readingPos = new Pose2d(readingX,readingY,Rotation2d.fromDegrees(readingYaw));
distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());
if(BasicVisionFilter.inBounds(readingPos)) {
io.addReading(new ApriltagReading(readingX, readingY, readingYaw,
distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0));
for (Transform3d cameraTransform: Constants.ROBOT_TO_CAMERA) {
for (Apriltag tag : Apriltag.values()) {
Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(cameraTransform);
if (tag.canSee(cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) {
Pose3d adjPose = tag.getPose().relativeTo(cameraPos);
double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm());
double distance = tag.getTranslation().getDistance(cameraPos.getTranslation());
if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) {
VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0);
Vector<N3> stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos);
double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0);
double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1);
double readingYaw = robotPoseSupplier.get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2);
Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw));
distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());
if (BasicVisionFilter.inBounds(readingPos)) {
io.addReading(new ApriltagReading(readingX, readingY, readingYaw,
distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0));
}
}
}
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -335,8 +335,8 @@ public double getError() {
public double getAverageError(){
return poseError.stream().mapToDouble(record -> record.error).average().orElse(0);
}
public Pose3d getCameraPose() {
return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA);
public Pose3d getCameraPose(int camera) {
return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA[camera]);
}

public Pose2d getSimulationPose() {
Expand Down
24 changes: 15 additions & 9 deletions src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult re

private final VisionFilter filter;
private final ConcurrentLinkedDeque<MeasurementRecord> lastSecondMeasurements = new ConcurrentLinkedDeque<>();
private final List<Pose2d> validMeasurementsPose = new ArrayList<>();
private final List<Pose2d> invalidMeasurementsPose = new ArrayList<>();;
private final List<Pose3d> acceptedTagsPose = new ArrayList<>();
private final List<VisionLog> log = new ArrayList<>();

public FilterablePoseManager(
PoseDeviation PoseDeviation,
Expand Down Expand Up @@ -57,19 +61,15 @@ public FilterablePoseManager(
}

@Override
public void processQueue() {
public void processQueue(int camera) {
double currentTime = Logger.getTimestamp()/1000000.0;
double oneSecondAgo = currentTime - 1.0;
lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo);
List<VisionLog> log = new ArrayList<>();
List<Pose2d> validMeasurementsPose = new ArrayList<>();
List<Pose2d> invalidMeasurementsPose = new ArrayList<>();;
List<Pose3d> acceptedTagsPose = new ArrayList<>();
for (Map.Entry<Integer, Queue<VisionMeasurement>> queueEntry : visionMeasurementQueueMap.entrySet()) {
int tagId = queueEntry.getKey();
Queue<VisionMeasurement> queue = queueEntry.getValue();
LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
filter.filter(queue, drivebase.getCameraPose());
filter.filter(queue, drivebase.getCameraPose(camera));
queue.clear();
for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
VisionMeasurement v = filterEntry.getKey();
Expand All @@ -78,11 +78,10 @@ public void processQueue() {
lastSecondMeasurements.add(new MeasurementRecord(Apriltag.of(tagId), v.timeOfMeasurement(),r));
switch (r) {
case ACCEPTED -> {
setVisionSTD(visionTruster.calculateTrust(v, drivebase.getCameraPose()));
setVisionSTD(visionTruster.calculateTrust(v, drivebase.getCameraPose(camera)));
validMeasurementsPose.add(v.measurement());
addVisionMeasurement(v);
acceptedTagsPose.add(Apriltag.of(tagId).getPose());

}
case NOT_PROCESSED -> queue.add(v);
case REJECTED -> {
Expand All @@ -91,15 +90,22 @@ public void processQueue() {
}
}
}


}
public void log() {
Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new));
Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new));
Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count());
Logger.recordOutput("Apriltag/numberNotProcessedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.NOT_PROCESSED).count());
Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count());
Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new));
Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new));
validMeasurementsPose.clear();
invalidMeasurementsPose.clear();
acceptedTagsPose.clear();
log.clear();
}

public VisionTruster getVisionTruster() {
return visionTruster;
}
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,10 @@ private void updateVision(int... invalidApriltagNumbers) {
}
long end = System.currentTimeMillis();
Logger.recordOutput("RegisteringVisionTimeMillis", end - start);
poseManager.processQueue();
for (int i=0; i<Constants.ROBOT_TO_CAMERA.length; i++) {
poseManager.processQueue(i);
}
poseManager.log();
}

private VisionMeasurement getVisionMeasurement(double[] pos, int index) {
Expand Down
9 changes: 6 additions & 3 deletions src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,15 +67,18 @@ public void registerVisionMeasurement(VisionMeasurement measurement, int tagId)
if (measurement == null) {
return;
}
visionMeasurementQueueMap.computeIfAbsent(tagId, k -> new LinkedList<>()).add(measurement);
while (visionMeasurementQueueMap.computeIfAbsent(tagId,k -> new LinkedList<>()).size() >= 3) {
visionMeasurementQueueMap.get(tagId).poll();
}
visionMeasurementQueueMap.get(tagId).add(measurement);
}

// override for filtering
public void processQueue() {
public void processQueue(int camera) {
for (Queue<VisionMeasurement> queue : visionMeasurementQueueMap.values()) {
VisionMeasurement m = queue.poll();
while (m != null) {
setVisionSTD(visionTruster.calculateTrust(m, drivebase.getCameraPose()));
setVisionSTD(visionTruster.calculateTrust(m, drivebase.getCameraPose(camera)));
addVisionMeasurement(m);
m = queue.poll();
}
Expand Down