diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index 8c2c0ad..4477856 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -5,6 +5,7 @@
 #include "aos/events/shm_event_loop.h"
 #include "aos/network/team_number.h"
 #include "aos/realtime.h"
+#include "frc971/constants/constants_sender_lib.h"
 #include "frc971/vision/calibration_generated.h"
 #include "frc971/vision/charuco_lib.h"
 #include "frc971/vision/target_map_generated.h"
@@ -15,9 +16,8 @@
 #include "third_party/apriltag/apriltag.h"
 #include "third_party/apriltag/apriltag_pose.h"
 #include "third_party/apriltag/tag16h5.h"
-#include "y2023/vision/april_debug_generated.h"
 #include "y2023/constants/constants_generated.h"
-#include "frc971/constants/constants_sender_lib.h"
+#include "y2023/vision/april_debug_generated.h"
 
 DECLARE_int32(team_number);
 
@@ -28,14 +28,20 @@
  public:
   AprilRoboticsDetector(aos::EventLoop *event_loop,
                         std::string_view channel_name);
-
   ~AprilRoboticsDetector();
 
   void SetWorkerpoolAffinities();
 
+  // Undistorts the april tag corners using the camera calibration
+  void UndistortDetection(apriltag_detection_t *det) const;
+
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
       cv::Mat image);
 
+  const cv::Mat extrinsics() const { return extrinsics_; }
+  const cv::Mat intrinsics() const { return intrinsics_; }
+  const cv::Mat dist_coeffs() const { return dist_coeffs_; }
+
  private:
   void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
 
@@ -44,36 +50,15 @@
       frc971::vision::TargetMapper::TargetId target_id,
       flatbuffers::FlatBufferBuilder *fbb);
 
-
-  static cv::Mat CameraIntrinsics(
-      const frc971::vision::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 CameraDistCoeffs(
-      const frc971::vision::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;
-  }
-
   apriltag_family_t *tag_family_;
   apriltag_detector_t *tag_detector_;
 
   const frc971::constants::ConstantsFetcher<Constants> calibration_data_;
   const frc971::vision::calibration::CameraCalibration *calibration_;
   cv::Mat intrinsics_;
-  cv::Mat camera_distortion_coeffs_;
+  cv::Mat projection_matrix_;
+  cv::Mat extrinsics_;
+  cv::Mat dist_coeffs_;
 
   aos::Ftrace ftrace_;