diff --git a/.gitignore b/.gitignore index 880023c..f74ec63 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,7 @@ build *.out *.app +### VSCode ### +.vscode/ +*.code-workspace +.history/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..de25784 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(zed_opencv_calibration) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_BUILD_TYPE "Release") + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +add_subdirectory(stereo_calibration) +add_subdirectory(reprojection_viewer) \ No newline at end of file diff --git a/reprojection_viewer/CMakeLists.txt b/reprojection_viewer/CMakeLists.txt index 285f284..faf5455 100644 --- a/reprojection_viewer/CMakeLists.txt +++ b/reprojection_viewer/CMakeLists.txt @@ -1,5 +1,5 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.5) -PROJECT(ZED_Depth_Repro) +PROJECT(zed_reprojection_viewer) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/stereo_calibration/CMakeLists.txt b/stereo_calibration/CMakeLists.txt index ca4b194..042ebdb 100644 --- a/stereo_calibration/CMakeLists.txt +++ b/stereo_calibration/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(ZED_Opencv_Calibration) +project(zed_stereo_calibration) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp new file mode 100644 index 0000000..ee686d2 --- /dev/null +++ b/stereo_calibration/include/calibration_checker.hpp @@ -0,0 +1,103 @@ +#ifndef CALIBRATION_CHECKER_HPP +#define CALIBRATION_CHECKER_HPP + +#include + +typedef struct _board { + cv::Size board_size = { + 0, 0}; // Number of inner corners per a chessboard row and column + float square_size = + 0.0f; // Size of a square in your defined unit (point, millimeter,etc). + std::vector objp; // 3D points in real world space + cv::Size2f board_size_mm = {0.0f, 0.0f}; // Physical size of the board in mm +} Board; + +typedef struct _detected_board_params { + cv::Point2f pos = { + -1.0f, -1.0f}; // Normalized position of the checkerboard in the image + float size = -1.0f; // Normalized size of the checkerboard + float skew = -1.0f; // Normalized skew of the checkerboard +} DetectedBoardParams; + +// Constants +const size_t DEFAULT_MIN_SAMPLES = 20; +const size_t DEFAULT_MAX_SAMPLES = 50; +const float DEFAULT_MIN_TARGET_AREA = 0.1f; // Ignore checkerboards smaller than 10% of the image area +const DetectedBoardParams DEFAULT_IDEAL_PARAMS = { + cv::Point2f( + 0.65f, // Checkerboard X position should cover 65% of the image width + 0.65f // Checkerboard Y position should cover 65% of the image height + ), + 0.4f, // Checkerboard size variation should be at least 40% + 0.6f // Checkerboard skew variation should be at least 70% +}; // Ideal parameters for a good sample database + +class CalibrationChecker { + public: + CalibrationChecker(cv::Size board_size, float square_size, + size_t min_samples = DEFAULT_MIN_SAMPLES, + size_t max_samples = DEFAULT_MAX_SAMPLES, + float min_target_area = DEFAULT_MIN_TARGET_AREA, + DetectedBoardParams idealParams = DEFAULT_IDEAL_PARAMS, + bool verbose = false); + ~CalibrationChecker() = default; + + // Test if the detected corners form a valid sample + bool testSample(const std::vector& corners, cv::Size image_size); + + // Retrieve valid corners + const std::vector>& getValidCorners() const { + return validCorners_; + } + + // Retrieve valid sample count + size_t getValidSampleCount() const { return validCorners_.size(); } + + // Calculate the sample collection status according to the stored samples + bool evaluateSampleCollectionStatus(float& size_score, float& skew_score, + float& pos_score_x, + float& pos_score_y) const; + + // Retrieve the last detected board parameters + const DetectedBoardParams& getLastDetectedBoardParams() const; + + private: + // Calculate the parameter of a detected checkerboard + DetectedBoardParams getDetectedBoardParams( + const std::vector& corners, cv::Size image_size); + + // Check if the detected corners are valid + bool isGoodSample(const DetectedBoardParams& params); + + // Helper functions + std::vector get_outside_corners( + const std::vector& + corners); // Get the 4 outside corners of a checkerboard + float compute_skew( + const std::vector& + outside_corners); // Compute skew based on the 4 outside corners + float compute_area( + const std::vector& + outside_corners); // Compute area based on the 4 outside corners + + private: + Board board_; + + std::vector + paramDb_; // Database of previously detected board parameters + std::vector> + validCorners_; // All the corners associated to the single parameters in + // paramDb_ + + DetectedBoardParams idealParams_ = DEFAULT_IDEAL_PARAMS; + size_t min_samples_ = DEFAULT_MIN_SAMPLES; // Minimum number of samples to + // consider the database complete + size_t max_samples_ = DEFAULT_MAX_SAMPLES; // Maximum number of samples to + // consider the database complete + + float min_target_area_ = DEFAULT_MIN_TARGET_AREA; // Ignore checkerboards smaller than this area (percentage of image area) + + bool verbose_ = false; +}; + +#endif // CALIBRATION_CHECKER_HPP diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 07cbd48..b594c12 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -1,137 +1,212 @@ #pragma once -#include -#include -#include #include +#include +#include +#include +#include + +constexpr int MIN_IMAGE = 20; -constexpr int MIN_IMAGE = 15; +struct CameraCalib { + cv::Mat K; + cv::Mat D; + bool disto_model_RadTan = true; -struct CameraCalib{ - cv::Mat K; - cv::Mat D; - bool disto_model_RadTan = true; + void print(const std::string &name) const { + std::cout << name << " K:" << std::endl << K << std::endl; + std::cout << " D:" << std::endl << D << std::endl; + } - void print(std::string name) { - std::cout << name << " K:\n" << K << std::endl; - std::cout << " D:\n" << D << std::endl; + void initDefault(bool radtan) { + disto_model_RadTan = radtan; + K = cv::Mat::eye(3, 3, CV_32FC1); + if (disto_model_RadTan) { + // Radial and tangential distortion + const int nb_coeff = 8; // 6 radial + 2 tangential; could be extended to + // 12 with prism distortion + D = cv::Mat::zeros(1, nb_coeff, CV_32FC1); + } else { + // Fisheye model has 4 coefficients: k1, k2, k3, k4 + D = cv::Mat::zeros(1, 4, CV_32FC1); } + } + + void setFrom(const sl::CameraParameters &cam) { + K = cv::Mat::eye(3, 3, CV_32FC1); + K.at(0, 0) = cam.fx; + K.at(1, 1) = cam.fy; + K.at(0, 2) = cam.cx; + K.at(1, 2) = cam.cy; - void initDefault(bool radtan){ - disto_model_RadTan = radtan; - K = cv::Mat::eye(3, 3, CV_64FC1); - if (disto_model_RadTan) { - // Radial and tangential distortion - const int nb_coeff = 8; // 6 radial + 2 tangential; could be extended to 12 with prism distortion - D = cv::Mat::zeros(1, nb_coeff, CV_64FC1); - } else { - // Fisheye model has 4 coefficients: k1, k2, k3, k4 - D = cv::Mat::zeros(1, 4, CV_64FC1); - } + // tangential distortion coefficients are not used in the Fisheye model, + // looking for p1 and p2 equal to 0 + if (cam.disto[2] == 0. && cam.disto[3] == 0. && cam.disto[4] != 0. && + cam.disto[5] != 0.) { + disto_model_RadTan = false; // -> Fisheye model + // Fisheye model has 4 coefficients: k1, k2, k3, k4 + D = cv::Mat::zeros(1, 4, CV_32FC1); + D.at(0) = cam.disto[0]; + D.at(1) = cam.disto[1]; + D.at(2) = cam.disto[4]; + D.at(3) = cam.disto[5]; + } else { + disto_model_RadTan = true; // Radial and tangential distortion + const int nb_coeff = 8; // 6 radial + 2 tangential; could be extended + // to 12 with prism distortion + D = cv::Mat::zeros(1, nb_coeff, CV_32FC1); + for (int i = 0; i < nb_coeff; i++) D.at(i) = cam.disto[i]; } + } - void setFrom(sl::CameraParameters & cam) { - K = cv::Mat::eye(3, 3, CV_64FC1); - K.at(0, 0) = cam.fx; - K.at(1, 1) = cam.fy; - K.at(0, 2) = cam.cx; - K.at(1, 2) = cam.cy; - - // tangential distortion coefficients are not used in the Fisheye model, looking for p1 and p2 equal to 0 - if(cam.disto[2]==0. && cam.disto[3]==0. && cam.disto[4]!=0. && cam.disto[5]!=0.) { - disto_model_RadTan = false; // -> Fisheye model - // Fisheye model has 4 coefficients: k1, k2, k3, k4 - D = cv::Mat::zeros(1, 4, CV_64FC1); - D.at(0) = cam.disto[0]; - D.at(1) = cam.disto[1]; - D.at(2) = cam.disto[4]; - D.at(3) = cam.disto[5]; - } else { - disto_model_RadTan = true; // Radial and tangential distortion - const int nb_coeff = 8; // 6 radial + 2 tangential; could be extended to 12 with prism distortion - D = cv::Mat::zeros(1, nb_coeff, CV_64FC1); - for(int i = 0; i < nb_coeff; i++) - D.at(i) = cam.disto[i]; - } + std::vector undistortPoints( + const std::vector &points) const { + std::cout << "K:" << std::endl << K << std::endl; + std::cout << "D:" << std::endl << D << std::endl; + std::vector undistorted_points; + if (disto_model_RadTan) { + cv::undistortPoints(points, undistorted_points, K, D); + } else { + cv::fisheye::undistortPoints(points, undistorted_points, K, D); } + return undistorted_points; + } - std::vector undistortPoints(const std::vector &points) const { - std::vector undistorted_points; - if (disto_model_RadTan) { - cv::undistortPoints(points, undistorted_points, K, D); - } else { - cv::fisheye::undistortPoints(points, undistorted_points, K, D); - } - return undistorted_points; + float mono_calibrate(const std::vector> &object_points, + const std::vector> &image_points, + const cv::Size &image_size, int flags, bool verbose) { + float rms = -1.0f; + std::vector rvec, tvec; + if (disto_model_RadTan) { + if (D.cols >= 8) flags += cv::CALIB_RATIONAL_MODEL; + if (verbose) { + std::cout << "[DEBUG][mono_calibrate] Calibrating with " + "Radial-Tangential model..." + << std::endl; + } + rms = cv::calibrateCamera(object_points, image_points, image_size, K, D, + rvec, tvec, flags); + } else { + if (verbose) { + std::cout << "[DEBUG][mono_calibrate] Calibrating with Fisheye model..." + << std::endl; + } + rms = cv::fisheye::calibrate( + object_points, image_points, image_size, K, D, rvec, tvec, + flags + cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC + + cv::fisheye::CALIB_FIX_SKEW); } - double calibrate(const std::vector> &object_points, - const std::vector> &image_points, - const cv::Size &image_size, - int flags) { - double rms = 0.0; - std::vector rvec, tvec; - if (disto_model_RadTan){ - if(D.cols >= 8) - flags += cv::CALIB_RATIONAL_MODEL; - rms = cv::calibrateCamera(object_points, image_points, image_size, K, D, rvec, tvec, flags); - } else - rms = cv::fisheye::calibrate(object_points, image_points, image_size, K, D, rvec, tvec, flags + cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC + cv::fisheye::CALIB_FIX_SKEW); - return rms; + if (verbose) { + std::cout << "[DEBUG][mono_calibrate] * Intrinsic matrix K:" << std::endl + << K << std::endl; + std::cout << "[DEBUG][mono_calibrate] * Distortion coefficients D:" << std::endl + << D << std::endl; + std::cout << "[DEBUG][mono_calibrate] * Re-projection error (RMS): " << rms << std::endl; } + + return rms; + } }; -struct StereoCalib{ - CameraCalib left; - CameraCalib right; - cv::Mat R; // Rotation matrix between left and right camera - cv::Mat Rv; // Rotation vector between left and right camera - cv::Mat T; // Translation vector between left and right camera +struct StereoCalib { + CameraCalib left; + CameraCalib right; + + cv::Mat R; // Rotation matrix between left and right camera + cv::Mat Rv; // Rotation vector between left and right camera + cv::Mat T; // Translation vector between left and right camera + + cv::Size imageSize; + + void initDefault(bool radtan) { + left.initDefault(radtan); + right.initDefault(radtan); + R = cv::Mat::eye(3, 3, CV_32FC1); + Rv = cv::Mat::zeros(3, 1, CV_32FC1); + T = cv::Mat::zeros(3, 1, CV_32FC1); + } + + void setFrom(const sl::CalibrationParameters &calib_params) { + left.setFrom(calib_params.left_cam); + right.setFrom(calib_params.right_cam); + + auto translation = calib_params.stereo_transform.getTranslation(); + T.at(0) = translation.x * -1; // the zed configuration file store + // the absolute value of the Tx part + T.at(1) = translation.y; + T.at(2) = translation.z; + + auto rot = calib_params.stereo_transform.getRotationVector(); + Rv.at(0) = rot.x; + Rv.at(1) = rot.y; + Rv.at(2) = rot.z; + cv::Rodrigues(Rv, R); + } + + float stereo_calibrate( + const std::vector> &object_points, + const std::vector> &image_points_left, + const std::vector> &image_points_right, + const cv::Size &image_size, int flags, bool verbose) { - void initDefault(bool radtan){ - left.initDefault(radtan); - right.initDefault(radtan); - R = cv::Mat::eye(3, 3, CV_64FC1); - Rv = cv::Mat::zeros(3, 1, CV_64FC1); - T = cv::Mat::zeros(3, 1, CV_64FC1); + imageSize = image_size; + + float rms = 0.0; + cv::Mat E, F; + + if (left.disto_model_RadTan && right.disto_model_RadTan) { + if (verbose) { + std::cout + << "[DEBUG][stereo_calibrate] Calibrating with Radial-Tangential model..." + << std::endl; + } + rms = cv::stereoCalibrate(object_points, image_points_left, + image_points_right, left.K, left.D, right.K, + right.D, image_size, R, T, E, F, flags); + } else { + if (verbose) { + std::cout + << "[DEBUG][stereo_calibrate] Calibrating with Fisheye model..." + << std::endl; + } + rms = cv::fisheye::stereoCalibrate(object_points, image_points_left, + image_points_right, left.K, left.D, + right.K, right.D, image_size, R, T, + flags + cv::fisheye::CALIB_CHECK_COND); } - void setFrom(sl::CalibrationParameters & calib_params) { - left.setFrom(calib_params.left_cam); - right.setFrom(calib_params.right_cam); - - auto translation = calib_params.stereo_transform.getTranslation(); - T.at(0) = translation.x * -1; // the zed configuration file store the absolute value of the Tx part - T.at(1) = translation.y; - T.at(2) = translation.z; - - auto rot = calib_params.stereo_transform.getRotationVector(); - Rv.at(0) = rot.x; - Rv.at(1) = rot.y; - Rv.at(2) = rot.z; - cv::Rodrigues(Rv, R); - std::cout<<" Lens disto model "<<(left.disto_model_RadTan && right.disto_model_RadTan)<> &object_points, - const std::vector> &image_points_left, - const std::vector> &image_points_right, - const cv::Size &image_size, - int flags) { - double rms = 0.0; - cv::Mat E, F; - if(left.disto_model_RadTan && right.disto_model_RadTan) - rms = cv::stereoCalibrate(object_points, image_points_left, image_points_right, - left.K, left.D, right.K, right.D, image_size, - R, T, E, F, flags); - else - rms = cv::fisheye::stereoCalibrate( - object_points, image_points_left, image_points_right, left.K, - left.D, right.K, right.D, image_size, R, T, - flags + cv::fisheye::CALIB_CHECK_COND); - cv::Rodrigues(R, Rv); - return rms; + if (verbose) { + std::cout << "[DEBUG][stereo_calibrate] * New Intrinsic matrix K left:" + << std::endl + << left.K << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * New Distortion coefficients D left:" << std::endl + << left.D << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * New Intrinsic matrix K right:" << std::endl + << right.K << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * New Distortion coefficients D right:" << std::endl + << right.D << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * Re-projection error (RMS): " << rms << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * Rotation matrix R:" << std::endl + << R << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * Rotation vector Rv:" << std::endl + << Rv << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * Translation vector T:" << std::endl + << T << std::endl; + std::cout << "[DEBUG][stereo_calibrate] * Re-projection error (RMS): " << rms << std::endl; } + + return rms; + } + + std::string saveCalibOpenCV(int serial); + std::string saveCalibZED(int serial, bool is_4k = false); }; -int calibrate(const std::string& folder, StereoCalib &raw_data, int target_w, int target_h, float square_size, int serial, bool save_calib_mono = false, bool use_intrinsic_prior = false); \ No newline at end of file +int calibrate(int img_count, const std::string &folder, StereoCalib &raw_data, + int h_edges, int v_edges, float square_size, int serial, + bool is_dual_mono, bool is_4k, bool save_calib_mono = false, + bool use_intrinsic_prior = false, float max_repr_error = 0.5f, + bool verbose = false); \ No newline at end of file diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp new file mode 100644 index 0000000..b3d2b29 --- /dev/null +++ b/stereo_calibration/src/calibration_checker.cpp @@ -0,0 +1,363 @@ +#include "calibration_checker.hpp" + +constexpr float PI = static_cast(M_PI); + +constexpr size_t up_left = 0; +constexpr size_t up_right = 1; +constexpr size_t down_right = 2; +constexpr size_t down_left = 3; + +CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, + size_t min_samples, size_t max_samples, + float min_target_area, + DetectedBoardParams idealParams, + bool verbose) { + verbose_ = verbose; + + // Calibration parameters + min_samples_ = min_samples; + max_samples_ = max_samples; + min_target_area_ = min_target_area; + idealParams_ = idealParams; + + // Initialize the board parameters + board_.board_size = board_size; + board_.square_size = square_size; + board_.objp.clear(); + board_.board_size_mm = + cv::Size(board_size.width * square_size, board_size.height * square_size); + + // Prepare object points based on the known board size and square size + for (int i = 0; i < board_size.height; i++) { + for (int j = 0; j < board_size.width; j++) { + board_.objp.push_back(cv::Point3f(j * square_size, i * square_size, 0)); + } + } +} + +bool CalibrationChecker::testSample(const std::vector& corners, + cv::Size image_size) { + DetectedBoardParams params = getDetectedBoardParams(corners, image_size); + + if (params.size < 0 || params.skew < 0) { + return false; // Invalid parameters + } + + std::cout << std::setprecision(3) << " * New Sample: Pos(" << params.pos.x + << ", " << params.pos.y << "), Size: " << params.size + << ", Skew: " << params.skew << std::endl; + + if (isGoodSample(params)) { + // Store the valid parameters and associated corners + paramDb_.push_back(params); + validCorners_.push_back(corners); + std::cout << " Sample stored. Total valid samples: " + << validCorners_.size() << std::endl; + return true; + } + + std::cout << " * Sample rejected." << std::endl; + return false; +} + +float CalibrationChecker::compute_skew( + const std::vector& outside_corners) { + /* Get skew for given checkerboard detection. + Scaled to [0,1], which 0 = no skew, 1 = high skew + Skew is proportional to the divergence of three outside corners from 90 + degrees. + */ + if (outside_corners.size() != 4) { + return -1.0f; // Invalid input + } + + auto angle = [](const cv::Point2f& a, const cv::Point2f& b, + const cv::Point2f& c) -> float { + cv::Point2f ab = a - b; + cv::Point2f cb = c - b; + float dot = ab.x * cb.x + ab.y * cb.y; + float norm_ab = std::sqrt(ab.x * ab.x + ab.y * ab.y); + float norm_cb = std::sqrt(cb.x * cb.x + cb.y * cb.y); + float cos_angle = dot / (norm_ab * norm_cb); + if (cos_angle < -1.0f) + cos_angle = -1.0f; + else if (cos_angle > 1.0f) + cos_angle = 1.0f; + return std::acos(cos_angle); + }; + + // Original code from here: + // https://github.com/ros-perception/image_pipeline/blob/rolling/camera_calibration/src/camera_calibration/calibrator.py#L187-L207 + // float skew = std::min( + // 1.0f, 2.0f * std::abs((PI / 2) - angle(outside_corners[up_left], + // outside_corners[up_right], + // outside_corners[down_right]))); + + float maxDeviation = 0.0f; + for (int i = 0; i < 4; i++) { + float ang = angle(outside_corners[(i + 3) % 4], outside_corners[i], + outside_corners[(i + 1) % 4]); + float deviation = std::abs((PI / 2) - ang); + if (deviation > maxDeviation) { + maxDeviation = deviation; + } + } + + return maxDeviation / (PI / 2); +} + +float CalibrationChecker::compute_area( + const std::vector& outside_corners) { + /* Get 2d image area of the detected checkerboard. + The projected checkerboard is assumed to be a convex quadrilateral, and the + area computed as |p X q|/2; see + http://mathworld.wolfram.com/Quadrilateral.html. + */ + + if (outside_corners.size() != 4) { + return -1.0f; // Invalid input + } + + // Using the shoelace formula to compute area of the quadrilateral + cv::Point2f a = outside_corners[up_right] - outside_corners[up_left]; + cv::Point2f b = outside_corners[down_right] - outside_corners[up_right]; + cv::Point2f c = outside_corners[down_left] - outside_corners[down_right]; + + cv::Point2f p = b + c; + cv::Point2f q = a + b; + + float area = 0.5f * std::abs(p.x * q.y - p.y * q.x); + + return area; +} + +std::vector CalibrationChecker::get_outside_corners( + const std::vector& corners) { + std::vector outside_corners; + + if (corners.size() != board_.board_size.area()) { + return outside_corners; + } + + size_t x_dim = board_.board_size.width; + size_t y_dim = board_.board_size.height; + + outside_corners.resize(4); + + outside_corners[up_left] = corners[0]; // Top-left + outside_corners[up_right] = corners[x_dim - 1]; // Top-right + outside_corners[down_right] = + corners[(y_dim - 1) * x_dim + (x_dim - 1)]; // Bottom-right + outside_corners[down_left] = corners[(y_dim - 1) * x_dim]; // Bottom-left + + return outside_corners; +} + +DetectedBoardParams CalibrationChecker::getDetectedBoardParams( + const std::vector& corners, cv::Size image_size) { + DetectedBoardParams params; + + auto outside_corners = get_outside_corners(corners); + float area = compute_area(outside_corners); + float skew = compute_skew(outside_corners); + + if (area < min_target_area_ || skew < 0) { + // Return invalid params + params.size = -1.0f; + params.skew = -1.0f; + return params; + } + + //float border = std::sqrt(area); + + // For X and Y, we "shrink" the image all around by approx.half the board + // size. Otherwise large boards are penalized because you can't get much X/Y + // variation. + float avg_x = 0.0f; + float avg_y = 0.0f; + for (const auto& corner : corners) { + avg_x += corner.x; + avg_y += corner.y; + } + avg_x /= static_cast(corners.size()); + avg_y /= static_cast(corners.size()); + + //float p_x = std::min(1.0f, std::max(0.0f, (avg_x - border / 2.0f) / (image_size.width - border))); + //float p_y = std::min(1.0f, std::max(0.0f, (avg_y - border / 2.0f) / (image_size.height - border))); + float p_x = std::min(1.0f, std::max(0.0f, avg_x / image_size.width)); + float p_y = std::min(1.0f, std::max(0.0f, avg_y / image_size.height)); + + params.pos = cv::Point2f(p_x, p_y); + params.size = std::sqrt(area / (image_size.width * image_size.height)); + params.skew = skew; + + return params; +} + +bool CalibrationChecker::isGoodSample(const DetectedBoardParams& params) { + if (paramDb_.empty()) { + return true; // First sample is always good + } + + // Original similarity check from: + // https://github.com/ros-perception/image_pipeline/blob/rolling/camera_calibration/src/camera_calibration/calibrator.py#L485-L507 + auto param_distance = [](const DetectedBoardParams& p1, + const DetectedBoardParams& p2) -> float { + return std::abs(p1.size - p2.size) + + std::abs(p1.skew - p2.skew) + + std::abs(p1.pos.x - p2.pos.x) + + std::abs(p1.pos.y - p2.pos.y); + }; + + int idx = 0; + for (auto& stored_params : paramDb_) { + float dist = param_distance(params, stored_params); + if (dist < 0.2f) { // TODO tune the threshold + std::cout << " Rejected: Too similar to sample #" << idx << " (dist=" << + dist << ")" << std::endl; + return false; + } + idx++; + } + + return true; + + // // New similarity check: + // auto is_different = [this](const DetectedBoardParams& p1, + // const DetectedBoardParams& p2) -> bool { + // // Check that at least one parameter differs by at least 10% from all the + // // stored samples + // constexpr float epsilon = 1e-6f; + // float pos_x_diff = std::abs(p1.pos.x - p2.pos.x) / std::max(std::max(p1.pos.x, p2.pos.x), epsilon); + // float pos_y_diff = std::abs(p1.pos.y - p2.pos.y) / std::max(std::max(p1.pos.y, p2.pos.y), epsilon); + // float size_diff = std::abs(p1.size - p2.size) / std::max(std::max(p1.size, p2.size), epsilon); + // float skew_diff = std::abs(p1.skew - p2.skew) / std::max(std::max(p1.skew, p2.skew), epsilon); + + // const float diff_thresh = 0.1f; // 10% difference threshold + + // if (verbose_) { + // std::cout << "[DEBUG][isGoodSample] Comparing: " << std::endl + // << std::setprecision(3) + // << " * Pos(" << p1.pos.x << ", "<< p1.pos.y << "), Size: " << p1.size << ", Skew: " << p1.skew << std::endl + // << " * Pos(" << p2.pos.x << ", "<< p2.pos.y << "), Size: " << p2.size << ", Skew: " << p2.skew << std::endl; + // std::cout << std::setprecision(3) + // << " * dPosX: " << pos_x_diff * 100.0f << "%, " << std::endl + // << " * dPosY: " << pos_y_diff * 100.0f << "%, " << std::endl + // << " * dSize: " << size_diff * 100.0f << "%, " << std::endl + // << " * dSkew: " << skew_diff * 100.0f << "%" << std::endl; + // } + + // if (size_diff > diff_thresh || skew_diff > diff_thresh || + // pos_x_diff > diff_thresh || pos_y_diff > diff_thresh) { + // if (verbose_) { + // std::cout << " => Different enough" << std::endl; + // } + // return true; // At least one parameter is sufficiently different + // } + + // if (verbose_) { + // std::cout << " => Too similar" << std::endl; + // } + + // return false; // All parameters are too similar + // }; + + // for (auto& stored_params : paramDb_) { + // // Stop at the first similar sample found + // if (!is_different(params, stored_params)) { + // std::cout << "Rejected: Too similar to an existing sample" << std::endl; + // return false; + // } + // } + + // return true; +} + +bool CalibrationChecker::evaluateSampleCollectionStatus( + float& size_score, float& skew_score, float& pos_score_x, + float& pos_score_y) const { + size_score = 0.0f; + skew_score = 0.0f; + pos_score_x = 0.0f; + pos_score_y = 0.0f; + if (paramDb_.empty()) { + return false; + } + + float min_px = 1.0f, max_px = 0.0f; + float min_py = 1.0f, max_py = 0.0f; + float min_size = 1.0f, max_size = 0.0f; + float min_skew = 1.0f, max_skew = 0.0f; + + for (const auto& params : paramDb_) { + if (params.pos.x < min_px) min_px = params.pos.x; + if (params.pos.x > max_px) max_px = params.pos.x; + if (params.pos.y < min_py) min_py = params.pos.y; + if (params.pos.y > max_py) max_py = params.pos.y; + if (params.size < min_size) min_size = params.size; + if (params.size > max_size) max_size = params.size; + if (params.skew < min_skew) min_skew = params.skew; + if (params.skew > max_skew) max_skew = params.skew; + } + + // Don't reward small size or skew + // min_skew = 0.0f; + // min_size = 0.0f; + + pos_score_x = std::min((max_px - min_px) / idealParams_.pos.x, 1.0f); + pos_score_y = std::min((max_py - min_py) / idealParams_.pos.y, 1.0f); + size_score = std::min((max_size - min_size) / idealParams_.size, 1.0f); + skew_score = std::min((max_skew - min_skew) / idealParams_.skew, 1.0f); + + std::cout << "Sample Collection Status:" << std::endl; + std::cout << " - PosX status: [" << min_px << " , " << max_px << "] -> " + << max_px - min_px << "/" << idealParams_.pos.x << std::endl; + std::cout << " * PosX Score : " << std::setprecision(3) << pos_score_x + << std::endl; + std::cout << " - PosY status: [" << min_py << " , " << max_py << "] -> " + << max_py - min_py << "/" << idealParams_.pos.y << std::endl; + std::cout << " * PosY Score : " << std::setprecision(3) << pos_score_y + << std::endl; + std::cout << " - Size status: [" << min_size << " , " << max_size << "] -> " + << max_size - min_size << "/" << idealParams_.size << std::endl; + std::cout << " * Size Score : " << std::setprecision(3) << size_score + << std::endl; + std::cout << " - Skew status: [" << min_skew << " , " << max_skew << "] -> " + << max_skew - min_skew << "/" << idealParams_.skew << std::endl; + std::cout << " * Skew Score : " << std::setprecision(3) << skew_score + << std::endl; + + if (paramDb_.size() < min_samples_) { + std::cout << "Sample collection incomplete: not reached the minimum sample " + "count (" + << paramDb_.size() << "/" << min_samples_ << ")" << std::endl; + return false; + } + + if (paramDb_.size() >= max_samples_) { + std::cout + << "Sample collection complete: Reached the maximum sample count (" + << paramDb_.size() << "/" << max_samples_ << ")" << std::endl; + return true; + } + + if (size_score >= 1.0f && skew_score >= 1.0f && pos_score_x >= 1.0f && + pos_score_y >= 1.0f) { + std::cout << "Sample collection complete: All scores are above threshold" + << std::endl; + return true; + } + + std::cout << "Sample collection incomplete." << std::endl; + return false; +} + +const DetectedBoardParams& CalibrationChecker::getLastDetectedBoardParams() + const { + if (paramDb_.empty()) { + static DetectedBoardParams empty_params = {cv::Point2f(-1.0f, -1.0f), -1.0f, + -1.0f}; + return empty_params; + } + return paramDb_.back(); +} \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 66a07ee..a858514 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -1,569 +1,650 @@ -#include "opencv_calibration.hpp" #include +#include + +#include "calibration_checker.hpp" +#include "opencv_calibration.hpp" namespace fs = std::filesystem; -// CHANGE THIS PARAM BASED ON THE CHECKERBOARD USED -// https://docs.opencv.org/4.x/da/d0d/tutorial_camera_calibration_pattern.html -constexpr int target_w = 9; // number of horizontal inner edges -constexpr int target_h = 6; // number of vertical inner edges -constexpr float square_size = 24.0; // mm +// ********************************************************************************* +// CHANGE THIS PARAMS USING THE COMMAND LINE OPTIONS +// Learn more: +// * https://docs.opencv.org/4.x/da/d0d/tutorial_camera_calibration_pattern.html -std::string folder = "zed-images/"; +int h_edges = 9; // number of horizontal inner edges +int v_edges = 6; // number of vertical inner edges +float square_size = 25.4; // mm -int verbose = 0; +// Default parameters are good for this checkerboard: +// https://github.com/opencv/opencv/blob/4.x/doc/pattern.png/ +// ********************************************************************************* -struct extrinsic_checker { - float rot_x_min; - float rot_y_min; - float rot_z_min; - float rot_x_max; - float rot_y_max; - float rot_z_max; +std::string image_folder = "zed-images/"; - float rot_x_delta; - float rot_y_delta; - float rot_z_delta; +// Argument parsing helper +std::map parseArguments(int argc, char* argv[]); - float d_min; - float d_max; - float distance_tot; -}; +// Coverage indicator fill helpers +void addNewCheckerboardPosition(cv::Mat& coverage_indicator, + cv::Mat& pos_indicator, float norm_x, + float norm_y, float norm_size); +void applyCoverageIndicatorOverlay(cv::Mat& image, + const cv::Mat& coverage_indicator); +void applyPosIndicatorOverlay(cv::Mat& image, const cv::Mat& pos_indicator); -std::map parseArguments(int argc, char* argv[]); -bool writeRotText(cv::Mat& image, float rot_x, float rot_y, float rot_z, float distance, int fontSize); -float CheckCoverage(const std::vector>& pts, const cv::Size& imgSize); -bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat t); +/// Rendering +constexpr int text_area_height = 210; +const cv::Size display_size(720, 404); // Size of the rendered images /// Calibration condition -const float min_coverage = 10; // in percentage -const float min_rotation = 60; // in degrees -const float acceptable_rotation = 50; // in degrees -const float min_distance = 200; // in mm -const float acceptable_distance = 150; // in mm - -std::vector> pts_detected; - -std::vector square_valid; -int bucketsize = 480; -const int MinPts = 10; -const int MaxPts = 90; -const cv::Scalar info_color = cv::Scalar(50,205,50); +const float max_repr_error = 0.5; // in pixels +const int min_samples = 25; +const int max_samples = 40; +const float min_x_coverage = + 0.6f; // Checkerboard X position covering percentage of the image width +const float min_y_coverage = + 0.6f; // Checkerboard Y position covering percentage of the image height +const float min_area_range = 0.45f; // Checkerboard area range size [min_area-max_area] +const float min_skew_range = 0.375f; // Checkerboard skew ange size [min_skew-max_skew] + +const float min_target_area = 0.1f; // Ignore checkerboards smaller than this area (percentage of image area) + +// Debug +bool verbose = true; +int sdk_verbose = 0; + +// Text colors +const cv::Scalar info_color = cv::Scalar(50, 205, 50); const cv::Scalar warn_color = cv::Scalar(0, 128, 255); -const bool image_stack_horizontal = true; // true for horizontal, false for vertical - +// SIDE-by-SIDE or TOP-BOTTOM image stacking for display +const bool image_stack_horizontal = + true; // true for horizontal, false for vertical -void scaleKP(std::vector &pts, cv::Size in, cv::Size out){ +// Scale keypoints according to image size change +void scaleKP(std::vector& pts, cv::Size in, cv::Size out) { + float rx = out.width / static_cast(in.width); + float ry = out.height / static_cast(in.height); - float rx = out.width / static_cast(in.width); - float ry = out.height / static_cast(in.height); - - for(auto &it: pts){ - it.x *= rx; - it.y *= ry; - } + for (auto& it : pts) { + it.x *= rx; + it.y *= ry; + } } struct Args { - std::string svo_path = ""; - bool is_radtan_lens = true; - bool is_zed_x_one_virtual_stereo = false; - - void parse(int argc, char* argv[]) { - for (int i = 1; i < argc; i++) { - std::string arg = argv[i]; - if (arg == "--svo" && i + 1 < argc) { - svo_path = argv[++i]; - } else if (arg == "--fisheye") { - is_radtan_lens = false; - } else if (arg == "--zed-x-one") { - is_zed_x_one_virtual_stereo = true; - } else if (arg == "--help" || arg == "-h") { - std::cout << "Usage: " << argv[0] << " [options]\n"; - std::cout << " --svo Path to the SVO file\n"; - std::cout << " --fisheye Use fisheye lens model\n"; - std::cout << " --zed-x-one Use ZED X One cameras as a " - "virtual stereo pair\n"; - std::cout << " --help, -h Show this help message\n"; - exit(0); - } - } + std::string app_name; + std::string svo_path = ""; + bool is_radtan_lens = true; + bool is_zed_x_one_virtual_stereo = false; + bool is_zed_sdk_format = false; + int left_camera_id = -1; + int right_camera_id = -1; + int left_camera_sn = -1; + int right_camera_sn = -1; + + void parse(int argc, char* argv[]) { + app_name = argv[0]; + for (int i = 1; i < argc; i++) { + std::string arg = argv[i]; + if (arg == "--svo" && i + 1 < argc) { + svo_path = argv[++i]; + } else if (arg == "--fisheye") { + is_radtan_lens = false; + } else if (arg == "--zedxone") { + is_zed_x_one_virtual_stereo = true; + } else if (arg == "--zed_sdk_format") { + is_zed_sdk_format = true; + } else if (arg == "--left_id" && i + 1 < argc) { + left_camera_id = std::stoi(argv[++i]); + } else if (arg == "--right_id" && i + 1 < argc) { + right_camera_id = std::stoi(argv[++i]); + } else if (arg == "--left_sn" && i + 1 < argc) { + left_camera_sn = std::stoi(argv[++i]); + } else if (arg == "--right_sn" && i + 1 < argc) { + right_camera_sn = std::stoi(argv[++i]); + } else if (arg == "--h_edges" && i + 1 < argc) { + h_edges = std::stoi(argv[++i]); + } else if (arg == "--v_edges" && i + 1 < argc) { + v_edges = std::stoi(argv[++i]); + } else if (arg == "--square_size" && i + 1 < argc) { + square_size = std::stof(argv[++i]); + } else if (arg == "--help" || arg == "-h") { + std::cout << "Usage: " << argv[0] << " [options]" << std::endl; + std::cout << " --h_edges Number of horizontal inner " + "edges of the checkerboard" + << std::endl; + std::cout << " --v_edges Number of vertical inner edges " + "of the checkerboard" + << std::endl; + std::cout << " --square_size Size of a square in the " + "checkerboard (in mm)" + << std::endl; + std::cout << " --svo Path to the SVO file" << std::endl; + std::cout << " --fisheye Use fisheye lens model" << std::endl; + std::cout << " --zedxone Use ZED X One cameras as a virtual " + "stereo pair" + << std::endl; + std::cout << " --left_id Id of the left camera if using " + "virtual stereo" + << std::endl; + std::cout << " --right_id Id of the right camera if using " + "virtual stereo" + << std::endl; + std::cout << " --left_sn S/N of the left camera if using " + "virtual stereo" + << std::endl; + std::cout << " --right_sn S/N of the right camera if using " + "virtual stereo" + << std::endl; + std::cout << " --zed_sdk_format Save calibration file in " + "ZED SDK format" + << std::endl; + std::cout << " --help, -h Show this help message" << std::endl + << std::endl; + std::cout << "Examples:" << std::endl; + std::cout << std::endl + << "* ZED Stereo Camera using an SVO file:" << std::endl; + std::cout << " " << argv[0] << " --svo camera.svo" << std::endl; + std::cout << std::endl + << "* Virtual Stereo Camera using camera IDs:" << std::endl; + std::cout << " " << argv[0] << " --zedxone --left_id 0 --right_id 1" + << std::endl; + std::cout << std::endl + << "* Virtual Stereo Camera using camera serial numbers and " + "a custom checkerboard:" + << std::endl; + std::cout << " " << argv[0] + << " --zedxone --left_sn 301528071 --right_sn 300473441 " + "--h_edges 12 --v_edges 9 --square_size 30.0" + << std::endl; + std::cout << std::endl + << "* Virtual Stereo Camera with fisheye lenses using camera " + "serial numbers:" + << std::endl; + std::cout + << " " << argv[0] + << " --fisheye --zedxone --left_sn 301528071 --right_sn 300473441" + << std::endl; + std::cout << std::endl; + exit(0); + } } + } }; +int main(int argc, char* argv[]) { + // Setup the calibration checker + const DetectedBoardParams idealParams = { + cv::Point2f(min_x_coverage, min_y_coverage), min_area_range, + min_skew_range}; + CalibrationChecker checker(cv::Size(h_edges, v_edges), square_size, + min_samples, max_samples, + min_target_area, idealParams, verbose); + // Coverage scores + float size_score = 0.0f, skew_score = 0.0f, pos_score_x = 0.0f, + pos_score_y = 0.0f; + + // Flags + bool is_dual_mono_camera = false; + bool is_4k_camera = false; + + Args args; + args.parse(argc, argv); + + std::cout << "*** Stereo Camera Calibration Tool ***" << std::endl; + std::cout << std::endl; + std::cout << "The calibration process requires a checkerboard of known " + "characteristics." + << std::endl; + std::cout << " * Expected checkerboard features:" << std::endl; + std::cout << " - Inner horizontal edges:\t" << h_edges << std::endl; + std::cout << " - Inner vertical edges:\t" << v_edges << std::endl; + std::cout << " - Square size:\t\t" << square_size << " mm" << std::endl; + std::cout << "Change these parameters using the command line options if " + "needed. Use the '-h' option for help." << std::endl; + std::cout << std::endl; + + sl::Camera zed_camera; + sl::InitParameters init_params; + init_params.depth_mode = + sl::DEPTH_MODE::NONE; // No depth required for calibration + init_params.camera_resolution = + sl::RESOLUTION::AUTO; // Use the camera's native resolution + init_params.camera_fps = 15; // Set the camera FPS + init_params.enable_image_validity_check = + false; // Disable image validity check for performance + init_params.camera_disable_self_calib = true; + init_params.sdk_verbose = sdk_verbose; + + // Configure the Virtual Stereo Camera if '--zedxone' argument is provided + if (args.is_zed_x_one_virtual_stereo) { + std::cout << " * Virtual Stereo Camera mode enabled." << std::endl; + is_dual_mono_camera = true; + int sn_left = args.left_camera_sn; + int sn_right = args.right_camera_sn; + + if (sn_left != -1 && sn_right != -1) { + std::cout << " * Using serial numbers for left and right cameras: " + << sn_left << ", " << sn_right << std::endl; + + int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); + std::cout << " * Unique Virtual SN: " << sn_stereo << " (Generated by the ZED SDK)" << std::endl; + init_params.input.setVirtualStereoFromSerialNumbers(sn_left, sn_right, + sn_stereo); + } else { + if (args.left_camera_id == -1 || args.right_camera_id == -1) { + std::cerr << "Error: Left and Right camera IDs or Left and Right " + "camera Serial Numbers must be both provided." + << std::endl; + std::cerr << " * use the command '" << args.app_name + << " -h' for details." << std::endl; + return EXIT_FAILURE; + } + + std::cout << "Using camera IDs for left and right cameras: " + << args.left_camera_id << ", " << args.right_camera_id + << std::endl; -int main(int argc, char *argv[]) { - - Args args; - args.parse(argc, argv); - - std::cout << std::endl; - std::cout << "The calibration process requires a checkerboard of known characteristics." << std::endl; - std::cout << "Expected checkerboard size: " << target_w << "x" << target_h << " - " << square_size << "mm" << std::endl; - std::cout << "Change those values in the code depending on the checkerboard you are using!" << std::endl; - std::cout << std::endl; - - sl::Camera zed_camera; - sl::InitParameters init_params; - init_params.depth_mode = sl::DEPTH_MODE::NONE; // No depth required for calibration - init_params.camera_resolution = sl::RESOLUTION::AUTO; // Use the camera's native resolution - init_params.camera_fps = 30; // Set the camera FPS - init_params.enable_image_validity_check = false; // Disable image validity check for performance - init_params.camera_disable_self_calib = true; - - if (args.is_zed_x_one_virtual_stereo) { // Setup virtual stereo camera from - // two ZED One cameras - // Use this method if you know the serial numbers of the left and right - // cameras - // init_params.input.setVirtualStereoFromSerialNumbers(sn_left, - // sn_right, sn_stereo); - const int sn_stereo = 110000000; - init_params.input.setVirtualStereoFromCameraIDs(0, 1, sn_stereo); + auto cams = sl::CameraOne::getDeviceList(); + + for (auto& cam : cams) { + if (cam.id == args.left_camera_id) { + sn_left = cam.serial_number; + } else if (cam.id == args.right_camera_id) { + sn_right = cam.serial_number; + } + } + + if (sn_left == -1 || sn_right == -1) { + std::cerr << "Error: Could not find serial numbers for the provided " + "camera IDs." + << std::endl; + std::cerr << " * use the command 'ZED_Explore --all' to get the camera " + "ID or the Serial Number of the connected cameras." + << std::endl; + return EXIT_FAILURE; + } + + int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); + std::cout << " * Unique Virtual SN: " << sn_stereo + << " (Generated by the ZED SDK)" << std::endl; + + init_params.input.setVirtualStereoFromCameraIDs( + args.left_camera_id, args.right_camera_id, sn_stereo); } - auto status = zed_camera.open(init_params); + int left_model = sn_left / 10000000; + int right_model = sn_right / 10000000; - // in case of a virtual stereo camera, the calibration file can be not available - if(status > sl::ERROR_CODE::SUCCESS && status != sl::ERROR_CODE::INVALID_CALIBRATION_FILE) { - std::cerr << "Error opening ZED camera: " << sl::toString(status) << std::endl; - return 1; + if (left_model != right_model) { + std::cerr << "Error: Left and Right cameras must be of the same model." + << std::endl; + return EXIT_FAILURE; } - - // change can_use_calib_prior if you dont want to use the calibration file - const bool can_use_calib_prior = status != sl::ERROR_CODE::INVALID_CALIBRATION_FILE; - bool need_intrinsic_estimation = !can_use_calib_prior; - std::cout << "Using prior calibration: " << (can_use_calib_prior ? "Yes" : "No") << std::endl; + if (left_model == static_cast(sl::MODEL::ZED_XONE_UHD) && + right_model == static_cast(sl::MODEL::ZED_XONE_UHD)) { + is_4k_camera = true; + init_params.camera_resolution = sl::RESOLUTION::HD4K; + std::cout << " * ZED X One 4K Virtual Stereo Camera detected." + << std::endl; + } else { + is_4k_camera = false; + init_params.camera_resolution = sl::RESOLUTION::HD1200; + std::cout << " * ZED X One GS Virtual Stereo Camera detected." + << std::endl; + } + } + + auto status = zed_camera.open(init_params); - StereoCalib calib; - calib.initDefault(args.is_radtan_lens); + // in case of a virtual stereo camera, the calibration file can be not + // available + if (status > sl::ERROR_CODE::SUCCESS && + status != sl::ERROR_CODE::INVALID_CALIBRATION_FILE) { + std::cerr << "Error opening ZED camera: " << sl::toString(status) + << std::endl; + return 1; + } - auto zed_info = zed_camera.getCameraInformation(); + // change can_use_calib_prior if you dont want to use the calibration file + const bool can_use_calib_prior = + status != sl::ERROR_CODE::INVALID_CALIBRATION_FILE; - if(can_use_calib_prior) - calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); + std::cout << " * Using prior calibration: " + << (can_use_calib_prior ? "Yes" : "No") << std::endl; - sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; + StereoCalib calib; + calib.initDefault(args.is_radtan_lens); + std::cout << " * Lens distorsion model: " + << (args.is_radtan_lens ? "Radial-Tangential" : "Fisheye") + << std::endl; - sl::Mat zed_imageL(camera_resolution, sl::MAT_TYPE::U8_C4, sl::MEM::CPU); - auto rgb_l = cv::Mat(camera_resolution.height, camera_resolution.width, CV_8UC4, zed_imageL.getPtr()); + auto zed_info = zed_camera.getCameraInformation(); - sl::Mat zed_imageR(camera_resolution, sl::MAT_TYPE::U8_C4, sl::MEM::CPU); - auto rgb_r = cv::Mat(camera_resolution.height, camera_resolution.width, CV_8UC4, zed_imageR.getPtr()); + if (can_use_calib_prior) + calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); - // Number of area to fill 4 horizontally - bucketsize = camera_resolution.width/4; + sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; - bool frames_rot_good=true; - cv::Mat blank = cv::Mat::zeros(camera_resolution.height, camera_resolution.width, CV_8UC1); + sl::Mat zed_imageL(camera_resolution, sl::MAT_TYPE::U8_C4, sl::MEM::CPU); + auto rgb_l = cv::Mat(camera_resolution.height, camera_resolution.width, + CV_8UC4, zed_imageL.getPtr()); - cv::Mat rgb_d, rgb2_d, rgb_d_fill, display, rendering_image; + sl::Mat zed_imageR(camera_resolution, sl::MAT_TYPE::U8_C4, sl::MEM::CPU); + auto rgb_r = cv::Mat(camera_resolution.height, camera_resolution.width, + CV_8UC4, zed_imageR.getPtr()); - extrinsic_checker checker; - float cov_left = 100; - bool angle_clb = false; - std::vector pts_obj_; - for (int i = 0; i < target_h; i++) { - for (int j = 0; j < target_w; j++) { - pts_obj_.push_back(cv::Point3f(square_size*j, square_size*i, 0)); - } + cv::Mat coverage_indicator = + cv::Mat::zeros(display_size.height, display_size.width, CV_8UC1); + + cv::Mat pos_indicator = + cv::Mat::zeros(display_size.height, display_size.width, CV_8UC1); + + cv::Mat rgb_d, rgb2_d, rgb_d_fill, rgb2_d_fill, display, rendering_image; + + bool acquisition_completed = false; + std::vector pts_obj_; + for (int i = 0; i < v_edges; i++) { + for (int j = 0; j < h_edges; j++) { + pts_obj_.push_back(cv::Point3f(square_size * j, square_size * i, 0.0)); } + } - // Check if the temp image folder exists and clear it - if (fs::exists(folder)) { - std::uintmax_t n{fs::remove_all(folder)}; + // Check if the temp image folder exists and clear it + if (fs::exists(image_folder)) { + std::uintmax_t n{fs::remove_all(image_folder)}; + if (verbose) { + std::cout << "[DEBUG][main] * Removed " << n + << " temporary files or directories from previous calibration." + << std::endl; } - // Create the temp image folder - if(!fs::create_directories(folder)) - { - std::cerr << "Error creating storage folder!"; - return 1; + } + // Create the temp image folder + if (!fs::create_directories(image_folder)) { + std::cerr << "Error creating storage folder!"; + return 1; + } + + char key = ' '; + int image_count = 0; + // bool coverage_mode = false; + bool missing_target_on_last_pics = false; + bool low_target_variability_on_last_pics = false; + + const std::string window_name = "ZED Calibration"; + cv::namedWindow(window_name, cv::WINDOW_KEEPRATIO); + cv::resizeWindow(window_name, display_size.width * 2, + display_size.height + text_area_height); + + while (1) { + if (key == 'q' || key == 'Q' || key == 27) { + std::cout << "Calibration aborted by user." << std::endl; + zed_camera.close(); + return EXIT_SUCCESS; } - - std::vector> pts_init_im_l, pts_init_im_r; - std::vector> pts_init_obj; - - const cv::Size display_size(720, 404); - - char key = ' '; - int image_count = 0; - bool coverage_mode = false; - bool missing_left_target_on_last_pic = false; - bool missing_right_target_on_last_pic = false; - - const std::string window_name = "ZED Calibration"; - cv::namedWindow(window_name, cv::WINDOW_NORMAL); - cv::resizeWindow(window_name, 1280, 960); // Set a larger window size - - while (key != 'q' && key != 27) { - - if (zed_camera.grab() == sl::ERROR_CODE::SUCCESS) { - zed_camera.retrieveImage(zed_imageL, sl::VIEW::LEFT_UNRECTIFIED); - zed_camera.retrieveImage(zed_imageR, sl::VIEW::RIGHT_UNRECTIFIED); - - cv::resize(rgb_l, rgb_d, display_size); - cv::resize(rgb_r, rgb2_d, display_size); - - if (!angle_clb) { - cv::Mat rgb_with_lack_of_pts; - std::vector channels; - cv::split(rgb_l, channels); - blank.setTo(0); - float x_end,y_end; - float x_max = 0; - for (int i = 0; i < square_valid.size(); i++) { - if(square_valid.at(i).x + bucketsize > blank.size[1]) - x_end = blank.size[1]; - else - x_end = square_valid.at(i).x + bucketsize; - if(square_valid.at(i).y + bucketsize > blank.size[0]) - y_end = blank.size[0]; - else - y_end = square_valid.at(i).y + bucketsize; - if(square_valid.at(i).x>x_max) - x_max = square_valid.at(i).x; - cv::rectangle(blank, square_valid.at(i), cv::Point(x_end, y_end), cv::Scalar(128, 0, 128), -1); - } - channels[0] = channels[0] - blank; - channels[2] = channels[2] - blank; - cv::merge(channels, rgb_with_lack_of_pts); - cv::resize(rgb_with_lack_of_pts, rgb_d_fill, display_size); - } else { - cv::resize(rgb_l, rgb_d_fill, display_size); - } - std::vector pts; - bool found = cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts, 3); - drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), cv::Mat(pts), found); - - if(image_stack_horizontal) - cv::hconcat(rgb_d_fill, rgb2_d, display); - else - cv::vconcat(rgb_d_fill, rgb2_d, display); - - cv::Mat text_info = cv::Mat::ones(cv::Size(display.size[1], 200), display.type()); - - if (angle_clb) { - coverage_mode = false; - bool ready_to_calibrate = writeRotText(text_info, checker.rot_x_delta, checker.rot_y_delta, checker.rot_z_delta, checker.distance_tot, 1); - if(ready_to_calibrate) { - if(image_count >= MIN_IMAGE) - break; - else - cv::putText(text_info, "Not enough images for calibration", cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - } - } + if (zed_camera.grab() == sl::ERROR_CODE::SUCCESS) { + zed_camera.retrieveImage(zed_imageL, sl::VIEW::LEFT_UNRECTIFIED); + zed_camera.retrieveImage(zed_imageR, sl::VIEW::RIGHT_UNRECTIFIED); + + cv::resize(rgb_l, rgb_d, display_size); + cv::resize(rgb_r, rgb2_d, display_size); + cv::resize(rgb_l, rgb_d_fill, display_size); + + applyCoverageIndicatorOverlay(rgb_d_fill, coverage_indicator); + applyPosIndicatorOverlay(rgb_d_fill, pos_indicator); + + std::vector pts_l, pts_r; + bool found_l = false; + bool found_r = false; + found_l = + cv::findChessboardCorners(rgb_d, cv::Size(h_edges, v_edges), pts_l); + drawChessboardCorners(rgb_d_fill, cv::Size(h_edges, v_edges), + cv::Mat(pts_l), found_l); + if (found_l) { + found_r = cv::findChessboardCorners(rgb2_d, cv::Size(h_edges, v_edges), + pts_r); + drawChessboardCorners(rgb2_d, cv::Size(h_edges, v_edges), + cv::Mat(pts_r), found_r); + } - cv::vconcat(display, text_info, rendering_image); - - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << square_size; - - if(missing_right_target_on_last_pic) - cv::putText(rendering_image, "Missing target on image right", cv::Point(10, display.size[0]+140), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - if(missing_left_target_on_last_pic) - cv::putText(rendering_image, "Missing target on image left", cv::Point(10, display.size[0]+110), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - - cv::putText(rendering_image, "Press 's' to save the current frames", cv::Point(10, display.size[0]+25), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); - if(coverage_mode) - cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); - if(!frames_rot_good) - cv::putText(rendering_image, "!!! Do not rotate the checkerboard more than 45 deg around Z !!!", - cv::Point(600, display.size[0]+25), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - - cv::imshow(window_name, rendering_image); - key = cv::waitKey(10); - - if ((key == 's' || key == 'S')) { - if (!angle_clb) coverage_mode = true; - std::vector pts_l, pts_r; - bool found_l = cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts_l); - bool found_r = cv::findChessboardCorners(rgb2_d, cv::Size(target_w, target_h), pts_r); - - if(!found_r) - missing_right_target_on_last_pic = true; - else - missing_right_target_on_last_pic = false; - if(!found_l) - missing_left_target_on_last_pic = true; - else - missing_left_target_on_last_pic = false; - - if (found_l && found_r) { - - scaleKP(pts_l, display_size, cv::Size(camera_resolution.width, camera_resolution.height)); - - if(need_intrinsic_estimation) { - pts_init_im_l.push_back(pts_l); - - scaleKP(pts_r, display_size, cv::Size(camera_resolution.width, camera_resolution.height)); - pts_init_im_r.push_back(pts_r); - - pts_init_obj.push_back(pts_obj_); - // wait 3 images before running the estimate - if(pts_init_im_l.size()>3){ - calib.left.K = cv::initCameraMatrix2D(pts_init_obj, pts_init_im_l, cv::Size(camera_resolution.width, camera_resolution.height)); - calib.right.K = cv::initCameraMatrix2D(pts_init_obj, pts_init_im_r, cv::Size(camera_resolution.width, camera_resolution.height)); - need_intrinsic_estimation = false; - } - }else{ - if (!angle_clb) { - pts_detected.push_back(pts_l); - cov_left = CheckCoverage(pts_detected, cv::Size(camera_resolution.width, camera_resolution.height)); - std::cout << "coverage : " << (1-cov_left)*100 << "%" << std::endl; - if (cov_left < 0.1) { - cv::Mat rvec(1, 3, CV_64FC1); - cv::Mat tvec(1, 3, CV_64FC1); - - auto undist_pts = calib.left.undistortPoints(pts_l); - - cv::Mat empty_dist; - cv::solvePnP(pts_obj_, undist_pts, calib.left.K, empty_dist, rvec, tvec, false, cv::SOLVEPNP_EPNP); - - double rx = rvec.at(0)*(180 / M_PI); - double ry = rvec.at(1)*(180 / M_PI); - double rz = rvec.at(2)*(180 / M_PI); - - checker.rot_x_min = rx; - checker.rot_x_max = rx; - checker.rot_y_min = ry; - checker.rot_y_max = ry; - checker.rot_z_min = rz; - checker.rot_z_max = rz; - - checker.d_min = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); - checker.d_max = checker.d_min; - - angle_clb = true; - } - } else { - cv::Mat rvec(1, 3, CV_64FC1); - cv::Mat tvec(1, 3, CV_64FC1); - auto undist_pts = calib.left.undistortPoints(pts_l); - cv::Mat empty_dist; - bool found_ = cv::solvePnP(pts_obj_, undist_pts, calib.left.K, empty_dist, rvec, tvec, false, cv::SOLVEPNP_EPNP); - if (found_) { - frames_rot_good = updateRT(checker, rvec, tvec); - if(frames_rot_good) - pts_detected.push_back(pts_l); - } - } - } - } - - if(frames_rot_good) { - // saves the images - cv::imwrite(folder + "image_left_" + std::to_string(image_count) + ".png", rgb_l); - cv::imwrite(folder + "image_right_" + std::to_string(image_count) + ".png", rgb_r); - std::cout << " * Images saved" << std::endl; - image_count++; - } - } + if (image_stack_horizontal) { + cv::hconcat(rgb_d_fill, rgb2_d, display); + } else { + cv::vconcat(rgb_d_fill, rgb2_d, display); + } + + cv::Mat text_info = cv::Mat::ones( + cv::Size(display.size[1], text_area_height), display.type()); + cv::vconcat(display, text_info, rendering_image); + + if (acquisition_completed) { + cv::putText(rendering_image, + "Acquisition completed! Wait for the calibration " + "computation to complete...", + cv::Point(20, display.size[0] + 50), + cv::FONT_HERSHEY_SIMPLEX, 0.7, info_color, 2); + cv::putText(rendering_image, + "Follow the console log for calibration progress details.", + cv::Point(20, display.size[0] + 80), + cv::FONT_HERSHEY_SIMPLEX, 0.7, info_color, 2); + } else { + if (missing_target_on_last_pics || + low_target_variability_on_last_pics) { + cv::putText(rendering_image, "Frames not stored for calibration.", + cv::Point(display.size[1] / 2, display.size[0] + 80), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); } - sl::sleep_ms(10); - } - int err = calibrate(folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior); - if (err == 0) - std::cout << "CALIBRATION success" << std::endl; - else - std::cout << "CALIBRATION fail" << std::endl; - zed_camera.close(); + if (missing_target_on_last_pics) { + cv::putText(rendering_image, + " * Missing target on one of the cameras.", + cv::Point(display.size[1] / 2, display.size[0] + 110), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); + } - return 0; -} + if (low_target_variability_on_last_pics) { + cv::putText(rendering_image, + " * Target too similar to a previous acquisition or too small.", + cv::Point(display.size[1] / 2, display.size[0] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); + } -// Function to perform linear interpolation -inline float interpolate(float x, float x0, float x1, float y0=0, float y1=100) { - float interpolatedValue = y0 + (x - x0) * (y1 - y0) / (x1 - x0); - interpolatedValue = (interpolatedValue < y0) ? y0 : ((interpolatedValue > y1) ? y1 : interpolatedValue); // clamp - return interpolatedValue; -} + cv::putText( + rendering_image, + "Press 's' or the spacebar to store the current frames when " + "the target is visible in both images.", + cv::Point(10, display.size[0] + 25), cv::FONT_HERSHEY_SIMPLEX, 0.7, + info_color, 1); + + std::stringstream ss_status; + ss_status << " * Horizontal coverage: " << std::fixed + << std::setprecision(2) << pos_score_x * 100.0f << "%"; + cv::putText(rendering_image, ss_status.str(), + cv::Point(10, display.size[0] + 55), + cv::FONT_HERSHEY_SIMPLEX, 0.7, + (pos_score_x > 1.0f ? info_color : warn_color), 1); + ss_status.str(""); + ss_status << " * Vertical coverage: " << std::fixed + << std::setprecision(2) << pos_score_y * 100.0f << "%"; + cv::putText(rendering_image, ss_status.str(), + cv::Point(10, display.size[0] + 85), + cv::FONT_HERSHEY_SIMPLEX, 0.7, + (pos_score_y > 1.0f ? info_color : warn_color), 1); + ss_status.str(""); + ss_status << " * Checkerboard sizes: " << std::fixed + << std::setprecision(2) << size_score * 100.0f << "%"; + cv::putText(rendering_image, ss_status.str(), + cv::Point(10, display.size[0] + 115), + cv::FONT_HERSHEY_SIMPLEX, 0.7, + (size_score > 1.0f ? info_color : warn_color), 1); + ss_status.str(""); + ss_status << " * Checkerboard skews: " << std::fixed + << std::setprecision(2) << skew_score * 100.0f << "%"; + cv::putText(rendering_image, ss_status.str(), + cv::Point(10, display.size[0] + 145), + cv::FONT_HERSHEY_SIMPLEX, 0.7, + (skew_score > 1.0f ? info_color : warn_color), 1); + + std::stringstream ss_img_count; + ss_img_count << " * Sample saved: " << image_count << " [min. " + << min_samples << "]"; + cv::putText(rendering_image, ss_img_count.str(), + cv::Point(10, display.size[0] + 175), + cv::FONT_HERSHEY_SIMPLEX, 0.7, + (image_count > min_samples ? info_color : warn_color), 1); + + cv::putText(rendering_image, + "Move the target horizontally, vertically, forward " + "and backward, and rotate it to improve " + "coverage and variability scores. Framerate can be low if " + "no target is detected.", + cv::Point(10, display.size[0] + 200), + cv::FONT_HERSHEY_SIMPLEX, 0.5, warn_color, 1); + } -bool writeRotText(cv::Mat& image, float rot_x, float rot_y, float rot_z, float distance, int fontSize) { - bool status = false; - // Define text from rotation and distance - - // Convert float values to string with two decimal places - std::stringstream ss_rot_x, ss_rot_y, ss_rot_z, ss_distance; - - int rot_x_idx = interpolate(rot_x, 0, min_rotation); - int rot_y_idx = interpolate(rot_y, 0, min_rotation); - int rot_z_idx = interpolate(rot_z, 0, min_rotation); - int distance_idx = interpolate(distance, 0, min_distance); - - ss_rot_x << "Rotation X: " << std::fixed << std::setprecision(1) << rot_x_idx << "% "; - ss_rot_y << " / Rotation Y: " << std::fixed << std::setprecision(1) << rot_y_idx << "% "; - ss_rot_z << " / Rotation Z: " << std::fixed << std::setprecision(1) << rot_z_idx << "% "; - ss_distance << " / Distance: " << std::fixed << std::setprecision(1) << distance_idx << "%"; - - std::string text1 = ss_rot_x.str(); - std::string text2 = ss_rot_y.str(); - std::string text3 = ss_rot_z.str(); - std::string text4 = ss_distance.str(); - - std::string text = text1 + text2 + text3 + text4; - - cv::Scalar color1, color2, color3, color4; - - // Condition on colors - if (rot_x > min_rotation) - color1 = cv::Scalar(0, 255, 0); - else if (rot_x >= acceptable_rotation) - color1 = warn_color; - else - color1 = cv::Scalar(0, 0, 255); - - if (rot_y > min_rotation) - color2 = cv::Scalar(0, 255, 0); - else if (rot_y >= acceptable_rotation) - color2 = warn_color; - else - color2 = cv::Scalar(0, 0, 255); - - if (rot_z > min_rotation) - color3 = cv::Scalar(0, 255, 0); - else if (rot_z >= acceptable_rotation) - color3 = warn_color; - else - color3 = cv::Scalar(0, 0, 255); - - if (distance > min_distance) - color4 = cv::Scalar(0, 255, 0); - else if (distance >= acceptable_distance) - color4 = warn_color; - else - color4 = cv::Scalar(0, 0, 255); - - // Get image dimensions - int width = image.cols; - int height = image.rows; - - // Calculate text size - int baseline = 0; - cv::Size textSize = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, fontSize, 1, &baseline); - - // Calculate text position - int x = (width - textSize.width) / 2; - int y = (height + textSize.height) / 2; - - status = (rot_x > min_rotation) && (rot_y > min_rotation) && (rot_z > min_rotation) && (distance > min_distance); - - // Draw text on image with different colors - cv::putText(image, text1, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, fontSize, color1, 2); - cv::putText(image, text2, cv::Point(x + textSize.width / 4, y), cv::FONT_HERSHEY_SIMPLEX, fontSize, color2, 2); - cv::putText(image, text3, cv::Point(x + textSize.width / 2, y), cv::FONT_HERSHEY_SIMPLEX, fontSize, color3, 2); - cv::putText(image, text4, cv::Point(x + 3 * textSize.width / 4, y), cv::FONT_HERSHEY_SIMPLEX, fontSize, color4, 2); - - if(status) - cv::putText(image, "Ready to calibrate...", cv::Point(10, y-40), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); - - return status; -} + cv::imshow(window_name, rendering_image); + key = cv::waitKey(10); + + if (acquisition_completed) { + std::cout << " *** Starting the calibration process ***" << std::endl; + break; + } -bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, std::vector> pts) { - int count = 0; + if ((key == 's' || key == 'S') || key == ' ') { + std::cout << "************************************************" + << std::endl; + + missing_target_on_last_pics = !found_r || !found_l; + + if (found_l && found_r) { + scaleKP(pts_l, display_size, + cv::Size(camera_resolution.width, camera_resolution.height)); + scaleKP(pts_r, display_size, + cv::Size(camera_resolution.width, camera_resolution.height)); + + if (checker.testSample(pts_l, cv::Size(camera_resolution.width, + camera_resolution.height))) { + low_target_variability_on_last_pics = false; + + std::cout << " * Valid sample detected. Total valid samples: " + << checker.getValidSampleCount() << std::endl; + + // saves the images + cv::imwrite(image_folder + "image_left_" + + std::to_string(image_count) + ".png", + rgb_l); + cv::imwrite(image_folder + "image_right_" + + std::to_string(image_count) + ".png", + rgb_r); + std::cout << " * Images saved" << std::endl; + image_count++; + + if (checker.evaluateSampleCollectionStatus( + size_score, skew_score, pos_score_x, pos_score_y)) { + std::cout << ">>> Sample collection status: COMPLETE <<<" + << std::endl + << std::endl; + acquisition_completed = true; + } - for (int i = 0; i < pts.size(); i++) { - for (int j = 0; j < pts.at(i).size(); j++) { - if ((pts.at(i).at(j).x < max_w) && (pts.at(i).at(j).x > min_w)) { - if ((pts.at(i).at(j).y < max_h) && (pts.at(i).at(j).y > min_h)) { - count++; + // Add the new checkerboard position to the coverage indicator + float norm_x = checker.getLastDetectedBoardParams().pos.x; + float norm_y = checker.getLastDetectedBoardParams().pos.y; + float norm_size = checker.getLastDetectedBoardParams().size; + addNewCheckerboardPosition(coverage_indicator, pos_indicator, + norm_x, norm_y, norm_size); + } else { + std::cout << " ! Sample detected but not valid. Please try again with a new position." + << std::endl; + low_target_variability_on_last_pics = true; + } } } } } - if (min) { - if (count > MinPts) - return true; - else - return false; - } else { - if (count < MaxPts) - return true; - else - return false; - } + // Start the calibration process + int err = + calibrate(image_count, image_folder, calib, h_edges, v_edges, square_size, + zed_info.serial_number, is_dual_mono_camera, is_4k_camera, + false, can_use_calib_prior, max_repr_error, verbose); + if (err == EXIT_SUCCESS) + std::cout << std::endl << " +++++ Calibration successful +++++" << std::endl; + else + std::cout << std::endl << " ----- Calibration failed -----" << std::endl; -} + zed_camera.close(); -float CheckCoverage(const std::vector>& pts, const cv::Size& imgSize) { - int min_h_ = 0; - int max_h_ = bucketsize; - float tot = 0; - float error = 0; - - while (min_h_ < imgSize.height) { - if (max_h_ > imgSize.height) - max_h_ = imgSize.height; - int min_w_ = 0; - int max_w_ = bucketsize; - while (min_w_ < imgSize.width) { - if (max_w_ > imgSize.width) - max_w_ = imgSize.width; - if (!CheckBucket(min_h_, max_h_, min_w_, max_w_, true, pts)) { - error++; - } else - square_valid.push_back(cv::Point(min_w_, min_h_)); - min_w_ += bucketsize; - max_w_ += bucketsize; - tot++; - - } - min_h_ += bucketsize; - max_h_ += bucketsize; - } - return error / tot; + return EXIT_SUCCESS; } -bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat tvec) { - double rx = r.at(0)*(180 / M_PI); - double ry = r.at(1)*(180 / M_PI); - double rz = r.at(2)*(180 / M_PI); - - std::cout << "* Rot X: " << rx << "°" << std::endl; - std::cout << "* Rot Y: " << ry << "°" << std::endl; - std::cout << "* Rot Z: " << rz << "°" << std::endl; +static int top_left_count = 0; +static int top_right_count = 0; +static int bottom_left_count = 0; +static int bottom_right_count = 0; + +void addNewCheckerboardPosition(cv::Mat& coverage_indicator, + cv::Mat& pos_indicator, float norm_x, + float norm_y, float norm_size) { + int x = static_cast(norm_x * coverage_indicator.cols); + int y = static_cast(norm_y * coverage_indicator.rows); + int size = static_cast(norm_size * 20.0f); + cv::circle(pos_indicator, cv::Point(x, y), size, cv::Scalar(255, 255, 255), + -1); + + if (norm_x < 0.5f && norm_y < 0.5f) { + top_left_count++; + } else if (norm_x >= 0.5f && norm_y < 0.5f) { + top_right_count++; + } else if (norm_x < 0.5f && norm_y >= 0.5f) { + bottom_left_count++; + } else { + bottom_right_count++; + } - if(fabs(rz)>45.0) - { - std::cerr << " * Images ignored: Rot Z > 45° ["<< rz <<"°]" << std::endl; - return false; - } + if (top_left_count >= min_samples / 4) { + cv::rectangle( + coverage_indicator, cv::Point(0, 0), + cv::Point(coverage_indicator.cols / 2, coverage_indicator.rows / 2), + cv::Scalar(255), -1); + } + if (top_right_count >= min_samples / 4) { + cv::rectangle( + coverage_indicator, cv::Point(coverage_indicator.cols / 2, 0), + cv::Point(coverage_indicator.cols, coverage_indicator.rows / 2), + cv::Scalar(255), -1); + } + if (bottom_left_count >= min_samples / 4) { + cv::rectangle( + coverage_indicator, cv::Point(0, coverage_indicator.rows / 2), + cv::Point(coverage_indicator.cols / 2, coverage_indicator.rows), + cv::Scalar(255), -1); + } + if (bottom_right_count >= min_samples / 4) { + cv::rectangle( + coverage_indicator, + cv::Point(coverage_indicator.cols / 2, coverage_indicator.rows / 2), + cv::Point(coverage_indicator.cols, coverage_indicator.rows), + cv::Scalar(255), -1); + } +} - // check min - if (checker_.rot_x_min > rx) - checker_.rot_x_min = rx; - if (checker_.rot_y_min > ry) - checker_.rot_y_min = ry; - if (checker_.rot_z_min > rz) - checker_.rot_z_min = rz; - - //check max - if (checker_.rot_x_max < rx) - checker_.rot_x_max = rx; - if (checker_.rot_y_max < ry) - checker_.rot_y_max = ry; - if (checker_.rot_z_max < rz) - checker_.rot_z_max = rz; - - if (checker_.d_min > sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2))) - checker_.d_min = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); - if (checker_.d_max < sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2))) - checker_.d_max = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); - - // compute deltas - checker_.rot_x_delta = checker_.rot_x_max - checker_.rot_x_min; - checker_.rot_y_delta = checker_.rot_y_max - checker_.rot_y_min; - checker_.rot_z_delta = checker_.rot_z_max - checker_.rot_z_min; - checker_.distance_tot = checker_.d_max - checker_.d_min; - - std::cout << " * delta rot x: " << checker_.rot_x_delta << std::endl; - std::cout << " * delta rot y: " << checker_.rot_y_delta << std::endl; - std::cout << " * delta rot z: " << checker_.rot_z_delta << std::endl; - std::cout << " * delta dist: " << checker_.distance_tot << std::endl; - - return true; +void applyCoverageIndicatorOverlay(cv::Mat& image, + const cv::Mat& coverage_indicator) { + std::vector channels; + cv::split(image, channels); + channels[0] = channels[0] - coverage_indicator; + channels[2] = channels[2] - coverage_indicator; + cv::merge(channels, image); } + +void applyPosIndicatorOverlay(cv::Mat& image, const cv::Mat& pos_indicator) { + std::vector channels; + cv::split(image, channels); + channels[2] = channels[2] - pos_indicator; + channels[1] = channels[1] - pos_indicator; + cv::merge(channels, image); +} \ No newline at end of file diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index cce5765..6eb7b44 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,116 +1,441 @@ #include "opencv_calibration.hpp" -int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, int target_h, float square_size, int serial, bool save_calib_mono, bool use_intrinsic_prior){ - - std::vector left_images, right_images; - - /// Read images - cv::Size imageSize = cv::Size(0, 0); - int img_number = 0; - - while (1) { - cv::Mat grey_l = cv::imread(folder + "image_left_" + std::to_string(img_number) + ".png", cv::IMREAD_GRAYSCALE); - cv::Mat grey_r = cv::imread(folder + "image_right_" + std::to_string(img_number) + ".png", cv::IMREAD_GRAYSCALE); - if (!grey_l.empty() && !grey_r.empty()) { - if (imageSize.width == 0) - imageSize = grey_l.size(); - else - if (imageSize != left_images.back().size()) { - std::cout << "Image number " << img_number << " does not have the same size as the previous ones"<< imageSize<<" vs "<< left_images.back().size() << std::endl; - break; - } - - left_images.push_back(grey_l); - right_images.push_back(grey_r); - }else break; - img_number++; +int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, + int h_edges, int v_edges, float square_size, int serial, + bool is_dual_mono, bool is_4k, bool save_calib_mono, + bool use_intrinsic_prior, float max_repr_error, bool verbose) { + std::vector left_images, right_images; + + /// Read images + cv::Size imageSize = cv::Size(0, 0); + + std::cout << std::endl + << "Loading the stored images from folder: " << folder << std::endl; + + for (int i = 0; i < img_count; i++) { + std::cout << "." << std::flush; + cv::Mat grey_l = + cv::imread(folder + "image_left_" + std::to_string(i) + ".png", + cv::IMREAD_GRAYSCALE); + cv::Mat grey_r = + cv::imread(folder + "image_right_" + std::to_string(i) + ".png", + cv::IMREAD_GRAYSCALE); + + if (!grey_l.empty() && !grey_r.empty()) { + if (imageSize.width == 0) + imageSize = grey_l.size(); + else if (imageSize != left_images.back().size()) { + std::cerr << std::endl + << " !!! ERROR !!! " << std::endl + << "Frames number #" << i + << " do not have the same size as the previous ones: " + << imageSize << " vs " << left_images.back().size() + << std::endl; + return EXIT_FAILURE; + } + + left_images.push_back(grey_l); + right_images.push_back(grey_r); } + } + + std::cout << std::endl + << " * " << left_images.size() << " samples collected" << std::endl; + + // Define object points of the target + std::vector pattern_points; + for (int i = 0; i < v_edges; i++) { + for (int j = 0; j < h_edges; j++) { + pattern_points.push_back( + cv::Point3f(square_size * j, square_size * i, 0)); + } + } + + std::vector> object_points; + std::vector> pts_l, pts_r; - std::cout << "\n\t"<< left_images.size() << " images opened" << std::endl; + cv::Size t_size(h_edges, v_edges); - // Define object points of the target - std::vector pattern_points; - for (int i = 0; i < target_h; i++) { - for (int j = 0; j < target_w; j++) { - pattern_points.push_back(cv::Point3f(square_size*j, square_size*i, 0)); - } + std::cout << "Detecting the target corners on the images" << std::endl; + + for (int i = 0; i < left_images.size(); i++) { + std::cout << "." << std::flush; + std::vector pts_l_, pts_r_; + bool found_l = + cv::findChessboardCorners(left_images.at(i), t_size, pts_l_, 3); + bool found_r = + cv::findChessboardCorners(right_images.at(i), t_size, pts_r_, 3); + + if (found_l && found_r) { + cv::cornerSubPix( + left_images.at(i), pts_l_, cv::Size(5, 5), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, + 30, 0.001)); + + cv::cornerSubPix( + right_images.at(i), pts_r_, cv::Size(5, 5), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, + 30, 0.001)); + + pts_l.push_back(pts_l_); + pts_r.push_back(pts_r_); + object_points.push_back(pattern_points); + } else { + std::cout << std::endl + << "- No valid targets detected on frames #" << i << " -" << std::endl; } + } + + /// Compute calibration + + if (pts_l.size() < MIN_IMAGE) { + std::cout << " !!! Not enough images with the target detected !!!" + << std::endl; + std::cout << " Please perform a new data acquisition." << std::endl + << std::endl; + return EXIT_FAILURE; + } + + std::cout << std::endl << " * Valid samples: " << pts_l.size() << "/" << img_count + << std::endl; + + auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; + + std::cout << "Left camera calibration... " << std::flush; + auto rms_l = calib_data.left.mono_calibrate(object_points, pts_l, imageSize, flags, verbose); + std::cout << "Done." << std::endl; + + std::cout << "Right camera calibration... " << std::flush; + auto rms_r = calib_data.right.mono_calibrate(object_points, pts_r, imageSize, flags, verbose); + std::cout << "Done." << std::endl; + + std::cout << "Stereo calibration... " << std::flush; + + auto err = calib_data.stereo_calibrate( + object_points, pts_l, pts_r, imageSize, + cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); + + std::cout << "Done." << std::endl; + + std::cout << std::endl << "*** Calibration Report ***" << std::endl; + + std::cout << " * Reprojection errors: " << std::endl; + std::cout << " * Left:\t" << rms_l << " px" + << (rms_l > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; + std::cout << " * Right:\t" << rms_r << " px" + << (rms_r > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; + std::cout << " * Stereo:\t" << err << " px" + << (err > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; + if (rms_l > max_repr_error || rms_r > max_repr_error || + err > max_repr_error) { + std::cerr << std::endl + << "\t!!! ERROR !!!" << std::endl + << "The max reprojection error looks too high (> " + << max_repr_error + << " px). Check that the lenses are clean (sharp images)" + " and that the calibration pattern is printed/mounted on a RIGID " + "and FLAT surface." + << std::endl; + + return EXIT_FAILURE; + } + + if (calib_data.left.K.type() == CV_64F) { + std::cout << " * Data type: double" << std::endl; + } else if (calib_data.left.K.type() == CV_32F) { + std::cout << " * Data type: float" << std::endl; + } else { + std::cerr << " !!! Cannot save the calibration file: 'Invalid data type'" + << std::endl; + return EXIT_FAILURE; + } + + if (calib_data.T.at(0) > 0) { + std::cerr << std::endl + << "\t !! Warning !!" << std::endl + << "The value of the baseline has opposite sign (T_x = " + << calib_data.T.at(0) << ")." << std::endl; + std::cerr << "Swap left and right cameras and redo the calibration." + << std::endl; + + return EXIT_FAILURE; + } + + if (calib_data.T.at(0) > 0) { + std::cerr + << std::endl + << "\t !! Warning !!" << std::endl + << "The value of the baseline has opposite sign than expected(T_x = " + << calib_data.T.at(0) << ")." << std::endl; + std::cerr << "Swap left and right cameras and redo the calibration." + << std::endl; + + return EXIT_FAILURE; + } + + constexpr float MIN_BASELINE = 30.0f; // Minimum possible baseline in mm + + if (fabs(calib_data.T.at(0)) < MIN_BASELINE) { + std::cerr << std::endl + << "\t !! Warning !!" << std::endl + << "The value of the baseline is too small (T_x = " + << calib_data.T.at(0) << ")." << std::endl; + std::cerr << "Please redo the calibration to obtain a value that is " + "phisically coherent (at least " + << MIN_BASELINE << " mm)." << std::endl; + + return EXIT_FAILURE; + } + + std::cout << std::endl; - std::vector> object_points; - std::vector> pts_l, pts_r; + std::cout << "** Camera parameters **" << std::endl; + std::cout << "* Intrinsic mat left:" << std::endl + << calib_data.left.K << std::endl; + std::cout << "* Distortion mat left:" << std::endl + << calib_data.left.D << std::endl; + std::cout << "* Intrinsic mat right:" << std::endl + << calib_data.right.K << std::endl; + std::cout << "* Distortion mat right:" << std::endl + << calib_data.right.D << std::endl; + std::cout << std::endl; + std::cout << "** Extrinsic parameters **" << std::endl; + std::cout << "* Translation:" << std::endl << calib_data.T << std::endl; + std::cout << "* Rotation:" << std::endl << calib_data.Rv << std::endl; + std::cout << std::endl; - cv::Size t_size(target_w, target_h); + std::cout << std::endl << "*** Save Calibration files ***" << std::endl; - for (int i = 0; i < left_images.size(); i++) { - std::vector pts_l_, pts_r_; - bool found_l = cv::findChessboardCorners(left_images.at(i), t_size, pts_l_, 3); - bool found_r = cv::findChessboardCorners(right_images.at(i), t_size, pts_r_, 3); + std::string opencv_file = calib_data.saveCalibOpenCV(serial); + if (!opencv_file.empty()) { + std::cout << " * OpenCV calibration file saved: " << opencv_file + << std::endl; + } else { + std::cout << " !!! Failed to save OpenCV calibration file " << opencv_file + << " !!!" << std::endl; + } - if (found_l && found_r) { - cv::cornerSubPix(left_images.at(i), pts_l_, cv::Size(5, 5), cv::Size(-1, -1), - cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 30, 0.001)); + // SDK format is only supported for dual-mono setups + if (is_dual_mono) { + std::string zed_file = calib_data.saveCalibZED(serial, is_4k); + if (!zed_file.empty()) { + std::cout << " * ZED SDK calibration file saved: " << zed_file + << std::endl; + } else { + std::cout << " !!! Failed to save ZED SDK calibration file " << zed_file + << " !!!" << std::endl; + } + } - cv::cornerSubPix(right_images.at(i), pts_r_, cv::Size(5, 5), cv::Size(-1, -1), - cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 30, 0.001)); + return EXIT_SUCCESS; +} - pts_l.push_back(pts_l_); - pts_r.push_back(pts_r_); - object_points.push_back(pattern_points); - } else { - std::cout << "No target detected on image " << i << std::endl; - } - } +std::string StereoCalib::saveCalibOpenCV(int serial) { + std::string calib_filename = + "zed_calibration_" + std::to_string(serial) + ".yml"; - /// Compute calibration - std::cout << std::endl << "*** Calibration Report ***" << std::endl; + cv::FileStorage fs(calib_filename, cv::FileStorage::WRITE); + if (fs.isOpened()) { + fs << "Size" << imageSize; + fs << "K_LEFT" << left.K << "K_RIGHT" << right.K; - if (pts_l.size() < MIN_IMAGE) { - std::cout << " !!! Not enough images with the target detected !!!" << std::endl; - std::cout << " Please perform a new data acquisition." << std::endl << std::endl; + if (left.disto_model_RadTan) { + fs << "D_LEFT" << left.D << "D_RIGHT" << right.D; } else { - std::cout << " * Enough points detected" << std::endl; - - auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; - auto rms_l = calib_data.left.calibrate(object_points, pts_l, imageSize, flags); - auto rms_r = calib_data.right.calibrate(object_points, pts_r, imageSize, flags); - std::cout << " * Reprojection error:\t Left "< 0.5f || rms_r > 0.5f || err > 0.5f) - std::cout<<"\n\t !! Warning !!\n The reprojection error looks too high, check that the lens are clean (sharp images) and that the pattern is printed/mounted on a strong and flat surface."<