Package frc.robot.Subsystems.Vision

Class VisionPoseCache

java.lang.Object
frc.robot.Subsystems.Vision.VisionPoseCache
public class VisionPoseCache extends Object

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    double
    last_ambiguity
     

  • Constructor Summary

    Constructors
    Constructor
    Description
    VisionPoseCache()
    Creates a new VisionPoseCache.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addPose(edu.wpi.first.math.geometry.Pose2d pose, edu.wpi.first.math.geometry.Pose2d referencePose, double timestampSeconds)
    Updates the cache with a new vision estimate.
    void
    clearOldPoses(double timestampSeconds)
     
    edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1>
    getStdevs()
    Returns the standard deviations for the associated camera based on the cached measurements.
    edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1>
    getWeightedStdevs()
    Returns standard deviations in combination with the last recorded ambiguity rating.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Field Details

    • last_ambiguity

      public double last_ambiguity

  • Constructor Details

    • VisionPoseCache

      public VisionPoseCache()
      Creates a new VisionPoseCache.

  • Method Details

    • addPose

      public void addPose(edu.wpi.first.math.geometry.Pose2d pose, edu.wpi.first.math.geometry.Pose2d referencePose, double timestampSeconds)
      Updates the cache with a new vision estimate. Clears out old datapoints.
      Parameters:
      pose - The vision-estimated pose.
      referencePose - The predicted pose of the robot from odometry.
      timestampSeconds - The time of the vision estimate.

    • clearOldPoses

      public void clearOldPoses(double timestampSeconds)

    • getStdevs

      public edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> getStdevs()
      Returns the standard deviations for the associated camera based on the cached measurements.
      Returns:
      The x, y, and angular standard deviations in meters and radians.

    • getWeightedStdevs

      public edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> getWeightedStdevs()
      Returns standard deviations in combination with the last recorded ambiguity rating.
      Returns:
      The weighted x, y, and angular standard deviations.