diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index a26c507..30ab84a 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -250,6 +250,7 @@
     deps = [
         ":blob_detector_lib",
         ":calibration_data",
+        ":camera_reader_lib",
         ":target_estimator_lib",
         "//aos:init",
         "//aos/events:shm_event_loop",
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 92d3727..0af4afc 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -21,12 +21,11 @@
 
 using namespace frc971::vision;
 
-const calibration::CameraCalibration *CameraReader::FindCameraCalibration()
-    const {
-  const std::string_view node_name = event_loop_->node()->name()->string_view();
-  const int team_number = aos::network::GetTeamNumber();
+const calibration::CameraCalibration *CameraReader::FindCameraCalibration(
+    const calibration::CalibrationData *calibration_data,
+    std::string_view node_name, int team_number) {
   for (const calibration::CameraCalibration *candidate :
-       *calibration_data_->camera_calibrations()) {
+       *calibration_data->camera_calibrations()) {
     if (candidate->node_name()->string_view() != node_name) {
       continue;
     }
@@ -92,9 +91,7 @@
 
 void CameraReader::ProcessImage(cv::Mat image_mat_distorted,
                                 int64_t image_monotonic_timestamp_ns) {
-  cv::Mat image_mat;
-  cv::undistort(image_mat_distorted, image_mat, CameraIntrinsics(),
-                CameraDistCoeffs());
+  cv::Mat image_mat = UndistortImage(image_mat_distorted, undistort_maps_);
 
   BlobDetector::BlobResult blob_result;
   BlobDetector::ExtractBlobs(image_mat, &blob_result);
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_;
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 26d9700..f76e0b9 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -378,16 +378,16 @@
       [](const std::pair<size_t, size_t> &a,
          const std::pair<size_t, size_t> &b) { return a.first < b.first; });
 
-  size_t middle_tape_index = 1000;
+  std::optional<size_t> middle_tape_index = std::nullopt;
   for (size_t i = 0; i < tape_indices.size(); ++i) {
     if (tape_indices[i].second == middle_blob_index_) {
       middle_tape_index = i;
     }
   }
-  CHECK_NE(middle_tape_index, 1000) << "Failed to find middle tape";
+  CHECK(middle_tape_index.has_value()) << "Failed to find middle tape";
 
   if (VLOG_IS_ON(2)) {
-    LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+    LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
               << middle_blob_index_;
     for (size_t i = 0; i < tape_indices.size(); ++i) {
       const auto distance = DistanceFromTapeIndex(
@@ -400,7 +400,7 @@
 
   {
     size_t offset = 0;
-    for (size_t i = middle_tape_index + 1; i < tape_indices.size(); ++i) {
+    for (size_t i = *middle_tape_index + 1; i < tape_indices.size(); ++i) {
       tape_indices[i].first -= offset;
 
       if (tape_indices[i].first > tape_indices[i - 1].first + 1) {
@@ -412,7 +412,7 @@
   }
 
   if (VLOG_IS_ON(2)) {
-    LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+    LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
               << middle_blob_index_;
     for (size_t i = 0; i < tape_indices.size(); ++i) {
       const auto distance = DistanceFromTapeIndex(
@@ -425,7 +425,7 @@
 
   {
     size_t offset = 0;
-    for (size_t i = middle_tape_index; i > 0; --i) {
+    for (size_t i = *middle_tape_index; i > 0; --i) {
       tape_indices[i - 1].first -= offset;
 
       if (tape_indices[i - 1].first + 1 < tape_indices[i].first) {
@@ -440,7 +440,7 @@
   }
 
   if (VLOG_IS_ON(2)) {
-    LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+    LOG(INFO) << "Middle tape is " << *middle_tape_index << ", blob "
               << middle_blob_index_;
     for (size_t i = 0; i < tape_indices.size(); ++i) {
       const auto distance = DistanceFromTapeIndex(
@@ -566,11 +566,11 @@
     size_t blob_index, const std::vector<cv::Point_<S>> &tape_points) const {
   auto distance = cv::Point_<S>(std::numeric_limits<S>::infinity(),
                                 std::numeric_limits<S>::infinity());
-  size_t final_match = 255;
+  std::optional<size_t> final_match = std::nullopt;
   if (blob_index == middle_blob_index_) {
     // Fix the middle blob so the solver can't go too far off
     final_match = tape_points.size() / 2;
-    distance = DistanceFromTapeIndex(blob_index, final_match, tape_points);
+    distance = DistanceFromTapeIndex(blob_index, *final_match, tape_points);
   } else {
     // Give the other blob_stats some freedom in case some are split into pieces
     for (auto it = tape_points.begin(); it < tape_points.end(); it++) {
@@ -585,11 +585,11 @@
     }
   }
 
-  VLOG(2) << "Matched index " << blob_index << " to " << final_match
+  CHECK(final_match.has_value());
+  VLOG(2) << "Matched index " << blob_index << " to " << *final_match
           << " distance " << distance.x << " " << distance.y;
-  CHECK_NE(final_match, 255);
 
-  return final_match;
+  return *final_match;
 }
 
 void TargetEstimator::DrawProjectedHub(
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 446f1f6..e455f66 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -13,6 +13,7 @@
 #include "frc971/vision/vision_generated.h"
 #include "y2022/vision/blob_detector.h"
 #include "y2022/vision/calibration_data.h"
+#include "y2022/vision/camera_reader.h"
 #include "y2022/vision/target_estimate_generated.h"
 #include "y2022/vision/target_estimator.h"
 
@@ -213,51 +214,24 @@
   const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
       CalibrationData());
 
-  const calibration::CameraCalibration *calibration = nullptr;
-  for (const calibration::CameraCalibration *candidate :
-       *calibration_data.message().camera_calibrations()) {
-    if ((candidate->node_name()->string_view() == FLAGS_calibration_node) &&
-        (candidate->team_number() == FLAGS_calibration_team_number)) {
-      calibration = candidate;
-      break;
-    }
-  }
+  const calibration::CameraCalibration *calibration =
+      CameraReader::FindCameraCalibration(&calibration_data.message(),
+                                          FLAGS_calibration_node,
+                                          FLAGS_calibration_team_number);
+  const auto intrinsics = CameraReader::CameraIntrinsics(calibration);
+  const auto extrinsics = CameraReader::CameraExtrinsics(calibration);
+  const auto dist_coeffs = CameraReader::CameraDistCoeffs(calibration);
 
-  CHECK(calibration) << "No calibration data found for node \""
-                     << FLAGS_calibration_node << "\" with team number "
-                     << FLAGS_calibration_team_number;
-
-  const auto intrinsics_float = cv::Mat(
-      3, 3, CV_32F,
-      const_cast<void *>(
-          static_cast<const void *>(calibration->intrinsics()->data())));
-  cv::Mat intrinsics;
-  intrinsics_float.convertTo(intrinsics, CV_64F);
-
-  const frc971::vision::calibration::TransformationMatrix *transform =
-      calibration->has_turret_extrinsics() ? calibration->turret_extrinsics()
-                                           : calibration->fixed_extrinsics();
-
-  const auto extrinsics_float = cv::Mat(
-      4, 4, CV_32F,
-      const_cast<void *>(static_cast<const void *>(transform->data()->data())));
-  cv::Mat extrinsics;
-  extrinsics_float.convertTo(extrinsics, CV_64F);
-
-  const auto dist_coeffs_float = cv::Mat(
-      5, 1, CV_32F,
-      const_cast<void *>(
-          static_cast<const void *>(calibration->dist_coeffs()->data())));
-  cv::Mat dist_coeffs;
-  dist_coeffs_float.convertTo(dist_coeffs, CV_64F);
+  // Compute undistortion map once for efficiency
+  const auto undistort_maps =
+      CameraReader::ComputeUndistortMaps(intrinsics, dist_coeffs);
 
   TargetEstimator estimator(intrinsics, extrinsics);
 
   for (auto it = file_list.begin() + FLAGS_skip; it < file_list.end(); it++) {
     LOG(INFO) << "Reading file " << (it - file_list.begin()) << ": " << *it;
-    cv::Mat image_mat_distorted = cv::imread(it->c_str());
-    cv::Mat image_mat;
-    cv::undistort(image_mat_distorted, image_mat, intrinsics, dist_coeffs);
+    cv::Mat image_mat =
+        CameraReader::UndistortImage(cv::imread(it->c_str()), undistort_maps);
 
     BlobDetector::BlobResult blob_result;
     blob_result.binarized_image =
diff --git a/y2022/vision/viewer_replay.cc b/y2022/vision/viewer_replay.cc
index b2d3464..5d09d55 100644
--- a/y2022/vision/viewer_replay.cc
+++ b/y2022/vision/viewer_replay.cc
@@ -194,6 +194,8 @@
 
         bool use_image = true;
         if (FLAGS_detected_only || FLAGS_filtered_only) {
+          // TODO(milind): if adding target estimation here in the future,
+          // undistortion is needed
           BlobDetector::BlobResult blob_result;
           BlobDetector::ExtractBlobs(image_mat, &blob_result);