diff --git a/wpical/src/main/native/cpp/cameracalibration.cpp b/wpical/src/main/native/cpp/cameracalibration.cpp index 1d2de356e38..5451d5af7d0 100644 --- a/wpical/src/main/native/cpp/cameracalibration.cpp +++ b/wpical/src/main/native/cpp/cameracalibration.cpp @@ -4,13 +4,38 @@ #include "cameracalibration.h" -#include +bool filter(std::vector charuco_corners, + std::vector charuco_ids, + std::vector> marker_corners, + std::vector marker_ids, int board_width, int board_height) { + if (charuco_ids.empty() || charuco_corners.empty()) { + return false; + } + + if (charuco_corners.size() < 10 || charuco_ids.size() < 10) { + return false; + } + + for (int i = 0; i < charuco_ids.size(); i++) { + if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) { + return false; + } + } + + for (int i = 0; i < marker_ids.size(); i++) { + if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) { + return false; + } + } + + return true; +} int cameracalibration::calibrate(const std::string& input_video, float square_width, float marker_width, int board_width, int board_height, bool show_debug_window) { - // Aruco Board + // ChArUco Board cv::aruco::Dictionary aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); cv::Ptr charuco_board = new cv::aruco::CharucoBoard( @@ -23,26 +48,22 @@ int cameracalibration::calibrate(const std::string& input_video, int frame_count = 0; cv::Size frame_shape; - // Detection output - std::vector all_corners; - std::vector all_ids; - std::vector all_counter; + std::vector> all_charuco_corners; + std::vector> all_charuco_ids; + + std::vector> all_obj_points; + std::vector> all_img_points; - while (video_capture.isOpened()) { + while (video_capture.grab()) { cv::Mat frame; - video_capture >> frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; if (frame.empty()) { break; } - // Limit FPS - frame_count++; - - if (frame_count % 10 != 0) { - continue; - } - // Detect cv::Mat frame_gray; cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); @@ -54,23 +75,30 @@ int cameracalibration::calibrate(const std::string& input_video, std::vector> marker_corners; std::vector marker_ids; + std::vector obj_points; + std::vector img_points; + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, marker_corners, marker_ids); - cv::Mat debug_image = frame; + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } + + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); - if (!charuco_ids.empty() && charuco_ids.size() >= 4) { - all_corners.insert(all_corners.end(), charuco_corners.begin(), - charuco_corners.end()); - all_ids.insert(all_ids.end(), charuco_ids.begin(), charuco_ids.end()); - all_counter.push_back(charuco_ids.size()); + all_charuco_corners.push_back(charuco_corners); + all_charuco_ids.push_back(charuco_ids); + all_obj_points.push_back(obj_points); + all_img_points.push_back(img_points); + + if (show_debug_window) { cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, charuco_ids); - } - - if (show_debug_window) { cv::imshow("Frame", debug_image); } if (cv::waitKey(1) == 'q') { @@ -85,15 +113,13 @@ int cameracalibration::calibrate(const std::string& input_video, cv::Mat camera_matrix, dist_coeffs; std::vector r_vecs, t_vecs; std::vector std_dev_intrinsics, std_dev_extrinsics, per_view_errors; - - std::vector> corners = {all_corners}; - std::vector> ids = {all_ids}; + double repError; try { - cv::aruco::calibrateCameraCharuco( - corners, ids, charuco_board, frame_shape, camera_matrix, dist_coeffs, - r_vecs, t_vecs, std_dev_intrinsics, std_dev_extrinsics, per_view_errors, - cv::CALIB_RATIONAL_MODEL); + int flags = cv::CALIB_RATIONAL_MODEL; + repError = cv::calibrateCamera( + all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs, + r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags); } catch (...) { std::cout << "calibration failed" << std::endl; return 1; @@ -106,13 +132,11 @@ int cameracalibration::calibrate(const std::string& input_video, {"distortion_coefficients", std::vector(dist_coeffs.begin(), dist_coeffs.end())}, - {"avg_reprojection_error", cv::mean(per_view_errors)[0]}}; + {"avg_reprojection_error", repError}}; size_t lastSeparatorPos = input_video.find_last_of("/\\"); std::string output_file_path; - // If a separator is found, return the substring from the beginning to the - // last separator if (lastSeparatorPos != std::string::npos) { output_file_path = input_video.substr(0, lastSeparatorPos) .append("/camera calibration.json"); @@ -126,53 +150,104 @@ int cameracalibration::calibrate(const std::string& input_video, } int cameracalibration::calibrate(const std::string& input_video, - float square_width, int board_width, - int board_height, double imagerWidthPixels, + float square_width, float marker_width, + int board_width, int board_height, + double imagerWidthPixels, double imagerHeightPixels, bool show_debug_window) { + // ChArUco Board + cv::aruco::Dictionary aruco_dict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::Ptr charuco_board = new cv::aruco::CharucoBoard( + cv::Size(board_width, board_height), square_width * 0.0254, + marker_width * 0.0254, aruco_dict); + cv::aruco::CharucoDetector charuco_detector(*charuco_board); + // Video capture cv::VideoCapture video_capture(input_video); int frame_count = 0; + cv::Size frame_shape; // Detection output - std::vector> all_corners; - std::vector> all_points; + std::vector observation_boards; + std::vector frames_rt_toref; - // Dimensions cv::Size boardSize(board_width - 1, board_height - 1); cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); - while (video_capture.isOpened()) { + while (video_capture.grab()) { cv::Mat frame; - video_capture >> frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; if (frame.empty()) { break; } + // frame_count++; + // if (frame_count % 10 != 0) + // { + // continue; + // } + // Detect cv::Mat frame_gray; cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); - cv::Mat debug_image = frame; + frame_shape = frame_gray.size(); - std::vector corners; + std::vector charuco_corners; + std::vector charuco_ids; + std::vector> marker_corners; + std::vector marker_ids; + + std::vector obj_points; + std::vector img_points; + + mrcal_point3_t points[(board_width - 1) * (board_height - 1)]; + + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, + marker_corners, marker_ids); + + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } - bool found = cv::findChessboardCorners( - frame_gray, boardSize, corners, - cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE); + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); - cv::drawChessboardCorners(frame, boardSize, corners, found); + for (int i = 0; i < charuco_ids.size(); i++) { + int id = charuco_ids.at(i); + points[id].x = img_points.at(i).x; + points[id].y = img_points.at(i).y; + points[id].z = 1.0f; + } - if (found) { - all_corners.push_back(corners); + for (int i = 0; i < (sizeof(points) / sizeof(points[0])); i++) { + if (points[i].z != 1.0f) { + points[i].x = -1.0f; + points[i].y = -1.0f; + points[i].z = -1.0f; + } } + std::vector points_vector( + points, points + (sizeof(points) / sizeof(points[0]))); + + frames_rt_toref.push_back(getSeedPose(points_vector.data(), boardSize, + imagerSize, square_width, 1000)); + observation_boards.insert(observation_boards.end(), points_vector.begin(), + points_vector.end()); + if (show_debug_window) { + cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); + cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, + charuco_ids); cv::imshow("Frame", debug_image); } - // Exit loop if 'q' is pressed - if (cv::waitKey(30) == 'q') { + if (cv::waitKey(1) == 'q') { break; } } @@ -180,43 +255,21 @@ int cameracalibration::calibrate(const std::string& input_video, video_capture.release(); cv::destroyAllWindows(); - for (int i = 0; i < all_corners.size(); i++) { - std::vector points; - for (int j = 0; j < all_corners.at(i).size(); j++) { - points.push_back(mrcal_point3_t{all_corners.at(i).at(j).x, - all_corners.at(i).at(j).y, 1.0f}); - } - all_points.push_back(points); - } - - std::vector observations_board; - std::vector frames_rt_toref; - - if (all_points.size() == 0) { + if (observation_boards.empty()) { std::cout << "calibration failed" << std::endl; return 1; + } else { + std::cout << "points detected" << std::endl; } - for (int i = 0; i < all_points.size(); i++) { - size_t total_points = - board_width * boardSize.height * all_points.at(i).size(); - observations_board.reserve(total_points); - frames_rt_toref.reserve(all_points.at(i).size()); - - auto ret = getSeedPose(all_points.at(i).data(), boardSize, imagerSize, - square_width * 0.0254, 1000); - - observations_board.insert(observations_board.end(), - all_points.at(i).begin(), all_points.at(i).end()); - frames_rt_toref.push_back(ret); - } - - auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, - square_width * 0.0254, imagerSize, 1000); + std::unique_ptr cal_result = + mrcal_main(observation_boards, frames_rt_toref, boardSize, + square_width * 0.0254, imagerSize, 1000); auto& stats = *cal_result; - std::vector cameraMatrix = { + // Camera matrix and distortion coefficients + std::vector camera_matrix = { // fx 0 cx stats.intrinsics[0], 0, stats.intrinsics[2], // 0 fy cy @@ -224,23 +277,24 @@ int cameracalibration::calibrate(const std::string& input_video, // 0 0 1 0, 0, 1}; - std::vector distortionCoefficients = {stats.intrinsics[4], - stats.intrinsics[5], - stats.intrinsics[6], - stats.intrinsics[7], - stats.intrinsics[8], - stats.intrinsics[9], - stats.intrinsics[10], - stats.intrinsics[11], - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0}; - - wpi::json result = {{"camera_matrix", cameraMatrix}, - {"distortion_coefficients", distortionCoefficients}, + std::vector distortion_coefficients = {stats.intrinsics[4], + stats.intrinsics[5], + stats.intrinsics[6], + stats.intrinsics[7], + stats.intrinsics[8], + stats.intrinsics[9], + stats.intrinsics[10], + stats.intrinsics[11], + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + // Save calibration output + wpi::json result = {{"camera_matrix", camera_matrix}, + {"distortion_coefficients", distortion_coefficients}, {"avg_reprojection_error", stats.rms_error}}; size_t lastSeparatorPos = input_video.find_last_of("/\\");