Skip to content

Vision and Odometry

Vision is an extremely powerful tool in modern FRC robotics. With the switch from retroreflective tape to AprilTags in 2023, vision has become a lot more reliable and easier to use. In addition to this, vision is almost a requirement for success in some FRC games, such as 2025's Reefscape, where precise and fast alignment was necessary for scoring.

Pose Estimation

Pose estimation is the process of determining the robot's position and orientation on the field. This is typically done by fusing vision data with odometry data from the drivetrain.

Luckily, most of this process is made easy by WPILib and FRC vision systems.

AprilTags

AprilTags are the standard localization markers used in FRC. They look sort of similar to QR codes, and because of their large square shape, their orientation and position can be determined relative to the robot. If you know the position of AprilTags on the field (which FIRST provides in the game manual every year), you can determine where the robot is on the field.

AprilTags have IDs assigned to them, which you can use to identify and filter what the robot sees. As of 2025, FRC uses the format 36h11, which is a specific type of AprilTag that is designed to be robust and easy to detect.

WPILib Pose Estimation

WPILib provides a PoseEstimator class which makes pose estimation simple for your robot. I recommend all teams use this, as it is updated with each FRC game and makes implementing vision extremely easy. It is also very easy to switch to, as it is basically an extension of the odometry classes you may already be using.

The PoseEstimator class takes in odometry data from your drivetrain, as well as vision measurements from your camera, and fuses them together to provide an accurate estimate of the robot's position on the field. In order to use it, you will need to provide "standard deviation" (stdev) values to weigh the accuracy of your odometry and vision data, therefore increasing your robot's overall pose accuracy.

Standard deviation values may be measured physically, which I will not go into here, but generally, these can be estimated and adjusted depending on accuracy.

Below is an example of what a PoseEstimator class might look like, with standard deviation values at a good starting point. 

private val poseEstimator = SwerveDrivePoseEstimator(
        kinematics, // positions of modules
        Rotation2d(gyroAngle), // initial robot yaw (converted to Rotation2d)
        modulePositions, // initial "positions" (how far the wheels have moved and in what direction)
        Pose2d(0.0, 0.0, Rotation2d(gyroAngle)), // starting pose (x (meters), y (meters), rotation (radians))
        VecBuilder.fill(0.1, 0.1, 0.1), // odometry stdev vector, (x (meters), y (meters), rotation (radians))
        VecBuilder.fill(.9, .9, 2.0) // vision stdev vector, (x (meters), y (meters), rotation (radians))
    )
Then, in your periodic function, you will need to update the PoseEstimator. 
// (for each camera you are using)
// first, filter vision estimates to exclude anything ambiguous or incorrect
// then, add the pose and timestamp of the camera's measurement to the pose estimator
poseEstimator.addVisionMeasurement(visionEstimatedPose2d, timestampOfVisionEstimate)

// update with swerve data
poseEstimator.update(
        Rotation2d(gyroAngle), // current robot yaw (converted to Rotation2d)
        modulePositions // current "positions" (how far the wheels have moved and in what direction)
    )
The way you obtain vision estimates will vary by library, but you can see how my team accomplished this using PhotonVision in 2025 here.

Calibration

Calibrating cameras is extraordinarily important!!! If your camera is not calibrated, the robot may not be able to see the AprilTags, only see them from a specific distance, or they may only be recognized when the robot is sitting still.

Luckily, this process is typically fairly simple and consistent across different vision libraries. This is the process I recommend for calibrating cameras:

  1. Print out the calibration board for your library, make sure your settings are correct, and start the calibration in the library's interface. Make sure you have a resolution selected that your vision processor can handle at a higher frame rate. A minimum of 20fps is what I recommend. Low resolution = higher frame rate = shorter detection range. Find a balance.
  2. Put the board fairly close to the camera, but allow it to see the entire board.
  3. While holding it still, take a snapshot, and move the board to a different location/angle.
  4. Repeat steps 2-3 until you have a bunch of snapshots (at least 30 for accuracy). Try to capture all parts of the camera's view evenly, but especially the corners, as they are the most distorted, and it increases calibration accuracy.
  5. Finish the calibration and save it to the camera. This should be done automatically. Ensure your camera is in 3D mode and set to the same resolution used during calibration.
  6. Calibrate for your field.

Note

There is a setting called Decimation that basically reduces the resolution of the processed image to increase frame rate. You can play with this setting, but it does reduce detection range. In most cases, a decimation of 1 or 2 is best.

Field Calibration

Field calibration is another extremely important part of the calibration process. Every field has different lighting conditions which can severely impact performance if calibration is not done properly. Therefore, this should be done at every competition field you go to during the dedicated "field calibration" time.

This is the process I recommend for field/lighting calibration:

  1. Set the robot in a position where it can see at least one AprilTag, preferably two or more, and make sure it's at a reasonable distance, but in the range of normal detection.
  2. Plug into the robot via Ethernet and visit the calibration interface for your vision library.
  3. Turn off auto-exposure or any other automatic settings.
  4. Reduce exposure to a very low setting (1-10 ms). Exposure increases how long the camera's sensor is exposed to light, which increases brightness, but also increases motion blur if the robot is moving, making AprilTags harder to detect, so lower is better.
  5. Increase camera gain until the AprilTags are visible and consistently detected. If you can't see the AprilTags when camera gain is at 100%, then start to increase the exposure until you can see things clearly and the AprilTag detection is visually stable.
  6. Change brightness and contrast if needed, but usually they don't need much/any adjustment.
  7. If you have multiple cameras, assuming all cameras are the same, duplicate the settings. Otherwise, repeat steps 3-6 for each type of camera.
  8. While it may sound funny, one thing I recommend is shaking the robot's drivetrain while someone watches the camera feed to ensure the AprilTags are still detected while the robot is moving. If it loses detection, you may need to decrease exposure and increase camera gain to compensate.
  9. Make sure config is saved! It might be a good idea to take a picture of your values if you need to re-enter them later, you only have one field calibration time.

Note

Sometimes, the lighting at events can "flicker" at a really high frequency. You can't see it with your eyes, but if the robot's camera appears to get brighter and darker periodically, this is likely the cause. To mitigate this, adjust camera exposure until it is at a stable brightness, then compensate with camera gain again.

Object Detection

TODO: add example and explanation