Light refactoring of intrinsics calibration code
Change-Id: Ic7e7a621d9e03ef85e6d3074bff993ee3e470aca
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 642ca0f..3b011f2 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -99,11 +99,9 @@
"//aos/network:message_bridge_server_fbs",
"//aos/network:team_number",
"//frc971/control_loops:quaternion_utils",
+ "//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2020/vision/sift:sift_fbs",
- "//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision/tools/python_code:sift_training_data",
"@com_github_foxglove_schemas//:schemas",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings:str_format",
@@ -303,3 +301,51 @@
"//aos/testing:tmpdir",
],
)
+
+cc_library(
+ name = "intrinsics_calibration_lib",
+ srcs = [
+ "intrinsics_calibration_lib.cc",
+ ],
+ hdrs = [
+ "intrinsics_calibration_lib.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "intrinsics_calibration",
+ srcs = [
+ "intrinsics_calibration.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = [
+ "//y2020:__subpackages__",
+ "//y2022:__subpackages__",
+ "//y2023:__subpackages__",
+ ],
+ deps = [
+ ":intrinsics_calibration_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/frc971/vision/calibration.fbs b/frc971/vision/calibration.fbs
index f5e30a6..df8b7b0 100644
--- a/frc971/vision/calibration.fbs
+++ b/frc971/vision/calibration.fbs
@@ -44,6 +44,10 @@
// Timestamp for when the calibration was taken on the realtime clock.
calibration_timestamp:int64 (id: 6);
+
+ // ID for the physical camera hardware (typically will be a string of the form
+ // YY-NN, with a two-digit year and an index).
+ camera_id:string (id: 7);
}
// Calibration information for all the cameras we know about.
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index ea9af28..711ed7d 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -140,11 +140,13 @@
node, channel_overrides);
}
-Calibration::Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
- aos::EventLoop *image_event_loop,
- aos::EventLoop *imu_event_loop, std::string_view pi,
- TargetType target_type, std::string_view image_channel,
- CalibrationData *data)
+Calibration::Calibration(
+ aos::SimulatedEventLoopFactory *event_loop_factory,
+ aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
+ std::string_view pi,
+ const calibration::CameraCalibration *intrinsics_calibration,
+ TargetType target_type, std::string_view image_channel,
+ CalibrationData *data)
: image_event_loop_(image_event_loop),
image_factory_(event_loop_factory->GetNodeEventLoopFactory(
image_event_loop_->node())),
@@ -152,7 +154,7 @@
imu_factory_(
event_loop_factory->GetNodeEventLoopFactory(imu_event_loop_->node())),
charuco_extractor_(
- image_event_loop_, pi, target_type, image_channel,
+ image_event_loop_, intrinsics_calibration, target_type, image_channel,
[this](cv::Mat rgb_image, monotonic_clock::time_point eof,
std::vector<cv::Vec4i> charuco_ids,
std::vector<std::vector<cv::Point2f>> charuco_corners,
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 5c435ad..c4dcae9 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -105,8 +105,10 @@
public:
Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi, TargetType target_type,
- std::string_view image_channel, CalibrationData *data);
+ std::string_view pi,
+ const calibration::CameraCalibration *intrinsics_calibration,
+ TargetType target_type, std::string_view image_channel,
+ CalibrationData *data);
// Processes a charuco detection that is returned from charuco_lib.
// For a valid detection(s), it stores camera observation
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index f12a6a5..116aba0 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -13,9 +13,6 @@
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/vision_generated.h"
#include "glog/logging.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"
DEFINE_string(board_template_path, "",
"If specified, write an image to the specified path for the "
@@ -37,59 +34,27 @@
using aos::monotonic_clock;
CameraCalibration::CameraCalibration(
- const absl::Span<const uint8_t> training_data_bfbs, std::string_view pi) {
- const aos::FlatbufferSpan<sift::TrainingData> training_data(
- training_data_bfbs);
- CHECK(training_data.Verify());
- camera_calibration_ = FindCameraCalibration(&training_data.message(), pi);
-}
-
-cv::Mat CameraCalibration::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;
-}
-
-Eigen::Matrix3d CameraCalibration::CameraIntrinsicsEigen() const {
- cv::Mat camera_intrinsics = CameraIntrinsics();
- Eigen::Matrix3d result;
- cv::cv2eigen(camera_intrinsics, result);
- return result;
-}
-
-cv::Mat CameraCalibration::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;
-}
-
-const sift::CameraCalibration *CameraCalibration::FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const {
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(pi);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(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 calibration::CameraCalibration *calibration)
+ : intrinsics_([calibration]() {
+ const cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ calibration->intrinsics()->data())));
+ CHECK_EQ(result.total(), calibration->intrinsics()->size());
+ return result;
+ }()),
+ intrinsics_eigen_([this]() {
+ cv::Mat camera_intrinsics = intrinsics_;
+ Eigen::Matrix3d result;
+ cv::cv2eigen(camera_intrinsics, result);
+ return result;
+ }()),
+ dist_coeffs_([calibration]() {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), calibration->dist_coeffs()->size());
+ return result;
+ }()) {}
ImageCallback::ImageCallback(
aos::EventLoop *event_loop, std::string_view channel,
@@ -257,8 +222,9 @@
const Eigen::Affine3d board_to_camera = translation * rotation;
- Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
- board_to_camera * Eigen::Vector3d::Zero();
+ Eigen::Vector3d result = calibration_.CameraIntrinsicsEigen() *
+ 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)
@@ -269,11 +235,13 @@
} else {
result /= result.z();
if (target_type_ == TargetType::kCharuco) {
- cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
+ cv::aruco::drawAxis(rgb_image, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs[i],
tvecs[i], 0.1);
} else {
- cv::drawFrameAxes(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
- tvecs[i], 0.1);
+ cv::drawFrameAxes(rgb_image, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs[i], tvecs[i],
+ 0.1);
}
}
std::stringstream ss;
@@ -307,7 +275,8 @@
}
CharucoExtractor::CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ aos::EventLoop *event_loop,
+ const calibration::CameraCalibration *calibration, TargetType target_type,
std::string_view image_channel,
std::function<void(cv::Mat, monotonic_clock::time_point,
std::vector<cv::Vec4i>,
@@ -315,21 +284,14 @@
std::vector<Eigen::Vector3d>,
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn)
: event_loop_(event_loop),
- calibration_(SiftTrainingData(), pi),
target_type_(target_type),
image_channel_(image_channel),
- camera_matrix_(calibration_.CameraIntrinsics()),
- eigen_camera_matrix_(calibration_.CameraIntrinsicsEigen()),
- dist_coeffs_(calibration_.CameraDistCoeffs()),
- pi_number_(aos::network::ParsePiNumber(pi)),
+ calibration_(CHECK_NOTNULL(calibration)),
handle_charuco_(std::move(handle_charuco_fn)) {
SetupTargetData();
- LOG(INFO) << "Camera matrix " << camera_matrix_;
- LOG(INFO) << "Distortion Coefficients " << dist_coeffs_;
-
- CHECK(pi_number_) << ": Invalid pi number " << pi
- << ", failed to parse pi number";
+ LOG(INFO) << "Camera matrix " << calibration_.CameraIntrinsics();
+ LOG(INFO) << "Distortion Coefficients " << calibration_.CameraDistCoeffs();
LOG(INFO) << "Connecting to channel " << image_channel_;
}
@@ -392,7 +354,7 @@
cv::aruco::interpolateCornersCharuco(
marker_corners, marker_ids, rgb_image, board_,
charuco_corners_with_calibration, charuco_ids_with_calibration,
- camera_matrix_, dist_coeffs_);
+ calibration_.CameraIntrinsics(), calibration_.CameraDistCoeffs());
if (charuco_ids.size() >= FLAGS_min_charucos) {
cv::aruco::drawDetectedCornersCharuco(
@@ -401,7 +363,8 @@
cv::Vec3d rvec, tvec;
valid = cv::aruco::estimatePoseCharucoBoard(
charuco_corners_with_calibration, charuco_ids_with_calibration,
- board_, camera_matrix_, dist_coeffs_, rvec, tvec);
+ board_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvec, tvec);
// if charuco pose is valid, return pose, with ids and corners
if (valid) {
@@ -433,9 +396,9 @@
// estimate pose for arucos 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);
+ cv::aruco::estimatePoseSingleMarkers(
+ marker_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
@@ -459,9 +422,9 @@
// 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);
+ cv::aruco::estimatePoseSingleMarkers(
+ diamond_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 984cef6..b2ca7ee 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -11,8 +11,7 @@
#include "absl/types/span.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
+#include "frc971/vision/calibration_generated.h"
#include "external/com_github_foxglove_schemas/ImageAnnotations_generated.h"
DECLARE_bool(visualize);
@@ -24,23 +23,19 @@
// training data.
class CameraCalibration {
public:
- CameraCalibration(const absl::Span<const uint8_t> training_data_bfbs,
- std::string_view pi);
+ CameraCalibration(const calibration::CameraCalibration *calibration);
// Intrinsics for the located camera.
- cv::Mat CameraIntrinsics() const;
- Eigen::Matrix3d CameraIntrinsicsEigen() const;
+ cv::Mat CameraIntrinsics() const { return intrinsics_; }
+ Eigen::Matrix3d CameraIntrinsicsEigen() const { return intrinsics_eigen_; }
// Distortion coefficients for the located camera.
- cv::Mat CameraDistCoeffs() const;
+ cv::Mat CameraDistCoeffs() const { return dist_coeffs_; }
private:
- // Finds the camera specific calibration flatbuffer.
- const sift::CameraCalibration *FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const;
-
- // Pointer to this camera's calibration parameters.
- const sift::CameraCalibration *camera_calibration_;
+ const cv::Mat intrinsics_;
+ const Eigen::Matrix3d intrinsics_eigen_;
+ const cv::Mat dist_coeffs_;
};
// Helper class to call a function with a cv::Mat and age when an image shows up
@@ -108,7 +103,8 @@
// multiple targets in an image; for charuco boards, there should be just one
// element
CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ aos::EventLoop *event_loop,
+ const calibration::CameraCalibration *calibration, TargetType target_type,
std::string_view image_channel,
std::function<void(cv::Mat, aos::monotonic_clock::time_point,
std::vector<cv::Vec4i>,
@@ -125,9 +121,11 @@
cv::Ptr<cv::aruco::CharucoBoard> board() const { return board_; }
// Returns the camera matrix for this camera.
- const cv::Mat camera_matrix() const { return camera_matrix_; }
+ const cv::Mat camera_matrix() const {
+ return calibration_.CameraIntrinsics();
+ }
// Returns the distortion coefficients for this camera.
- const cv::Mat dist_coeffs() const { return dist_coeffs_; }
+ const cv::Mat dist_coeffs() const { return calibration_.CameraDistCoeffs(); }
private:
// Creates the dictionary, board, and other parameters for the appropriate
@@ -146,7 +144,6 @@
std::vector<Eigen::Vector3d> *tvecs_eigen);
aos::EventLoop *event_loop_;
- CameraCalibration calibration_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::CharucoBoard> board_;
@@ -161,15 +158,7 @@
// 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_;
+ CameraCalibration calibration_;
// Function to call.
std::function<void(
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
new file mode 100644
index 0000000..224a37c
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -0,0 +1,60 @@
+#include <cmath>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <regex>
+
+#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/vision/intrinsics_calibration_lib.h"
+
+DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
+DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+DEFINE_bool(display_undistorted, false,
+ "If true, display the undistorted image.");
+DEFINE_string(pi, "", "Pi name to calibrate.");
+DEFINE_string(base_intrinsics, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ std::string hostname = FLAGS_pi;
+ if (hostname == "") {
+ hostname = aos::network::GetHostname();
+ LOG(INFO) << "Using pi name from hostname as " << hostname;
+ }
+ CHECK(!FLAGS_base_intrinsics.empty())
+ << "Need a base intrinsics json to use to auto-capture images when the "
+ "camera moves.";
+ std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
+ IntrinsicsCalibration extractor(
+ &event_loop, hostname, FLAGS_camera_id, FLAGS_base_intrinsics,
+ FLAGS_display_undistorted, FLAGS_calibration_folder, exit_handle.get());
+
+ event_loop.Run();
+
+ extractor.MaybeCalibrate();
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::Main();
+}
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
new file mode 100644
index 0000000..6cf53e8
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -0,0 +1,245 @@
+#include "frc971/vision/intrinsics_calibration_lib.h"
+
+namespace frc971 {
+namespace vision {
+
+IntrinsicsCalibration::IntrinsicsCalibration(
+ aos::EventLoop *event_loop, std::string_view pi, std::string_view camera_id,
+ std::string_view base_intrinsics_file, bool display_undistorted,
+ std::string_view calibration_folder, aos::ExitHandle *exit_handle)
+ : pi_(pi),
+ pi_number_(aos::network::ParsePiNumber(pi)),
+ camera_id_(camera_id),
+ prev_H_camera_board_(Eigen::Affine3d()),
+ prev_image_H_camera_board_(Eigen::Affine3d()),
+ base_intrinsics_(
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ base_intrinsics_file)),
+ charuco_extractor_(
+ event_loop, &base_intrinsics_.message(), TargetType::kCharuco,
+ "/camera",
+ [this](cv::Mat rgb_image, const aos::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) {
+ HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
+ 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);
+ },
+ std::chrono::milliseconds(5)),
+ display_undistorted_(display_undistorted),
+ calibration_folder_(calibration_folder),
+ exit_handle_(exit_handle) {
+ CHECK(pi_number_) << ": Invalid pi number " << pi
+ << ", failed to parse pi number";
+ std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
+ CHECK(std::regex_match(camera_id_, re))
+ << ": Invalid camera_id '" << camera_id_ << "', should be of form YY-NN";
+}
+
+void IntrinsicsCalibration::HandleCharuco(
+ cv::Mat rgb_image, const aos::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) {
+ // 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));
+ cv::imshow("Display", rgb_image);
+
+ if (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);
+ }
+
+ int keystroke = cv::waitKey(1);
+
+ // If we haven't got a valid pose estimate, don't use these points
+ 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(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_;
+
+ Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
+
+ Eigen::Vector3d delta_t = H_delta.translation();
+
+ double r_norm = std::abs(delta_r.angle());
+ double t_norm = delta_t.norm();
+
+ bool store_image = false;
+ double percent_motion =
+ std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
+ 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
+ Eigen::Affine3d frame_H_delta = H_board_camera * prev_image_H_camera_board_;
+
+ Eigen::AngleAxisd frame_delta_r =
+ Eigen::AngleAxisd(frame_H_delta.rotation());
+
+ Eigen::Vector3d frame_delta_t = frame_H_delta.translation();
+
+ double frame_r_norm = std::abs(frame_delta_r.angle());
+ double frame_t_norm = frame_delta_t.norm();
+
+ // Make sure camera has stopped moving before storing image
+ store_image =
+ frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+ double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
+ frame_t_norm / kFrameDeltaTLimit);
+ 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_;
+
+ if (store_image) {
+ if (valid) {
+ prev_H_camera_board_ = H_camera_board_;
+
+ // 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 << " > "
+ << kDeltaRThreshold;
+ }
+ if (t_norm > kDeltaTThreshold) {
+ LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
+ << kDeltaTThreshold;
+ }
+ }
+
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ exit_handle_->Exit();
+ }
+}
+
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+IntrinsicsCalibration::BuildCalibration(
+ cv::Mat camera_matrix, cv::Mat dist_coeffs,
+ aos::realtime_clock::time_point realtime_now, int pi_number,
+ std::string_view camera_id, uint16_t team_number) {
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::String> name_offset =
+ fbb.CreateString(absl::StrFormat("pi%d", pi_number));
+ flatbuffers::Offset<flatbuffers::String> camera_id_offset =
+ fbb.CreateString(camera_id);
+ flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
+ fbb.CreateVector<float>(9u, [&camera_matrix](size_t i) {
+ return static_cast<float>(
+ reinterpret_cast<double *>(camera_matrix.data)[i]);
+ });
+
+ flatbuffers::Offset<flatbuffers::Vector<float>>
+ distortion_coefficients_offset =
+ fbb.CreateVector<float>(5u, [&dist_coeffs](size_t i) {
+ return static_cast<float>(
+ reinterpret_cast<double *>(dist_coeffs.data)[i]);
+ });
+
+ calibration::CameraCalibration::Builder camera_calibration_builder(fbb);
+
+ camera_calibration_builder.add_node_name(name_offset);
+ camera_calibration_builder.add_team_number(team_number);
+ camera_calibration_builder.add_camera_id(camera_id_offset);
+ 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);
+ fbb.Finish(camera_calibration_builder.Finish());
+
+ return fbb.Release();
+}
+
+void IntrinsicsCalibration::MaybeCalibrate() {
+ // TODO: This number should depend on coarse vs. fine pattern
+ // Maybe just on total # of ids found, not just images
+ if (all_charuco_ids_.size() >= 50) {
+ LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
+ << " images";
+ cv::Mat camera_matrix, dist_coeffs;
+ std::vector<cv::Mat> rvecs, tvecs;
+ cv::Mat std_deviations_intrinsics, std_deviations_extrinsics,
+ per_view_errors;
+
+ // Set calibration flags (same as 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_, charuco_extractor_.board(),
+ img_size, camera_matrix, dist_coeffs, rvecs, tvecs,
+ std_deviations_intrinsics, std_deviations_extrinsics, per_view_errors,
+ calibration_flags);
+ CHECK_LE(reprojection_error, 1.0)
+ << ": Reproduction error is bad-- greater than 1 pixel.";
+ LOG(INFO) << "Reprojection Error is " << reprojection_error;
+
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ std::optional<uint16_t> team_number =
+ aos::network::team_number_internal::ParsePiTeamNumber(pi_);
+ CHECK(team_number) << ": Invalid pi hostname " << pi_
+ << ", failed to parse team number";
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ camera_calibration = BuildCalibration(camera_matrix, dist_coeffs,
+ realtime_now, pi_number_.value(),
+ camera_id_, team_number.value());
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+
+ const std::string calibration_filename =
+ calibration_folder_ +
+ absl::StrFormat("/calibration_pi-%d-%d_cam-%s_%s.json",
+ team_number.value(), pi_number_.value(), camera_id_,
+ time_ss.str());
+
+ LOG(INFO) << calibration_filename << " -> "
+ << aos::FlatbufferToJson(camera_calibration,
+ {.multi_line = true});
+
+ aos::util::WriteStringToFileOrDie(
+ calibration_filename,
+ aos::FlatbufferToJson(camera_calibration, {.multi_line = true}));
+ } else {
+ LOG(INFO) << "Skipping calibration due to not enough images.";
+ }
+}
+} // namespace vision
+} // namespace frc971
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
new file mode 100644
index 0000000..10062ba
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -0,0 +1,78 @@
+#ifndef FRC971_VISION_CALIBRATION_LIB_H_
+#define FRC971_VISION_CALIBRATION_LIB_H_
+#include <cmath>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <regex>
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "absl/strings/str_format.h"
+#include "aos/events/event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/vision/charuco_lib.h"
+
+namespace frc971 {
+namespace vision {
+
+class IntrinsicsCalibration {
+ public:
+ IntrinsicsCalibration(aos::EventLoop *event_loop, std::string_view pi,
+ std::string_view camera_id,
+ std::string_view base_intrinsics_file,
+ bool display_undistorted,
+ std::string_view calibration_folder,
+ aos::ExitHandle *exit_handle);
+
+ void HandleCharuco(cv::Mat rgb_image,
+ const aos::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);
+
+ void MaybeCalibrate();
+
+ static aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ BuildCalibration(cv::Mat camera_matrix, cv::Mat dist_coeffs,
+ aos::realtime_clock::time_point realtime_now, int pi_number,
+ std::string_view camera_id, uint16_t team_number);
+
+ private:
+ static constexpr double kDeltaRThreshold = M_PI / 6.0;
+ static constexpr double kDeltaTThreshold = 0.3;
+
+ static constexpr double kFrameDeltaRLimit = M_PI / 60;
+ static constexpr double kFrameDeltaTLimit = 0.01;
+
+ std::string pi_;
+ const std::optional<uint16_t> pi_number_;
+ const std::string camera_id_;
+
+ std::vector<std::vector<int>> all_charuco_ids_;
+ std::vector<std::vector<cv::Point2f>> all_charuco_corners_;
+
+ Eigen::Affine3d H_camera_board_;
+ Eigen::Affine3d prev_H_camera_board_;
+ Eigen::Affine3d prev_image_H_camera_board_;
+
+ // Camera intrinsics that we will use to bootstrap the intrinsics estimation
+ // here. We make use of the intrinsics in this calibration to allow us to
+ // estimate the relative pose of the charuco board and then identify how much
+ // the board is moving.
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ base_intrinsics_;
+ CharucoExtractor charuco_extractor_;
+ ImageCallback image_callback_;
+
+ const bool display_undistorted_;
+ const std::string calibration_folder_;
+ aos::ExitHandle *exit_handle_;
+};
+
+} // namespace vision
+} // namespace frc971
+#endif // FRC971_VISION_CALIBRATION_LIB_H_