diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index 8125e5e..f1ab4fc 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -1,16 +1,17 @@
 #include "frc971/vision/calibration_accumulator.h"
 
+#include <algorithm>
+#include <limits>
+#include <opencv2/highgui/highgui.hpp>
+
 #include "Eigen/Dense"
 #include "aos/events/simulated_event_loop.h"
+#include "aos/network/team_number.h"
 #include "aos/time/time.h"
 #include "frc971/control_loops/quaternion_utils.h"
 #include "frc971/vision/charuco_lib.h"
 #include "frc971/wpilib/imu_batch_generated.h"
 
-#include <algorithm>
-#include <limits>
-#include <opencv2/highgui/highgui.hpp>
-
 DEFINE_bool(display_undistorted, false,
             "If true, display the undistorted image.");
 DEFINE_string(save_path, "", "Where to store annotated images");
@@ -123,11 +124,20 @@
       charuco_extractor_(
           image_event_loop_, pi,
           [this](cv::Mat rgb_image, monotonic_clock::time_point eof,
-                 std::vector<int> charuco_ids,
-                 std::vector<cv::Point2f> charuco_corners, bool valid,
-                 Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
+                 std::vector<cv::Vec4i> charuco_ids,
+                 std::vector<std::vector<cv::Point2f>> charuco_corners,
+                 bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+                 std::vector<Eigen::Vector3d> tvecs_eigen) {
             HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
-                          rvec_eigen, tvec_eigen);
+                          rvecs_eigen, tvecs_eigen);
+          }),
+      image_callback_(
+          image_event_loop_,
+          absl::StrCat("/pi",
+                       std::to_string(aos::network::ParsePiNumber(pi).value()),
+                       "/camera"),
+          [this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
+            charuco_extractor_.HandleImage(rgb_image, eof);
           }),
       data_(data) {
   imu_factory_->OnShutdown([]() { cv::destroyAllWindows(); });
@@ -159,15 +169,17 @@
       });
 }
 
-void Calibration::HandleCharuco(cv::Mat rgb_image,
-                                const monotonic_clock::time_point eof,
-                                std::vector<int> /*charuco_ids*/,
-                                std::vector<cv::Point2f> /*charuco_corners*/,
-                                bool valid, Eigen::Vector3d rvec_eigen,
-                                Eigen::Vector3d tvec_eigen) {
+void Calibration::HandleCharuco(
+    cv::Mat rgb_image, const monotonic_clock::time_point eof,
+    std::vector<cv::Vec4i> /*charuco_ids*/,
+    std::vector<std::vector<cv::Point2f>> /*charuco_corners*/, bool valid,
+    std::vector<Eigen::Vector3d> rvecs_eigen,
+    std::vector<Eigen::Vector3d> tvecs_eigen) {
   if (valid) {
-    data_->AddCameraPose(image_factory_->ToDistributedClock(eof), rvec_eigen,
-                         tvec_eigen);
+    CHECK(rvecs_eigen.size() > 0) << "Require at least one target detected";
+    // We only use one (the first) target detected for calibration
+    data_->AddCameraPose(image_factory_->ToDistributedClock(eof),
+                         rvecs_eigen[0], tvecs_eigen[0]);
 
     Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
                              "[", "]");
@@ -177,20 +189,23 @@
             image_event_loop_->monotonic_now() - eof)
             .count();
     VLOG(1) << std::fixed << std::setprecision(6) << "Age: " << age_double
-            << ", Pose is R:" << rvec_eigen.transpose().format(HeavyFmt)
-            << "\nT:" << tvec_eigen.transpose().format(HeavyFmt);
+            << ", Pose is R:" << rvecs_eigen[0].transpose().format(HeavyFmt)
+            << "\nT:" << tvecs_eigen[0].transpose().format(HeavyFmt);
   }
 
-  cv::imshow("Display", rgb_image);
+  if (FLAGS_visualize) {
+    if (FLAGS_display_undistorted) {
+      const cv::Size image_size(rgb_image.cols, rgb_image.rows);
+      cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
+      cv::undistort(rgb_image, undistorted_rgb_image,
+                    charuco_extractor_.camera_matrix(),
+                    charuco_extractor_.dist_coeffs());
 
-  if (FLAGS_display_undistorted) {
-    const cv::Size image_size(rgb_image.cols, rgb_image.rows);
-    cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
-    cv::undistort(rgb_image, undistorted_rgb_image,
-                  charuco_extractor_.camera_matrix(),
-                  charuco_extractor_.dist_coeffs());
+      cv::imshow("Display undist", undistorted_rgb_image);
+    }
 
-    cv::imshow("Display undist", undistorted_rgb_image);
+    cv::imshow("Display", rgb_image);
+    cv::waitKey(1);
   }
 
   if (FLAGS_save_path != "") {
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index cb5609b..d21f4c6 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -84,14 +84,18 @@
               aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
               std::string_view pi, CalibrationData *data);
 
-  // Processes a charuco detection.
+  // Processes a charuco detection that is returned from charuco_lib.
+  // For a valid detection(s), it stores camera observation
+  // Also optionally displays and saves annotated images based on visualize and
+  // save_path flags, respectively
   void HandleCharuco(cv::Mat rgb_image,
                      const aos::monotonic_clock::time_point eof,
-                     std::vector<int> /*charuco_ids*/,
-                     std::vector<cv::Point2f> /*charuco_corners*/, bool valid,
-                     Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen);
+                     std::vector<cv::Vec4i> /*charuco_ids*/,
+                     std::vector<std::vector<cv::Point2f>> /*charuco_corners*/,
+                     bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+                     std::vector<Eigen::Vector3d> tvecs_eigen);
 
-  // Processes an IMU reading.
+  // Processes an IMU reading by storing for later processing
   void HandleIMU(const frc971::IMUValues *imu);
 
  private:
@@ -101,6 +105,7 @@
   aos::NodeEventLoopFactory *imu_factory_;
 
   CharucoExtractor charuco_extractor_;
+  ImageCallback image_callback_;
 
   CalibrationData *data_;
 
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 1398af5..0db571f 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -17,13 +17,17 @@
 #include "y2020/vision/sift/sift_training_generated.h"
 #include "y2020/vision/tools/python_code/sift_training_data.h"
 
-DEFINE_uint32(min_targets, 10,
-              "The mininum number of targets required to match.");
-DEFINE_bool(large_board, true, "If true, use the large calibration board.");
-DEFINE_bool(coarse_pattern, true, "If true, use coarse arucos; else, use fine");
 DEFINE_string(board_template_path, "",
               "If specified, write an image to the specified path for the "
               "charuco board pattern.");
+DEFINE_bool(coarse_pattern, true, "If true, use coarse arucos; else, use fine");
+DEFINE_bool(large_board, true, "If true, use the large calibration board.");
+DEFINE_uint32(
+    min_charucos, 10,
+    "The mininum number of aruco targets in charuco board required to match.");
+DEFINE_string(target_type, "charuco",
+              "Type of target: april_tag|aruco|charuco|charuco_diamond");
+DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
 
 namespace frc971 {
 namespace vision {
@@ -87,7 +91,8 @@
 
 ImageCallback::ImageCallback(
     aos::EventLoop *event_loop, std::string_view channel,
-    std::function<void(cv::Mat, monotonic_clock::time_point)> &&fn)
+    std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn)
+
     : event_loop_(event_loop),
       server_fetcher_(
           event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
@@ -97,7 +102,7 @@
           event_loop_->GetChannel<CameraImage>(channel)
               ->source_node()
               ->string_view())),
-      handle_image_(std::move(fn)) {
+      handle_image_(std::move(handle_image_fn)) {
   event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
     const monotonic_clock::time_point eof_source_node =
         monotonic_clock::time_point(
@@ -150,47 +155,142 @@
   });
 }
 
+void CharucoExtractor::SetupTargetData() {
+  // TODO(Jim): Put correct values here
+  marker_length_ = 0.15;
+  square_length_ = 0.1651;
+
+  // Only charuco board has a board associated with it
+  board_ = static_cast<cv::Ptr<cv::aruco::CharucoBoard>>(NULL);
+
+  if (FLAGS_target_type == "charuco" || FLAGS_target_type == "aruco") {
+    dictionary_ = cv::aruco::getPredefinedDictionary(
+        FLAGS_large_board ? cv::aruco::DICT_5X5_250 : cv::aruco::DICT_6X6_250);
+
+    if (FLAGS_target_type == "charuco") {
+      LOG(INFO) << "Using " << (FLAGS_large_board ? " large " : " small ")
+                << " charuco board with "
+                << (FLAGS_coarse_pattern ? "coarse" : "fine") << " pattern";
+      board_ =
+          (FLAGS_large_board
+               ? (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
+                                             12, 9, 0.06, 0.04666, dictionary_)
+                                       : cv::aruco::CharucoBoard::create(
+                                             25, 18, 0.03, 0.0233, dictionary_))
+               : (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
+                                             7, 5, 0.04, 0.025, dictionary_)
+                                       // TODO(jim): Need to figure out what
+                                       // size is for small board, fine pattern
+                                       : cv::aruco::CharucoBoard::create(
+                                             7, 5, 0.03, 0.0233, 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);
+      }
+    }
+  } else if (FLAGS_target_type == "charuco_diamond") {
+    // TODO<Jim>: Measure this
+    marker_length_ = 0.15;
+    square_length_ = 0.1651;
+    dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
+  } else if (FLAGS_target_type == "april_tag") {
+    // Current printout is supposed to be 200mm
+    // TODO<Jim>: Verify this
+    square_length_ = 0.2;
+    dictionary_ =
+        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_36h11);
+  } else {
+    // Bail out if it's not a supported target
+    LOG(FATAL) << "Target type undefined: " << FLAGS_target_type
+               << " vs. april_tag|aruco|charuco|charuco_diamond";
+  }
+}
+
+void CharucoExtractor::DrawTargetPoses(cv::Mat rgb_image,
+                                       std::vector<cv::Vec3d> rvecs,
+                                       std::vector<cv::Vec3d> tvecs) {
+  const Eigen::Matrix<double, 3, 4> camera_projection =
+      Eigen::Matrix<double, 3, 4>::Identity();
+
+  int x_coord = 10;
+  int y_coord = 0;
+  // draw axis for each marker
+  for (uint i = 0; i < rvecs.size(); i++) {
+    Eigen::Vector3d rvec_eigen, tvec_eigen;
+    cv::cv2eigen(rvecs[i], rvec_eigen);
+    cv::cv2eigen(tvecs[i], tvec_eigen);
+
+    Eigen::Quaternion<double> rotation(
+        frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
+    Eigen::Translation3d translation(tvec_eigen);
+
+    const Eigen::Affine3d board_to_camera = translation * rotation;
+
+    Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
+                             board_to_camera * Eigen::Vector3d::Zero();
+
+    // Found that drawAxis hangs if you try to draw with z values too
+    // small (trying to draw axes at inifinity)
+    // TODO<Jim>: Explore what real thresholds for this should be;
+    // likely Don't need to get rid of negative values
+    if (result.z() < 0.01) {
+      LOG(INFO) << "Skipping, due to z value too small: " << result.z();
+    } else {
+      result /= result.z();
+      if (FLAGS_target_type == "charuco") {
+        cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
+                            tvecs[i], 0.1);
+      } else {
+        cv::drawFrameAxes(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
+                          tvecs[i], 0.1);
+      }
+    }
+    std::stringstream ss;
+    ss << "tvec[" << i << "] = " << tvecs[i];
+    y_coord += 25;
+    cv::putText(rgb_image, ss.str(), cv::Point(x_coord, y_coord),
+                cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
+    ss.str("");
+    ss << "rvec[" << i << "] = " << rvecs[i];
+    y_coord += 25;
+    cv::putText(rgb_image, ss.str(), cv::Point(x_coord, y_coord),
+                cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
+  }
+}
+
+void CharucoExtractor::PackPoseResults(
+    std::vector<cv::Vec3d> &rvecs, std::vector<cv::Vec3d> &tvecs,
+    std::vector<Eigen::Vector3d> *rvecs_eigen,
+    std::vector<Eigen::Vector3d> *tvecs_eigen) {
+  for (cv::Vec3d rvec : rvecs) {
+    Eigen::Vector3d rvec_eigen = Eigen::Vector3d::Zero();
+    cv::cv2eigen(rvec, rvec_eigen);
+    rvecs_eigen->emplace_back(rvec_eigen);
+  }
+
+  for (cv::Vec3d tvec : tvecs) {
+    Eigen::Vector3d tvec_eigen = Eigen::Vector3d::Zero();
+    cv::cv2eigen(tvec, tvec_eigen);
+    tvecs_eigen->emplace_back(tvec_eigen);
+  }
+}
+
 CharucoExtractor::CharucoExtractor(
     aos::EventLoop *event_loop, std::string_view pi,
-    std::function<void(cv::Mat, monotonic_clock::time_point, std::vector<int>,
-                       std::vector<cv::Point2f>, bool, Eigen::Vector3d,
-                       Eigen::Vector3d)> &&fn)
+    std::function<void(cv::Mat, monotonic_clock::time_point,
+                       std::vector<cv::Vec4i>,
+                       std::vector<std::vector<cv::Point2f>>, bool,
+                       std::vector<Eigen::Vector3d>,
+                       std::vector<Eigen::Vector3d>)> &&handle_charuco_fn)
     : event_loop_(event_loop),
       calibration_(SiftTrainingData(), pi),
-      dictionary_(cv::aruco::getPredefinedDictionary(
-          FLAGS_large_board ? cv::aruco::DICT_5X5_250
-                            : cv::aruco::DICT_6X6_250)),
-      board_(
-          FLAGS_large_board
-              ? (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
-                                            12, 9, 0.06, 0.04666, dictionary_)
-                                      : cv::aruco::CharucoBoard::create(
-                                            25, 18, 0.03, 0.0233, dictionary_))
-              : (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
-                                            7, 5, 0.04, 0.025, dictionary_)
-                                      // TODO(jim): Need to figure out what size
-                                      // is for small board, fine pattern
-                                      : cv::aruco::CharucoBoard::create(
-                                            7, 5, 0.03, 0.0233, dictionary_))),
       camera_matrix_(calibration_.CameraIntrinsics()),
       eigen_camera_matrix_(calibration_.CameraIntrinsicsEigen()),
       dist_coeffs_(calibration_.CameraDistCoeffs()),
       pi_number_(aos::network::ParsePiNumber(pi)),
-      image_callback_(
-          event_loop,
-          absl::StrCat("/pi", std::to_string(pi_number_.value()), "/camera"),
-          [this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
-            HandleImage(rgb_image, eof);
-          }),
-      handle_charuco_(std::move(fn)) {
-  LOG(INFO) << "Using " << (FLAGS_large_board ? "large" : "small")
-            << " board with " << (FLAGS_coarse_pattern ? "coarse" : "fine")
-            << " pattern";
-  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);
-  }
+      handle_charuco_(std::move(handle_charuco_fn)) {
+  SetupTargetData();
 
   LOG(INFO) << "Camera matrix " << camera_matrix_;
   LOG(INFO) << "Distortion Coefficients " << dist_coeffs_;
@@ -207,99 +307,143 @@
       std::chrono::duration_cast<std::chrono::duration<double>>(
           event_loop_->monotonic_now() - eof)
           .count();
+
+  // Set up the variables we'll use in the callback function
+  bool valid = false;
+  // Return a list of poses; for Charuco Board there will be just one
+  std::vector<Eigen::Vector3d> rvecs_eigen;
+  std::vector<Eigen::Vector3d> tvecs_eigen;
+
+  // ids and corners for initial aruco marker detections
   std::vector<int> marker_ids;
   std::vector<std::vector<cv::Point2f>> marker_corners;
 
-  cv::aruco::detectMarkers(rgb_image, board_->dictionary, marker_corners,
-                           marker_ids);
+  // ids and corners for final, refined board / marker detections
+  // Using Vec4i type since it supports Charuco Diamonds
+  // And overloading it using 1st int in Vec4i for others target types
+  std::vector<cv::Vec4i> result_ids;
+  std::vector<std::vector<cv::Point2f>> result_corners;
 
-  std::vector<cv::Point2f> charuco_corners;
-  std::vector<int> charuco_ids;
-  bool valid = false;
-  Eigen::Vector3d rvec_eigen = Eigen::Vector3d::Zero();
-  Eigen::Vector3d tvec_eigen = Eigen::Vector3d::Zero();
+  // Do initial marker detection; this is the same for all target types
+  cv::aruco::detectMarkers(rgb_image, dictionary_, marker_corners, marker_ids);
+  cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_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);
+  VLOG(2) << "Handle Image, with target type = " << FLAGS_target_type << " and "
+          << marker_ids.size() << " markers detected initially";
 
-    std::vector<cv::Point2f> charuco_corners_with_calibration;
-    std::vector<int> charuco_ids_with_calibration;
+  if (marker_ids.size() == 0) {
+    VLOG(2) << "Didn't find any markers";
+  } else {
+    if (FLAGS_target_type == "charuco") {
+      std::vector<int> charuco_ids;
+      std::vector<cv::Point2f> charuco_corners;
 
-    cv::aruco::interpolateCornersCharuco(
-        marker_corners, marker_ids, rgb_image, board_,
-        charuco_corners_with_calibration, charuco_ids_with_calibration,
-        camera_matrix_, dist_coeffs_);
+      // If enough aruco markers detected for the Charuco board
+      if (marker_ids.size() >= FLAGS_min_charucos) {
+        // Run everything twice, once with the calibration, and once
+        // without. This lets us both collect data to calibrate the
+        // intrinsics of the camera (to determine the intrinsics from
+        // multiple samples), and also to use data from a previous/stored
+        // calibration to determine a more accurate pose in real time (used
+        // for extrinsics calibration)
+        cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids,
+                                             rgb_image, board_, charuco_corners,
+                                             charuco_ids);
 
-    cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
+        std::vector<cv::Point2f> charuco_corners_with_calibration;
+        std::vector<int> charuco_ids_with_calibration;
 
-    if (charuco_ids.size() >= FLAGS_min_targets) {
-      cv::aruco::drawDetectedCornersCharuco(rgb_image, charuco_corners,
-                                            charuco_ids, cv::Scalar(255, 0, 0));
+        // This call uses a previous intrinsic calibration to get more
+        // accurate marker locations, for a better pose estimate
+        cv::aruco::interpolateCornersCharuco(
+            marker_corners, marker_ids, rgb_image, board_,
+            charuco_corners_with_calibration, charuco_ids_with_calibration,
+            camera_matrix_, dist_coeffs_);
 
-      cv::Vec3d rvec, tvec;
-      valid = cv::aruco::estimatePoseCharucoBoard(
-          charuco_corners_with_calibration, charuco_ids_with_calibration,
-          board_, camera_matrix_, dist_coeffs_, rvec, tvec);
+        if (charuco_ids.size() >= FLAGS_min_charucos) {
+          cv::aruco::drawDetectedCornersCharuco(
+              rgb_image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0));
 
-      // if charuco pose is valid
-      if (valid) {
-        cv::cv2eigen(rvec, rvec_eigen);
-        cv::cv2eigen(tvec, tvec_eigen);
+          cv::Vec3d rvec, tvec;
+          valid = cv::aruco::estimatePoseCharucoBoard(
+              charuco_corners_with_calibration, charuco_ids_with_calibration,
+              board_, camera_matrix_, dist_coeffs_, rvec, tvec);
 
-        Eigen::Quaternion<double> rotation(
-            frc971::controls::ToQuaternionFromRotationVector(rvec_eigen));
-        Eigen::Translation3d translation(tvec_eigen);
+          // if charuco pose is valid, return pose, with ids and corners
+          if (valid) {
+            std::vector<cv::Vec3d> rvecs, tvecs;
+            rvecs.emplace_back(rvec);
+            tvecs.emplace_back(tvec);
+            DrawTargetPoses(rgb_image, rvecs, tvecs);
 
-        const Eigen::Affine3d board_to_camera = translation * rotation;
-
-        Eigen::Matrix<double, 3, 4> camera_projection =
-            Eigen::Matrix<double, 3, 4>::Identity();
-        Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
-                                 board_to_camera * Eigen::Vector3d::Zero();
-
-        // Found that drawAxis hangs if you try to draw with z values too small
-        // (trying to draw axes at inifinity)
-        // TODO<Jim>: Explore what real thresholds for this should be; likely
-        // Don't need to get rid of negative values
-        if (result.z() < 0.01) {
-          LOG(INFO) << "Skipping, due to z value too small: " << result.z();
-          valid = false;
+            PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
+            // Store the corners without calibration, since we use them to
+            // do calibration
+            result_corners.emplace_back(charuco_corners);
+            for (auto id : charuco_ids) {
+              result_ids.emplace_back(cv::Vec4i{id, 0, 0, 0});
+            }
+          } else {
+            VLOG(2) << "Age: " << age_double << ", invalid charuco board pose";
+          }
         } else {
-          result /= result.z();
-          cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
-                     cv::Scalar(255, 255, 255), 0, cv::LINE_8);
-
-          cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec,
-                              tvec, 0.1);
+          VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "
+                  << charuco_ids.size() << ", needed " << FLAGS_min_charucos;
         }
-        std::stringstream ss;
-        ss << "tvec = " << tvec << "; |t| = " << tvec_eigen.norm();
-        cv::putText(rgb_image, ss.str(), cv::Point(10, 25),
-                    cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
-        ss.str("");
-        ss << "rvec = " << rvec;
-        cv::putText(rgb_image, ss.str(), cv::Point(10, 50),
-                    cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
       } else {
-        VLOG(2) << "Age: " << age_double << ", invalid pose";
+        VLOG(2) << "Age: " << age_double
+                << ", not enough marker IDs for charuco board, got "
+                << marker_ids.size() << ", needed " << FLAGS_min_charucos;
+      }
+    } else if (FLAGS_target_type == "april_tag" ||
+               FLAGS_target_type == "aruco") {
+      // estimate pose for april tags doesn't return valid, so marking true
+      valid = true;
+      std::vector<cv::Vec3d> rvecs, tvecs;
+      cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
+                                           camera_matrix_, dist_coeffs_, rvecs,
+                                           tvecs);
+      DrawTargetPoses(rgb_image, rvecs, tvecs);
+
+      PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
+      for (uint i = 0; i < marker_ids.size(); i++) {
+        result_ids.emplace_back(cv::Vec4i{marker_ids[i], 0, 0, 0});
+      }
+      result_corners = marker_corners;
+    } else if (FLAGS_target_type == "charuco_diamond") {
+      // Extract the diamonds associated with the markers
+      std::vector<cv::Vec4i> diamond_ids;
+      std::vector<std::vector<cv::Point2f>> diamond_corners;
+      cv::aruco::detectCharucoDiamond(rgb_image, marker_corners, marker_ids,
+                                      square_length_ / marker_length_,
+                                      diamond_corners, diamond_ids);
+
+      // Check to see if we found any diamond targets
+      if (diamond_ids.size() > 0) {
+        cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
+                                        diamond_ids);
+
+        // estimate pose for diamonds doesn't return valid, so marking true
+        valid = true;
+        std::vector<cv::Vec3d> rvecs, tvecs;
+        cv::aruco::estimatePoseSingleMarkers(diamond_corners, square_length_,
+                                             camera_matrix_, dist_coeffs_,
+                                             rvecs, tvecs);
+        DrawTargetPoses(rgb_image, rvecs, tvecs);
+
+        PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
+        result_ids = diamond_ids;
+        result_corners = diamond_corners;
+      } else {
+        VLOG(2) << "Found aruco markers, but no charuco diamond targets";
       }
     } else {
-      VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "
-              << charuco_ids.size() << ", needed " << FLAGS_min_targets;
+      LOG(FATAL) << "Unknown target type: " << FLAGS_target_type;
     }
-  } else {
-    VLOG(2) << "Age: " << age_double << ", not enough marker IDs, got "
-            << marker_ids.size() << ", needed " << FLAGS_min_targets;
-    cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
   }
 
-  handle_charuco_(rgb_image, eof, charuco_ids, charuco_corners, valid,
-                  rvec_eigen, tvec_eigen);
+  handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
+                  rvecs_eigen, tvecs_eigen);
 }
 
 }  // namespace vision
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index a54bfca..362bb7d 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -15,6 +15,9 @@
 #include "y2020/vision/sift/sift_generated.h"
 #include "y2020/vision/sift/sift_training_generated.h"
 
+DECLARE_bool(visualize);
+DECLARE_string(target_type);
+
 namespace frc971 {
 namespace vision {
 
@@ -41,14 +44,16 @@
   const sift::CameraCalibration *camera_calibration_;
 };
 
-// Class to call a function with a cv::Mat and age when an image shows up on the
-// provided channel.  This hides all the conversions and wrangling needed to
-// view the image.
+// Helper class to call a function with a cv::Mat and age when an image shows up
+// on the provided channel.  This hides all the conversions and wrangling needed
+// to view the image.
+// Can connect this with HandleImage function from CharucoExtrator for
+// full-service callback functionality
 class ImageCallback {
  public:
-  ImageCallback(
-      aos::EventLoop *event_loop, std::string_view channel,
-      std::function<void(cv::Mat, aos::monotonic_clock::time_point)> &&fn);
+  ImageCallback(aos::EventLoop *event_loop, std::string_view channel,
+                std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
+                    &&handle_image_fn);
 
  private:
   aos::EventLoop *event_loop_;
@@ -65,16 +70,25 @@
   //   cv::Mat -> image with overlays drawn on it.
   //   monotonic_clock::time_point -> Time on this node when this image was
   //                                  captured.
-  //   std::vector<int> -> charuco_ids
-  //   std::vector<cv::Point2f> -> charuco_corners
+  //   std::vector<Vec4i> -> target ids (aruco/april in first slot of Vec4i)
+  // NOTE: We use Vec4i since that stores the ids for the charuco diamond target
+  //   std::vector<std::vector<cv::Point2f>> -> charuco_corners
   //   bool -> true if rvec/tvec is valid.
-  //   Eigen::Vector3d -> rvec
-  //   Eigen::Vector3d -> tvec
+  //   std::vector<Eigen::Vector3d> -> rvec
+  //   std::vector<Eigen::Vector3d> -> tvec
+  // NOTE: we return as a vector since all but charuco boards could have
+  // multiple targets in an image; for charuco boards, there should be just one
+  // element
   CharucoExtractor(
       aos::EventLoop *event_loop, std::string_view pi,
       std::function<void(cv::Mat, aos::monotonic_clock::time_point,
-                         std::vector<int>, std::vector<cv::Point2f>, bool,
-                         Eigen::Vector3d, Eigen::Vector3d)> &&fn);
+                         std::vector<cv::Vec4i>,
+                         std::vector<std::vector<cv::Point2f>>, bool,
+                         std::vector<Eigen::Vector3d>,
+                         std::vector<Eigen::Vector3d>)> &&handle_charuco_fn);
+
+  // Handles the image by detecting the charuco board in it.
+  void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
 
   // Returns the aruco dictionary in use.
   cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
@@ -87,8 +101,20 @@
   const cv::Mat dist_coeffs() const { return dist_coeffs_; }
 
  private:
-  // Handles the image by detecting the charuco board in it.
-  void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
+  // Creates the dictionary, board, and other parameters for the appropriate
+  // (ch)aruco target
+  void SetupTargetData();
+
+  // Draw the axes from the pose(s) on the image
+  void DrawTargetPoses(cv::Mat rgb_image, std::vector<cv::Vec3d> rvecs,
+                       std::vector<cv::Vec3d> tvecs);
+
+  // Helper function to convert rotation (rvecs) and translation (tvecs) vectors
+  // into Eigen vectors and store in corresponding vectors
+  void PackPoseResults(std::vector<cv::Vec3d> &rvecs,
+                       std::vector<cv::Vec3d> &tvecs,
+                       std::vector<Eigen::Vector3d> *rvecs_eigen,
+                       std::vector<Eigen::Vector3d> *tvecs_eigen);
 
   aos::EventLoop *event_loop_;
   CameraCalibration calibration_;
@@ -96,18 +122,26 @@
   cv::Ptr<cv::aruco::Dictionary> dictionary_;
   cv::Ptr<cv::aruco::CharucoBoard> board_;
 
+  // Length of a side of the aruco marker
+  double marker_length_;
+  // Length of a side of the checkerboard squares (around the marker)
+  double square_length_;
+
+  // Intrinsic calibration matrix
   const cv::Mat camera_matrix_;
+  // Intrinsic calibration matrix as Eigen::Matrix3d
   const Eigen::Matrix3d eigen_camera_matrix_;
+  // Intrinsic distortion coefficients
   const cv::Mat dist_coeffs_;
 
+  // Index number of the raspberry pi
   const std::optional<uint16_t> pi_number_;
 
-  ImageCallback image_callback_;
-
   // Function to call.
-  std::function<void(cv::Mat, aos::monotonic_clock::time_point,
-                     std::vector<int>, std::vector<cv::Point2f>, bool,
-                     Eigen::Vector3d, Eigen::Vector3d)>
+  std::function<void(
+      cv::Mat, aos::monotonic_clock::time_point, std::vector<cv::Vec4i>,
+      std::vector<std::vector<cv::Point2f>>, bool, std::vector<Eigen::Vector3d>,
+      std::vector<Eigen::Vector3d>)>
       handle_charuco_;
 };
 
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index b5d1b32..c6bacdb 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -38,11 +38,21 @@
             event_loop, pi,
             [this](cv::Mat rgb_image,
                    const aos::monotonic_clock::time_point eof,
-                   std::vector<int> charuco_ids,
-                   std::vector<cv::Point2f> charuco_corners, bool valid,
-                   Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
+                   std::vector<cv::Vec4i> charuco_ids,
+                   std::vector<std::vector<cv::Point2f>> charuco_corners,
+                   bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+                   std::vector<Eigen::Vector3d> tvecs_eigen) {
               HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
-                            rvec_eigen, tvec_eigen);
+                            rvecs_eigen, tvecs_eigen);
+            }),
+        image_callback_(
+            event_loop,
+            absl::StrCat(
+                "/pi", std::to_string(aos::network::ParsePiNumber(pi).value()),
+                "/camera"),
+            [this](cv::Mat rgb_image,
+                   const aos::monotonic_clock::time_point eof) {
+              charuco_extractor_.HandleImage(rgb_image, eof);
             }) {
     CHECK(pi_number_) << ": Invalid pi number " << pi
                       << ", failed to parse pi number";
@@ -50,13 +60,16 @@
     CHECK(std::regex_match(camera_id_, re))
         << ": Invalid camera_id '" << camera_id_
         << "', should be of form YY-NN";
+    CHECK_EQ(FLAGS_target_type, "charuco")
+        << "Intrinsic calibration only works with Charuco board";
   }
 
   void HandleCharuco(cv::Mat rgb_image,
                      const aos::monotonic_clock::time_point /*eof*/,
-                     std::vector<int> charuco_ids,
-                     std::vector<cv::Point2f> charuco_corners, bool valid,
-                     Eigen::Vector3d rvec_eigen, Eigen::Vector3d tvec_eigen) {
+                     std::vector<cv::Vec4i> charuco_ids,
+                     std::vector<std::vector<cv::Point2f>> charuco_corners,
+                     bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+                     std::vector<Eigen::Vector3d> tvecs_eigen) {
     // Reduce resolution displayed on remote viewer to prevent lag
     cv::resize(rgb_image, rgb_image,
                cv::Size(rgb_image.cols / 2, rgb_image.rows / 2));
@@ -78,12 +91,17 @@
     if (!valid) {
       return;
     }
+    CHECK(tvecs_eigen.size() == 1)
+        << "Charuco board should only return one translational pose";
+    CHECK(rvecs_eigen.size() == 1)
+        << "Charuco board should only return one rotational pose";
     // Calibration calculates rotation and translation delta from last image
     // stored to automatically capture next image
 
     Eigen::Affine3d H_board_camera =
-        Eigen::Translation3d(tvec_eigen) *
-        Eigen::AngleAxisd(rvec_eigen.norm(), rvec_eigen / rvec_eigen.norm());
+        Eigen::Translation3d(tvecs_eigen[0]) *
+        Eigen::AngleAxisd(rvecs_eigen[0].norm(),
+                          rvecs_eigen[0] / rvecs_eigen[0].norm());
     Eigen::Affine3d H_camera_board_ = H_board_camera.inverse();
     Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
 
@@ -97,8 +115,8 @@
     bool store_image = false;
     double percent_motion =
         std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
-    LOG(INFO) << all_charuco_ids_.size() << ": Moved " << percent_motion
-              << "% of what's needed";
+    LOG(INFO) << "Captured: " << all_charuco_ids_.size() << " points; Moved "
+              << percent_motion << "% of what's needed";
     // Verify that camera has moved enough from last stored image
     if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
       // frame_ refers to deltas between current and last captured image
@@ -118,9 +136,10 @@
           frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
       double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
                                              frame_t_norm / kFrameDeltaTLimit);
-      LOG(INFO) << all_charuco_ids_.size() << ": Moved enough ("
-                << percent_motion << "%); Need to stop (last motion was "
-                << percent_stop << "%";
+      LOG(INFO) << "Captured: " << all_charuco_ids_.size()
+                << "points; Moved enough (" << percent_motion
+                << "%); Need to stop (last motion was " << percent_stop
+                << "% of limit; needs to be < 1 to capture)";
     }
     prev_image_H_camera_board_ = H_camera_board_;
 
@@ -128,8 +147,13 @@
       if (valid) {
         prev_H_camera_board_ = H_camera_board_;
 
-        all_charuco_ids_.emplace_back(std::move(charuco_ids));
-        all_charuco_corners_.emplace_back(std::move(charuco_corners));
+        // Unpack the Charuco ids from Vec4i
+        std::vector<int> charuco_ids_int;
+        for (cv::Vec4i charuco_id : charuco_ids) {
+          charuco_ids_int.emplace_back(charuco_id[0]);
+        }
+        all_charuco_ids_.emplace_back(std::move(charuco_ids_int));
+        all_charuco_corners_.emplace_back(std::move(charuco_corners[0]));
 
         if (r_norm > kDeltaRThreshold) {
           LOG(INFO) << "Triggered by rotation delta = " << r_norm << " > "
@@ -164,7 +188,8 @@
           img_size, cameraMatrix, distCoeffs, rvecs, tvecs,
           stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors,
           calibration_flags);
-      CHECK_LE(reprojection_error, 1.0) << ": Reproduction error is bad.";
+      CHECK_LE(reprojection_error, 1.0)
+          << ": Reproduction error is bad-- greater than 1 pixel.";
       LOG(INFO) << "Reprojection Error is " << reprojection_error;
 
       flatbuffers::FlatBufferBuilder fbb;
@@ -246,6 +271,7 @@
   Eigen::Affine3d prev_image_H_camera_board_;
 
   CharucoExtractor charuco_extractor_;
+  ImageCallback image_callback_;
 };
 
 namespace {
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 83d8760..cf0c8f2 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -9,6 +9,7 @@
 #include "aos/time/time.h"
 #include "aos/util/file.h"
 #include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/charuco_lib.h"
 #include "frc971/vision/vision_generated.h"
 #include "frc971/wpilib/imu_batch_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 8ae97c2..521992c 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -17,7 +17,6 @@
 
 DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
 DEFINE_bool(plot, false, "Whether to plot the resulting data.");
-DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
 DEFINE_bool(turret, true, "If true, the camera is on the turret");
 
 namespace frc971 {