diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
new file mode 100644
index 0000000..15f1f4c
--- /dev/null
+++ b/y2020/vision/calibration.cc
@@ -0,0 +1,322 @@
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include <opencv2/aruco/charuco.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "absl/strings/str_format.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2020/vision/vision_generated.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_string(pi, "pi-7971-1", "Pi name to calibrate.");
+DEFINE_string(calibration_folder, "y2020/vision/tools/python_code/calib_files",
+              "Folder to place calibration files.");
+DEFINE_bool(large_board, true, "If true, use the large calibration board.");
+DEFINE_bool(display_undistorted, false,
+            "If true, display the undistorted image.");
+DEFINE_string(board_template_path, "",
+              "If specified, write an image to the specified path for the "
+              "charuco board pattern.");
+DEFINE_uint32(min_targets, 10,
+              "The mininum number of targets required to match.");
+
+namespace frc971 {
+namespace vision {
+
+class CameraCalibration {
+ public:
+  CameraCalibration(const std::string_view training_data_bfbs) {
+    const sift::TrainingData *const training_data =
+        flatbuffers::GetRoot<sift::TrainingData>(training_data_bfbs.data());
+    {
+      flatbuffers::Verifier verifier(
+          reinterpret_cast<const uint8_t *>(training_data_bfbs.data()),
+          training_data_bfbs.size());
+      CHECK(training_data->Verify(verifier));
+    }
+
+    camera_calibration_ = FindCameraCalibration(training_data);
+  }
+
+  cv::Mat CameraIntrinsics() const {
+    const cv::Mat result(3, 3, CV_32F,
+                         const_cast<void *>(static_cast<const void *>(
+                             camera_calibration_->intrinsics()->data())));
+    CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
+    return result;
+  }
+
+  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;
+  }
+
+ private:
+  const sift::CameraCalibration *FindCameraCalibration(
+      const sift::TrainingData *const training_data) const {
+    std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+    std::optional<uint16_t> team_number =
+        aos::network::team_number_internal::ParsePiTeamNumber(FLAGS_pi);
+    CHECK(pi_number);
+    CHECK(team_number);
+    const std::string node_name = absl::StrFormat("pi%d", pi_number.value());
+    LOG(INFO) << "Looking for node name " << node_name << " team number "
+              << team_number.value();
+    for (const sift::CameraCalibration *candidate :
+         *training_data->camera_calibrations()) {
+      if (candidate->node_name()->string_view() != node_name) {
+        continue;
+      }
+      if (candidate->team_number() != team_number.value()) {
+        continue;
+      }
+      return candidate;
+    }
+    LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+               << " on " << team_number.value();
+  }
+
+  const sift::CameraCalibration *camera_calibration_;
+};
+
+namespace {
+
+
+void ViewerMain() {
+  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+      aos::configuration::ReadConfig(FLAGS_config);
+
+  aos::ShmEventLoop event_loop(&config.message());
+
+  const CameraCalibration calibration(SiftTrainingData());
+
+  cv::Ptr<cv::aruco::Dictionary> dictionary =
+      cv::aruco::getPredefinedDictionary(FLAGS_large_board
+                                             ? cv::aruco::DICT_5X5_100
+                                             : cv::aruco::DICT_6X6_250);
+  cv::Ptr<cv::aruco::CharucoBoard> board =
+      FLAGS_large_board
+          ? cv::aruco::CharucoBoard::create(12, 9, 0.06, 0.045, dictionary)
+          : cv::aruco::CharucoBoard::create(7, 5, 0.04, 0.025, dictionary);
+
+  if (!FLAGS_board_template_path.empty()) {
+    cv::Mat board_image;
+    board->draw(cv::Size(600, 500), board_image, 10, 1);
+    cv::imwrite(FLAGS_board_template_path, board_image);
+  }
+
+  std::vector<std::vector<int>> all_charuco_ids;
+  std::vector<std::vector<cv::Point2f>> all_charuco_corners;
+
+  const cv::Mat camera_matrix = calibration.CameraIntrinsics();
+  Eigen::Matrix3d eigen_camera_matrix;
+  cv::cv2eigen(camera_matrix, eigen_camera_matrix);
+
+  const cv::Mat dist_coeffs = calibration.CameraDistCoeffs();
+  LOG(INFO) << "Camera matrix " << camera_matrix;
+  LOG(INFO) << "Distortion Coefficients " << dist_coeffs;
+
+  std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+  CHECK(pi_number) << ": Invalid pi number " << FLAGS_pi
+                   << ", failed to parse pi number";
+
+  const std::string channel =
+      absl::StrCat("/pi", std::to_string(pi_number.value()), "/camera");
+  LOG(INFO) << "Connecting to channel " << channel;
+
+  event_loop.MakeWatcher(
+      channel, [&event_loop, &board, &all_charuco_ids, &all_charuco_corners,
+                camera_matrix, dist_coeffs,
+                eigen_camera_matrix](const CameraImage &image) {
+        const aos::monotonic_clock::duration age =
+            event_loop.monotonic_now() -
+            event_loop.context().monotonic_event_time;
+        const double age_double =
+            std::chrono::duration_cast<std::chrono::duration<double>>(age)
+                .count();
+        if (age > std::chrono::milliseconds(100)) {
+          LOG(INFO) << "Age: " << age_double << ", getting behind, skipping";
+          return;
+        }
+        // Create color image:
+        cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+                                (void *)image.data()->data());
+        const cv::Size image_size(image.cols(), image.rows());
+        cv::Mat rgb_image(image_size, CV_8UC3);
+        cv::cvtColor(image_color_mat, rgb_image, CV_YUV2BGR_YUYV);
+
+        std::vector<int> marker_ids;
+        std::vector<std::vector<cv::Point2f>> marker_corners;
+
+        cv::aruco::detectMarkers(rgb_image, board->dictionary, marker_corners,
+                                 marker_ids);
+
+        std::vector<cv::Point2f> charuco_corners;
+        std::vector<int> charuco_ids;
+        // If at least one marker detected
+        if (marker_ids.size() >= FLAGS_min_targets) {
+          // Run everything twice, once with the calibration, and once without.
+          // This lets us both calibrate, and also print out the pose real time
+          // with the previous calibration.
+          cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids,
+                                               rgb_image, board,
+                                               charuco_corners, charuco_ids);
+
+          std::vector<cv::Point2f> charuco_corners_with_calibration;
+          std::vector<int> charuco_ids_with_calibration;
+
+          cv::aruco::interpolateCornersCharuco(
+              marker_corners, marker_ids, rgb_image, board,
+              charuco_corners_with_calibration, charuco_ids_with_calibration,
+              camera_matrix, dist_coeffs);
+
+          cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
+
+          if (charuco_ids.size() >= FLAGS_min_targets) {
+            cv::aruco::drawDetectedCornersCharuco(
+                rgb_image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0));
+
+            cv::Vec3d rvec, tvec;
+            bool valid = cv::aruco::estimatePoseCharucoBoard(
+                charuco_corners_with_calibration, charuco_ids_with_calibration,
+                board, camera_matrix, dist_coeffs, rvec, tvec);
+
+
+            // if charuco pose is valid
+            if (valid) {
+              LOG(INFO) << std::fixed << std::setprecision(6)
+                        << "Age: " << age_double << ", Pose is R:" << rvec
+                        << " T:" << tvec;
+              cv::aruco::drawAxis(rgb_image, camera_matrix,
+                                  dist_coeffs, rvec, tvec, 0.1);
+            } else {
+              LOG(INFO) << "Age: " << age_double << ", invalid pose";
+            }
+          } else {
+            LOG(INFO) << "Age: " << age_double << ", no charuco IDs.";
+          }
+        } else {
+          LOG(INFO) << "Age: " << age_double << ", no marker IDs";
+        }
+
+        cv::imshow("Display", rgb_image);
+
+        if (FLAGS_display_undistorted) {
+          cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
+          cv::undistort(rgb_image, undistorted_rgb_image, camera_matrix,
+                        dist_coeffs);
+
+          cv::imshow("Display undist", undistorted_rgb_image);
+        }
+
+        int keystroke = cv::waitKey(1);
+        if ((keystroke & 0xFF) == static_cast<int>('c')) {
+          if (charuco_ids.size() >= FLAGS_min_targets) {
+            all_charuco_ids.emplace_back(std::move(charuco_ids));
+            all_charuco_corners.emplace_back(std::move(charuco_corners));
+            LOG(INFO) << "Image " << all_charuco_corners.size();
+          }
+
+          if (all_charuco_ids.size() >= 50) {
+            LOG(INFO) << "Got enough images to calibrate";
+            event_loop.Exit();
+          }
+        } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+          event_loop.Exit();
+        }
+      });
+
+  event_loop.Run();
+
+  if (all_charuco_ids.size() >= 50) {
+    cv::Mat cameraMatrix, distCoeffs;
+    std::vector<cv::Mat> rvecs, tvecs;
+    cv::Mat stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors;
+
+    // Set calibration flags (same than in calibrateCamera() function)
+    int calibration_flags = 0;
+    cv::Size img_size(640, 480);
+    const double reprojection_error = cv::aruco::calibrateCameraCharuco(
+        all_charuco_corners, all_charuco_ids, board, img_size, cameraMatrix,
+        distCoeffs, rvecs, tvecs, stdDeviationsIntrinsics,
+        stdDeviationsExtrinsics, perViewErrors, calibration_flags);
+    CHECK_LE(reprojection_error, 1.0) << ": Reproduction error is bad.";
+    LOG(INFO) << "Reprojection Error is " << reprojection_error;
+
+    flatbuffers::FlatBufferBuilder fbb;
+    flatbuffers::Offset<flatbuffers::String> name_offset =
+        fbb.CreateString(absl::StrFormat("pi%d", pi_number.value()));
+    flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
+        fbb.CreateVector<float>(9u, [&cameraMatrix](size_t i) {
+          return static_cast<float>(
+              reinterpret_cast<double *>(cameraMatrix.data)[i]);
+        });
+
+    flatbuffers::Offset<flatbuffers::Vector<float>>
+        distortion_coefficients_offset =
+            fbb.CreateVector<float>(5u, [&distCoeffs](size_t i) {
+              return static_cast<float>(
+                  reinterpret_cast<double *>(distCoeffs.data)[i]);
+            });
+
+    sift::CameraCalibration::Builder camera_calibration_builder(fbb);
+    std::optional<uint16_t> team_number =
+        aos::network::team_number_internal::ParsePiTeamNumber(FLAGS_pi);
+    CHECK(team_number) << ": Invalid pi hostname " << FLAGS_pi
+                       << ", failed to parse team number";
+
+    const aos::realtime_clock::time_point realtime_now =
+        aos::realtime_clock::now();
+    camera_calibration_builder.add_node_name(name_offset);
+    camera_calibration_builder.add_team_number(team_number.value());
+    camera_calibration_builder.add_calibration_timestamp(
+        realtime_now.time_since_epoch().count());
+    camera_calibration_builder.add_intrinsics(intrinsics_offset);
+    camera_calibration_builder.add_dist_coeffs(distortion_coefficients_offset);
+    camera_calibration_builder.add_dist_coeffs(distortion_coefficients_offset);
+    fbb.Finish(camera_calibration_builder.Finish());
+
+    aos::FlatbufferDetachedBuffer<sift::CameraCalibration> camera_calibration(
+        fbb.Release());
+    std::stringstream time_ss;
+    time_ss << realtime_now;
+
+    const std::string calibration_filename =
+        FLAGS_calibration_folder +
+        absl::StrFormat("/calibration_pi-%d-%d_%s.json", team_number.value(),
+                        pi_number.value(), time_ss.str());
+
+    LOG(INFO) << calibration_filename << " -> "
+              << aos::FlatbufferToJson(camera_calibration, true);
+
+    aos::util::WriteStringToFileOrDie(
+        calibration_filename, aos::FlatbufferToJson(camera_calibration, true));
+  } else {
+    LOG(INFO) << "Skipping calibration due to not enough images.";
+  }
+}
+
+}  // namespace
+}  // namespace vision
+}  // namespace frc971
+
+// Quick and lightweight grayscale viewer for images
+int main(int argc, char **argv) {
+  aos::InitGoogle(&argc, &argv);
+  frc971::vision::ViewerMain();
+}