diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 3186f44..839f11e 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -30,8 +30,11 @@
         "//aos:init",
         "//aos:json_to_flatbuffer",
         "//aos/events:shm_event_loop",
+        "//frc971/constants:constants_sender_lib",
         "//frc971/vision:vision_fbs",
         "//third_party:opencv",
+        "//y2023/vision:april_debug_fbs",
+        "//y2023/vision:vision_util",
         "@com_google_absl//absl/strings",
     ],
 )
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 68495b1..7877a57 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -6,7 +6,12 @@
 #include "aos/init.h"
 #include "aos/json_to_flatbuffer.h"
 #include "aos/time/time.h"
+#include "frc971/constants/constants_sender_lib.h"
 #include "frc971/vision/vision_generated.h"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/imgproc.hpp"
+#include "y2023/vision/april_debug_generated.h"
+#include "y2023/vision/vision_util.h"
 
 DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
 DEFINE_string(channel, "/camera", "Channel name for the image.");
@@ -16,23 +21,32 @@
 
 DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
 
-namespace frc971 {
+namespace y2023 {
 namespace vision {
 namespace {
 
-aos::Fetcher<CameraImage> image_fetcher;
-bool DisplayLoop() {
+using frc971::vision::CameraImage;
+
+bool DisplayLoop(const cv::Mat intrinsics, const cv::Mat dist_coeffs,
+                 aos::Fetcher<CameraImage> *image_fetcher,
+                 aos::Fetcher<AprilDebug> *april_debug_fetcher) {
   const CameraImage *image;
+  std::optional<const AprilDebug *> april_debug = std::nullopt;
 
   // Read next image
-  if (!image_fetcher.Fetch()) {
+  if (!image_fetcher->Fetch()) {
     VLOG(2) << "Couldn't fetch next image";
     return true;
   }
-
-  image = image_fetcher.get();
+  image = image_fetcher->get();
   CHECK(image != nullptr) << "Couldn't read image";
 
+  if (april_debug_fetcher->Fetch()) {
+    april_debug = april_debug_fetcher->get();
+  } else {
+    VLOG(2) << "Couldn't fetch next target map";
+  }
+
   // Create color image:
   cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
                           (void *)image->data()->data());
@@ -41,7 +55,8 @@
 
   if (!FLAGS_capture.empty()) {
     if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
-      aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+      aos::WriteFlatbufferToFile(FLAGS_capture,
+                                 image_fetcher->CopyFlatBuffer());
     } else {
       cv::imwrite(FLAGS_capture, bgr_image);
     }
@@ -49,7 +64,21 @@
     return false;
   }
 
-  cv::imshow("Display", bgr_image);
+  cv::Mat undistorted_image;
+  cv::undistort(bgr_image, undistorted_image, intrinsics, dist_coeffs);
+
+  if (april_debug.has_value() && april_debug.value()->corners()->size() > 0) {
+    for (const auto *corners : *april_debug.value()->corners()) {
+      std::vector<cv::Point> points;
+      for (const auto *point_fbs : *corners->points()) {
+        points.emplace_back(point_fbs->x(), point_fbs->y());
+      }
+      cv::polylines(undistorted_image, points, true, cv::Scalar(255, 0, 0), 10);
+    }
+  }
+
+  cv::imshow("Display", undistorted_image);
+
   int keystroke = cv::waitKey(1);
   if ((keystroke & 0xFF) == static_cast<int>('c')) {
     // Convert again, to get clean image
@@ -68,14 +97,26 @@
   aos::FlatbufferDetachedBuffer<aos::Configuration> config =
       aos::configuration::ReadConfig(FLAGS_config);
 
+  frc971::constants::WaitForConstants<Constants>(&config.message());
+
   aos::ShmEventLoop event_loop(&config.message());
 
-  image_fetcher = event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+  frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+  const auto *calibration_data = FindCameraCalibration(
+      constants_fetcher.constants(), event_loop.node()->name()->string_view());
+  const cv::Mat intrinsics = CameraIntrinsics(calibration_data);
+  const cv::Mat dist_coeffs = CameraDistCoeffs(calibration_data);
+
+  aos::Fetcher<CameraImage> image_fetcher =
+      event_loop.MakeFetcher<CameraImage>(FLAGS_channel);
+  aos::Fetcher<AprilDebug> april_debug_fetcher =
+      event_loop.MakeFetcher<AprilDebug>("/camera");
 
   // Run the display loop
   event_loop.AddPhasedLoop(
-      [&event_loop](int) {
-        if (!DisplayLoop()) {
+      [&](int) {
+        if (!DisplayLoop(intrinsics, dist_coeffs, &image_fetcher,
+                         &april_debug_fetcher)) {
           LOG(INFO) << "Calling event_loop Exit";
           event_loop.Exit();
         };
@@ -89,9 +130,9 @@
 
 }  // namespace
 }  // namespace vision
-}  // namespace frc971
+}  // namespace y2023
 
 int main(int argc, char **argv) {
   aos::InitGoogle(&argc, &argv);
-  frc971::vision::ViewerMain();
+  y2023::vision::ViewerMain();
 }