Add C++ charuco camera calibration code
It subscribes to the image message and generates the intrinsics
calibration when enough images have been captured.
To deal with calibration from the python calibration code, the
flatbuffer generator also supports loading both the new and old json
files.
Change-Id: I83190e5c17f970a94ca0eb9310f5787fe714c518
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();
+}