diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 7128890..8317c09 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -29,12 +29,73 @@
 // TODO<jim>: Probably need to break out LED control to separate process
 class CameraReader {
  public:
+  static const calibration::CameraCalibration *FindCameraCalibration(
+      const calibration::CalibrationData *calibration_data,
+      std::string_view node_name, int team_number);
+
+  static cv::Mat CameraIntrinsics(
+      const calibration::CameraCalibration *camera_calibration) {
+    cv::Mat result(3, 3, CV_32F,
+                   const_cast<void *>(static_cast<const void *>(
+                       camera_calibration->intrinsics()->data())));
+    result.convertTo(result, CV_64F);
+    CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+    return result;
+  }
+
+  static cv::Mat CameraExtrinsics(
+      const calibration::CameraCalibration *camera_calibration) {
+    // TODO(james): What's the principled way to handle non-z-axis turrets?
+    const frc971::vision::calibration::TransformationMatrix *transform =
+        camera_calibration->has_turret_extrinsics()
+            ? camera_calibration->turret_extrinsics()
+            : camera_calibration->fixed_extrinsics();
+
+    cv::Mat result(4, 4, CV_32F,
+                   const_cast<void *>(
+                       static_cast<const void *>(transform->data()->data())));
+    result.convertTo(result, CV_64F);
+    CHECK_EQ(result.total(), transform->data()->size());
+    return result;
+  }
+
+  static cv::Mat CameraDistCoeffs(
+      const calibration::CameraCalibration *camera_calibration) {
+    const cv::Mat result(5, 1, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             camera_calibration->dist_coeffs()->data())));
+    CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+    return result;
+  }
+
+  static std::pair<cv::Mat, cv::Mat> ComputeUndistortMaps(
+      const cv::Mat intrinsics, const cv::Mat dist_coeffs) {
+    std::pair<cv::Mat, cv::Mat> undistort_maps;
+    static const cv::Size kImageSize = {640, 480};
+    cv::initUndistortRectifyMap(intrinsics, dist_coeffs, cv::Mat(), intrinsics,
+                                kImageSize, CV_16SC2, undistort_maps.first,
+                                undistort_maps.second);
+    return undistort_maps;
+  }
+
+  static cv::Mat UndistortImage(cv::Mat image_distorted,
+                                std::pair<cv::Mat, cv::Mat> undistort_maps) {
+    cv::Mat image;
+    cv::remap(image_distorted, image, undistort_maps.first,
+              undistort_maps.second, cv::INTER_LINEAR);
+    return image;
+  }
+
   CameraReader(aos::ShmEventLoop *event_loop,
                const calibration::CalibrationData *calibration_data,
                V4L2Reader *reader)
       : event_loop_(event_loop),
         calibration_data_(calibration_data),
-        camera_calibration_(FindCameraCalibration()),
+        camera_calibration_(FindCameraCalibration(
+            calibration_data_, event_loop_->node()->name()->string_view(),
+            aos::network::GetTeamNumber())),
+        undistort_maps_(
+            ComputeUndistortMaps(CameraIntrinsics(), CameraDistCoeffs())),
         reader_(reader),
         image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
         target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
@@ -60,8 +121,6 @@
   double GetDutyCycle() { return duty_cycle_; }
 
  private:
-  const calibration::CameraCalibration *FindCameraCalibration() const;
-
   // Processes an image (including sending the results).
   void ProcessImage(cv::Mat image_mat_distorted,
                     int64_t image_monotonic_timestamp_ns);
@@ -70,40 +129,21 @@
   void ReadImage();
 
   cv::Mat CameraIntrinsics() const {
-    cv::Mat result(3, 3, CV_32F,
-                   const_cast<void *>(static_cast<const void *>(
-                       camera_calibration_->intrinsics()->data())));
-    result.convertTo(result, CV_64F);
-    CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
-    return result;
+    return CameraIntrinsics(camera_calibration_);
   }
 
   cv::Mat CameraExtrinsics() const {
-    // TODO(james): What's the principled way to handle non-z-axis turrets?
-    const frc971::vision::calibration::TransformationMatrix *transform =
-        camera_calibration_->has_turret_extrinsics()
-            ? camera_calibration_->turret_extrinsics()
-            : camera_calibration_->fixed_extrinsics();
-
-    cv::Mat result(4, 4, CV_32F,
-                   const_cast<void *>(
-                       static_cast<const void *>(transform->data()->data())));
-    result.convertTo(result, CV_64F);
-    CHECK_EQ(result.total(), transform->data()->size());
-    return result;
+    return CameraExtrinsics(camera_calibration_);
   }
 
   cv::Mat CameraDistCoeffs() const {
-    const cv::Mat result(5, 1, CV_32F,
-                         const_cast<void *>(static_cast<const void *>(
-                             camera_calibration_->dist_coeffs()->data())));
-    CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
-    return result;
+    return CameraDistCoeffs(camera_calibration_);
   }
 
   aos::ShmEventLoop *const event_loop_;
   const calibration::CalibrationData *const calibration_data_;
   const calibration::CameraCalibration *const camera_calibration_;
+  std::pair<cv::Mat, cv::Mat> undistort_maps_;
   V4L2Reader *const reader_;
   aos::Sender<CameraImage> image_sender_;
   TargetEstimator target_estimator_;