Skip to content

Commit

Permalink
move camera std dev factors to constructor for configuration
Browse files Browse the repository at this point in the history
  • Loading branch information
linglejack06 committed Dec 11, 2024
1 parent 536cb6a commit 39b9efe
Showing 1 changed file with 8 additions and 3 deletions.
11 changes: 8 additions & 3 deletions vision/src/main/java/coppercore/vision/VisionLocalizer.java
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,17 @@ public class VisionLocalizer extends SubsystemBase {
// avoid NullPointerExceptions by setting a default no-op
private VisionConsumer consumer;
private AprilTagFieldLayout aprilTagLayout;
private double[] cameraStdDevFactors;

public VisionLocalizer(
VisionConsumer consumer, AprilTagFieldLayout aprilTagLayout, VisionIO... io) {
VisionConsumer consumer,
AprilTagFieldLayout aprilTagLayout,
double[] cameraStdDevFactors,
VisionIO... io) {
this.consumer = consumer;
this.io = io;
this.aprilTagLayout = aprilTagLayout;
this.cameraStdDevFactors = cameraStdDevFactors;

// Initialize inputs
this.inputs = new VisionIOInputsAutoLogged[io.length];
Expand Down Expand Up @@ -127,8 +132,8 @@ private Matrix<N3, N1> getLatestVariance(
/ CoreVisionConstants.distanceFactor));

// adjustment based on position of camera
if (cameraIndex < CoreVisionConstants.cameraStdDevFactors.length) {
stdDev = stdDev.times(CoreVisionConstants.cameraStdDevFactors[cameraIndex]);
if (cameraIndex < this.cameraStdDevFactors.length) {
stdDev = stdDev.times(this.cameraStdDevFactors[cameraIndex]);
}

return stdDev;
Expand Down

0 comments on commit 39b9efe

Please sign in to comment.