From f69e462f200a103103d1f8c709e048c3436e0d8f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 11 Nov 2025 18:54:08 +0100 Subject: [PATCH 01/34] Add parameters for Virtual Stereo init --- .../include/opencv_calibration.hpp | 1 + stereo_calibration/src/main.cpp | 171 +++++++++++++----- stereo_calibration/src/opencv_calibration.cpp | 1 + 3 files changed, 131 insertions(+), 42 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 07cbd48..2ce9f89 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 66a07ee..f380e80 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -3,11 +3,18 @@ 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 +// Learn more: +// * 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.0f; // mm + +// Default parameters are good for this checkerboard: +// https://github.com/opencv/opencv/blob/4.x/doc/pattern.png// +// ********************************************************************************* std::string folder = "zed-images/"; @@ -36,7 +43,7 @@ float CheckCoverage(const std::vector>& pts, const cv:: bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat t); /// Calibration condition -const float min_coverage = 10; // in percentage +const float min_coverage = 65; // in percentage const float min_rotation = 60; // in degrees const float acceptable_rotation = 50; // in degrees const float min_distance = 200; // in mm @@ -53,7 +60,6 @@ const cv::Scalar warn_color = cv::Scalar(0, 128, 255); const bool image_stack_horizontal = true; // true for horizontal, false for vertical - void scaleKP(std::vector &pts, cv::Size in, cv::Size out){ float rx = out.width / static_cast(in.width); @@ -66,30 +72,70 @@ void scaleKP(std::vector &pts, cv::Size in, cv::Size out){ } 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 == "--help" || arg == "-h") { + std::cout << "Usage: " << argv[0] << " [options]" << 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 << "* ZED Stereo Camera using an SVO file:" << std::endl; + std::cout << " " << argv[0] << " --svo camera.svo" << std::endl; + std::cout << "* Virtual Stereo Camera using camera IDs:" << std::endl; + std::cout << " " << argv[0] << " --zedxone --left_id 0 --right_id 1" + << std::endl; + std::cout << "* 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); + } } + } }; @@ -107,19 +153,60 @@ int main(int argc, char *argv[]) { 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.camera_resolution = sl::RESOLUTION::HD1080; // 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; - 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); + // Configure the Virtual Stereo Camera if '--zedxone' argument is provided + if (args.is_zed_x_one_virtual_stereo) { + if(args.left_camera_sn != -1 && args.right_camera_sn != -1) { + std::cout << "Using serial numbers for left and right cameras: " + << args.left_camera_sn << ", " << args.right_camera_sn + << std::endl; + + int sn_stereo = sl::generateVirtualStereoSerialNumber( args.left_camera_sn, args.right_camera_sn); + std::cout << "Virtual SN: " << sn_stereo << std::endl; + init_params.input.setVirtualStereoFromSerialNumbers( args.left_camera_sn, args.right_camera_sn, 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 1; + + } + + std::cout << "Using camera IDs for left and right cameras: " + << args.left_camera_id << ", " << args.right_camera_id + << std::endl; + + auto cams = sl::CameraOne::getDeviceList(); + int sn_left = -1; + int sn_right = -1; + + 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 1; + } + + int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); + std::cout << "Virtual Stereo SN: " << sn_stereo << std::endl; + + init_params.input.setVirtualStereoFromCameraIDs(args.left_camera_id, + args.right_camera_id,sn_stereo); + } } auto status = zed_camera.open(init_params); @@ -311,8 +398,8 @@ int main(int argc, char *argv[]) { 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) { + std::cout << "Coverage : " << (1-cov_left)*100 << "%/" << (100-min_coverage) << "%" << std::endl; + if (cov_left < ((100-min_coverage)/100.0f)) { cv::Mat rvec(1, 3, CV_64FC1); cv::Mat tvec(1, 3, CV_64FC1); @@ -364,7 +451,7 @@ int main(int argc, char *argv[]) { 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) + if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; else std::cout << "CALIBRATION fail" << std::endl; diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index cce5765..8c1d6e2 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -67,6 +67,7 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, 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; } else { std::cout << " * Enough points detected" << std::endl; From 51404dc726f4b829b24906883cdc899ffc8f4503 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 11 Nov 2025 19:28:51 +0100 Subject: [PATCH 02/34] Improve result report --- .../include/opencv_calibration.hpp | 15 ++++---- stereo_calibration/src/main.cpp | 24 ++++++++----- stereo_calibration/src/opencv_calibration.cpp | 35 +++++++++++++------ 3 files changed, 50 insertions(+), 24 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 2ce9f89..44e937a 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -13,9 +13,9 @@ struct CameraCalib{ cv::Mat D; bool disto_model_RadTan = true; - void print(std::string name) { - std::cout << name << " K:\n" << K << std::endl; - std::cout << " D:\n" << D << std::endl; + 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 initDefault(bool radtan){ @@ -31,7 +31,7 @@ struct CameraCalib{ } } - void setFrom(sl::CameraParameters & cam) { + void setFrom(const sl::CameraParameters & cam) { K = cv::Mat::eye(3, 3, CV_64FC1); K.at(0, 0) = cam.fx; K.at(1, 1) = cam.fy; @@ -97,7 +97,7 @@ struct StereoCalib{ T = cv::Mat::zeros(3, 1, CV_64FC1); } - void setFrom(sl::CalibrationParameters & calib_params) { + void setFrom(const sl::CalibrationParameters & calib_params) { left.setFrom(calib_params.left_cam); right.setFrom(calib_params.right_cam); @@ -135,4 +135,7 @@ struct StereoCalib{ } }; -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(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, + float max_repr_error = 0.5f); \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index f380e80..79346fe 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -1,5 +1,6 @@ #include "opencv_calibration.hpp" #include +#include namespace fs = std::filesystem; @@ -46,8 +47,8 @@ bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat t); const float min_coverage = 65; // 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 +const float min_distance = 300; // in mm +const float acceptable_distance = 200; // in mm std::vector> pts_detected; @@ -248,7 +249,7 @@ int main(int argc, char *argv[]) { cv::Mat rgb_d, rgb2_d, rgb_d_fill, display, rendering_image; extrinsic_checker checker; - float cov_left = 100; + float cov_left = 1.0f; bool angle_clb = false; std::vector pts_obj_; for (int i = 0; i < target_h; i++) { @@ -260,6 +261,7 @@ int main(int argc, char *argv[]) { // Check if the temp image folder exists and clear it if (fs::exists(folder)) { std::uintmax_t n{fs::remove_all(folder)}; + std::cout << " * Removed " << n << " temporary files or directories from previous calibration." << std::endl; } // Create the temp image folder if(!fs::create_directories(folder)) @@ -335,7 +337,7 @@ int main(int argc, char *argv[]) { 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) + 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); @@ -353,8 +355,12 @@ int main(int argc, char *argv[]) { 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(coverage_mode) { + std::stringstream ss_cov; + ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << (1-cov_left)*100 << "%/" << min_coverage << "%"; + cv::putText(rendering_image, ss_cov.str(), cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); + cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+85), 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); @@ -394,11 +400,11 @@ int main(int argc, char *argv[]) { 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{ + } 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 << "%/" << (100-min_coverage) << "%" << std::endl; + std::cout << "Coverage : " << (1-cov_left)*100 << "%/" << min_coverage << "%" << std::endl; if (cov_left < ((100-min_coverage)/100.0f)) { cv::Mat rvec(1, 3, CV_64FC1); cv::Mat tvec(1, 3, CV_64FC1); @@ -450,6 +456,8 @@ int main(int argc, char *argv[]) { } sl::sleep_ms(10); } + + // Add "Calibration in progress" message int err = calibrate(folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 8c1d6e2..14cd341 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,6 +1,6 @@ #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){ +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, float max_repr_error){ std::vector left_images, right_images; @@ -26,7 +26,8 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, img_number++; } - std::cout << "\n\t"<< left_images.size() << " images opened" << std::endl; + std::cout << std::endl << "\t" << left_images.size() << " images opened" + << std::endl; // Define object points of the target std::vector pattern_points; @@ -74,14 +75,25 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, 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 "< max_repr_error ? " !!! TOO HIGH !!!" : "") + << std::endl; + std::cout << " * Right " << rms_r << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") + << std::endl; auto err = calib_data.calibrate(object_points, pts_l, pts_r, imageSize, cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY); - std::cout << " * Reprojection error:\t Stereo " << err << std::endl; + std::cout << " * Stereo " << err << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; - if(rms_l > 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."< 0.5f || rms_r > 0.5f || err > 0.5f) { + std::cout << std::endl << "\t !! Warning !!" << std::endl << " The reprojection error looks too high (>" + << max_repr_error << "), check that the lens are clean (sharp images)" + " and that the pattern is printed/mounted on a not flexible and flat surface." << std::endl; + + return EXIT_FAILURE; + } + std::cout << std::endl; + std::cout << " ** Camera parameters **" << std::endl; std::cout << " * Intrinsic mat left:\t" << calib_data.left.K << std::endl; std::cout << " * Distortion mat left:\t" << calib_data.left.D << std::endl; @@ -94,8 +106,9 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, std::cout << std::endl << "*** Calibration file ***" << std::endl; std::string calib_filename = "zed_calibration_" + std::to_string(serial) + ".yml"; - std::cout<<" * Saving calibration file: " << calib_filename << std::endl; - std::cout<<" * If you want to use this calibration with the ZED SDK, you can load it by using sl::InitParameters::optional_opencv_calibration_file" << std::endl; + std::cout << " * Saving calibration file: " << calib_filename << std::endl; + std::cout << " * If you want to use this calibration with the ZED SDK, you can load it by using sl::InitParameters::optional_opencv_calibration_file" << std::endl; + std::cout << std::endl; cv::FileStorage fs(calib_filename, cv::FileStorage::WRITE); if (fs.isOpened()) { @@ -110,8 +123,10 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, fs << "R" << calib_data.Rv << "T" << calib_data.T; fs.release(); - } else - std::cout << "Error: can not save the extrinsic parameters\n"; + } else { + std::cout << "Error: can not save the extrinsic parameters" << std::endl; + return EXIT_FAILURE; + } } return EXIT_SUCCESS; } \ No newline at end of file From 8847f0e9cdf1bd07aacfee84c5a9ccf4bb6937ad Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 11 Nov 2025 20:13:12 +0100 Subject: [PATCH 03/34] Improve calibration feedback --- .../include/opencv_calibration.hpp | 4 +- stereo_calibration/src/main.cpp | 155 ++++++++++-------- stereo_calibration/src/opencv_calibration.cpp | 24 ++- 3 files changed, 103 insertions(+), 80 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 44e937a..ac14a36 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -6,7 +6,7 @@ #include #include -constexpr int MIN_IMAGE = 15; +constexpr int MIN_IMAGE = 20; struct CameraCalib{ cv::Mat K; @@ -46,7 +46,7 @@ struct CameraCalib{ D.at(0) = cam.disto[0]; D.at(1) = cam.disto[1]; D.at(2) = cam.disto[4]; - D.at(3) = cam.disto[5]; + 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 diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 79346fe..7de6184 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -49,6 +49,7 @@ const float min_rotation = 60; // in degrees const float acceptable_rotation = 50; // in degrees const float min_distance = 300; // in mm const float acceptable_distance = 200; // in mm +const float max_repr_error = 0.5f; // in pixels std::vector> pts_detected; @@ -251,6 +252,7 @@ int main(int argc, char *argv[]) { extrinsic_checker checker; float cov_left = 1.0f; bool angle_clb = false; + bool acquisition_completed = false; std::vector pts_obj_; for (int i = 0; i < target_h; i++) { for (int j = 0; j < target_w; j++) { @@ -294,80 +296,95 @@ int main(int argc, char *argv[]) { 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); + 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); } - 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); + 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) { + acquisition_completed = true; + } else { + std::stringstream ss; + ss << "Not enough images for calibration. Missing " << (MIN_IMAGE - image_count) << " images."; + cv::putText(text_info, ss.str(), + cv::Point(10, display.size[0] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.8, + warn_color, 2); + } + } } - } - cv::vconcat(display, text_info, rendering_image); + 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) { - std::stringstream ss_cov; - ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << (1-cov_left)*100 << "%/" << min_coverage << "%"; - cv::putText(rendering_image, ss_cov.str(), cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); - cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+85), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << square_size; + + if (acquisition_completed) { + cv::putText(rendering_image, "Acquisition completed! Wait for the calibration computation...", + cv::Point(10, display.size[0]+50), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + } else { + 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) { + std::stringstream ss_cov; + ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << (1-cov_left)*100 << "%/" << min_coverage << "%"; + cv::putText(rendering_image, ss_cov.str(), cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); + cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+85), 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); } - 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 (acquisition_completed) { + break; + } + if ((key == 's' || key == 'S')) { if (!angle_clb) coverage_mode = true; std::vector pts_l, pts_r; @@ -453,16 +470,17 @@ int main(int argc, char *argv[]) { image_count++; } } - } + } sl::sleep_ms(10); } // Add "Calibration in progress" message - int err = calibrate(folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior); + int err = calibrate(folder, calib, target_w, target_h, square_size, + zed_info.serial_number, false, can_use_calib_prior, max_repr_error); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; else - std::cout << "CALIBRATION fail" << std::endl; + std::cout << "CALIBRATION failed" << std::endl; zed_camera.close(); @@ -551,9 +569,6 @@ bool writeRotText(cv::Mat& image, float rot_x, float rot_y, float rot_z, float d 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; } diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 14cd341..58af3f5 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -94,14 +94,22 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, std::cout << std::endl; - std::cout << " ** Camera parameters **" << std::endl; - std::cout << " * Intrinsic mat left:\t" << calib_data.left.K << std::endl; - std::cout << " * Distortion mat left:\t" << calib_data.left.D << std::endl; - std::cout << " * Intrinsic mat right:\t" << calib_data.right.K << std::endl; - std::cout << " * Distortion mat right:\t" << calib_data.right.D << std::endl; - std::cout << " ** Extrinsic parameters **" << std::endl; - std::cout << " * Translation:\t" << calib_data.T << std::endl; - std::cout << " * Rotation:\t" << calib_data.Rv << std::endl; + 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; std::cout << std::endl << "*** Calibration file ***" << std::endl; std::string calib_filename = "zed_calibration_" + std::to_string(serial) + ".yml"; From 09e149df568988e6b3a3d5f1f0d1c69ae8bac044 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 12 Nov 2025 12:21:05 +0100 Subject: [PATCH 04/34] Fix image reading error --- stereo_calibration/.vscode/settings.json | 3 + .../include/opencv_calibration.hpp | 4 +- stereo_calibration/src/main.cpp | 105 +++++++++++------- stereo_calibration/src/opencv_calibration.cpp | 13 +-- 4 files changed, 74 insertions(+), 51 deletions(-) create mode 100644 stereo_calibration/.vscode/settings.json diff --git a/stereo_calibration/.vscode/settings.json b/stereo_calibration/.vscode/settings.json new file mode 100644 index 0000000..70e34ec --- /dev/null +++ b/stereo_calibration/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "C_Cpp.errorSquiggles": "disabled" +} \ No newline at end of file diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index ac14a36..b3eee22 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -135,7 +135,7 @@ struct StereoCalib{ } }; -int calibrate(const std::string &folder, StereoCalib &raw_data, int target_w, - int target_h, float square_size, int serial, +int calibrate(int img_count, 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, float max_repr_error = 0.5f); \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 7de6184..a0f5b72 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -373,9 +373,10 @@ int main(int argc, char *argv[]) { cv::putText(rendering_image, ss_cov.str(), cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+85), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); } - if(!frames_rot_good) + 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); @@ -386,7 +387,10 @@ int main(int argc, char *argv[]) { } if ((key == 's' || key == 'S')) { - if (!angle_clb) coverage_mode = true; + 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); @@ -401,7 +405,6 @@ int main(int argc, char *argv[]) { 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) { @@ -423,27 +426,20 @@ int main(int argc, char *argv[]) { cov_left = CheckCoverage(pts_detected, cv::Size(camera_resolution.width, camera_resolution.height)); std::cout << "Coverage : " << (1-cov_left)*100 << "%/" << min_coverage << "%" << std::endl; if (cov_left < ((100-min_coverage)/100.0f)) { - 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; + checker.distance_tot = 0.0f; + checker.rot_x_delta = 0.0f; + checker.rot_y_delta = 0.0f; + checker.rot_z_delta = 0.0f; + + checker.rot_x_min = 180.0f; + checker.rot_y_min = 180.0f; + checker.rot_z_min = 180.0f; + checker.rot_x_max = -180.0f; + checker.rot_y_max = -180.0f; + checker.rot_z_max = -180.0f; + + checker.d_min = 10000.0f; + checker.d_max = 0.0f; angle_clb = true; } @@ -455,27 +451,32 @@ int main(int argc, char *argv[]) { 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) + 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 (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++; + } } } - } - sl::sleep_ms(10); + } + //sl::sleep_ms(10); } // Add "Calibration in progress" message - int err = calibrate(folder, calib, target_w, target_h, square_size, + int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior, max_repr_error); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; @@ -628,14 +629,34 @@ float CheckCoverage(const std::vector>& pts, const cv:: return error / tot; } -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); +bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec) { + + // Convert rotation vector to rotation matrix + cv::Mat R; + cv::Rodrigues(rvec, R); + + // Extract Euler angles (RPY) from rotation matrix + double sy = sqrt(R.at(0, 0) * R.at(0, 0) + + R.at(1, 0) * R.at(1, 0)); + bool singular = sy < 1e-6; + + double rx, ry, rz; // roll (z), pitch (x), yaw (y) + if (!singular) { + rx = atan2(R.at(2, 1), R.at(2, 2)); // Pitch + ry = atan2(-R.at(2, 0), sy); // Yaw + rz = atan2(R.at(1, 0), R.at(0, 0)); // Roll + } else { + rx = atan2(-R.at(1, 2), R.at(1, 1)); + ry = atan2(-R.at(2, 0), sy); + rz = 0; + } + + // Convert radians to degrees + rx = rx * 180.0 / M_PI; + ry = ry * 180.0 / M_PI; + rz = rz * 180.0 / M_PI; - std::cout << "* Rot X: " << rx << "°" << std::endl; - std::cout << "* Rot Y: " << ry << "°" << std::endl; - std::cout << "* Rot Z: " << rz << "°" << std::endl; + std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry << std::endl; if(fabs(rz)>45.0) { diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 58af3f5..1defc87 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,16 +1,16 @@ #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, float max_repr_error){ +int calibrate( int img_count, 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, float max_repr_error){ 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); + for(int i = 0; i < img_count; i++) { + 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(); @@ -22,8 +22,7 @@ int calibrate(const std::string& folder, StereoCalib &calib_data, int target_w, left_images.push_back(grey_l); right_images.push_back(grey_r); - }else break; - img_number++; + } } std::cout << std::endl << "\t" << left_images.size() << " images opened" From 529a117b3a03f2359e9ad99087804659d47b6d4e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 12 Nov 2025 15:54:12 +0100 Subject: [PATCH 05/34] Debug info --- stereo_calibration/src/main.cpp | 286 ++++++++++-------- stereo_calibration/src/opencv_calibration.cpp | 7 +- 2 files changed, 166 insertions(+), 127 deletions(-) diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index a0f5b72..b08d0d9 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -11,13 +11,13 @@ namespace fs = std::filesystem; 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.0f; // mm +constexpr float square_size = 25.4f; // mm // Default parameters are good for this checkerboard: -// https://github.com/opencv/opencv/blob/4.x/doc/pattern.png// +// https://github.com/opencv/opencv/blob/4.x/doc/pattern.png/ // ********************************************************************************* -std::string folder = "zed-images/"; +std::string image_folder = "zed-images/"; int verbose = 0; @@ -41,7 +41,10 @@ struct extrinsic_checker { 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); +bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat t, bool first_time); + +/// Rendering +constexpr int text_area_height = 200; /// Calibration condition const float min_coverage = 65; // in percentage @@ -252,6 +255,7 @@ int main(int argc, char *argv[]) { extrinsic_checker checker; float cov_left = 1.0f; bool angle_clb = false; + bool first_angle_check = true; bool acquisition_completed = false; std::vector pts_obj_; for (int i = 0; i < target_h; i++) { @@ -261,12 +265,12 @@ int main(int argc, char *argv[]) { } // Check if the temp image folder exists and clear it - if (fs::exists(folder)) { - std::uintmax_t n{fs::remove_all(folder)}; + if (fs::exists(image_folder)) { + std::uintmax_t n{fs::remove_all(image_folder)}; std::cout << " * Removed " << n << " temporary files or directories from previous calibration." << std::endl; } // Create the temp image folder - if(!fs::create_directories(folder)) + if(!fs::create_directories(image_folder)) { std::cerr << "Error creating storage folder!"; return 1; @@ -284,10 +288,16 @@ int main(int argc, char *argv[]) { 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) { + 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; + } if (zed_camera.grab() == sl::ERROR_CODE::SUCCESS) { zed_camera.retrieveImage(zed_imageL, sl::VIEW::LEFT_UNRECTIFIED); @@ -296,75 +306,72 @@ int main(int argc, char *argv[]) { 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); + 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); + 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); + 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()); + cv::Mat text_info = cv::Mat::ones(cv::Size(display.size[1], text_area_height), 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) { - acquisition_completed = true; - } else { - std::stringstream ss; - ss << "Not enough images for calibration. Missing " << (MIN_IMAGE - image_count) << " images."; - cv::putText(text_info, ss.str(), - cv::Point(10, display.size[0] + 140), - cv::FONT_HERSHEY_SIMPLEX, 0.8, - warn_color, 2); - } + 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) { + acquisition_completed = true; + } else { + std::stringstream ss; + ss << "Not enough images for calibration saved. Missing " << (MIN_IMAGE - image_count) << " images."; + cv::putText(text_info, ss.str(), + cv::Point(10, display.size[0] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.8, + warn_color, 2); } } + } - cv::vconcat(display, text_info, rendering_image); - - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << square_size; + cv::vconcat(display, text_info, rendering_image); if (acquisition_completed) { - cv::putText(rendering_image, "Acquisition completed! Wait for the calibration computation...", - cv::Point(10, display.size[0]+50), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + 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.8, info_color, 2); } else { 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); + cv::putText(rendering_image, "Missing target on image right", cv::Point(display.size[1]/2+10, display.size[0]+170), 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, "Missing target on image left", cv::Point(10, display.size[0]+170), 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) { @@ -426,20 +433,8 @@ int main(int argc, char *argv[]) { cov_left = CheckCoverage(pts_detected, cv::Size(camera_resolution.width, camera_resolution.height)); std::cout << "Coverage : " << (1-cov_left)*100 << "%/" << min_coverage << "%" << std::endl; if (cov_left < ((100-min_coverage)/100.0f)) { - checker.distance_tot = 0.0f; - checker.rot_x_delta = 0.0f; - checker.rot_y_delta = 0.0f; - checker.rot_z_delta = 0.0f; - - checker.rot_x_min = 180.0f; - checker.rot_y_min = 180.0f; - checker.rot_z_min = 180.0f; - checker.rot_x_max = -180.0f; - checker.rot_y_max = -180.0f; - checker.rot_z_max = -180.0f; - - checker.d_min = 10000.0f; - checker.d_max = 0.0f; + // Coverage Complete. Start angle calibration + std::cout << "Coverage complete. Start angle calibration." << std::endl; angle_clb = true; } @@ -450,9 +445,10 @@ int main(int argc, char *argv[]) { 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); + frames_rot_good = updateRT(checker, rvec, tvec, first_angle_check); if(frames_rot_good) { pts_detected.push_back(pts_l); + first_angle_check = false; } } } @@ -460,10 +456,10 @@ int main(int argc, char *argv[]) { if (frames_rot_good) { // saves the images - cv::imwrite(folder + "image_left_" + + cv::imwrite(image_folder + "image_left_" + std::to_string(image_count) + ".png", rgb_l); - cv::imwrite(folder + "image_right_" + + cv::imwrite(image_folder + "image_right_" + std::to_string(image_count) + ".png", rgb_r); std::cout << " * Images saved" << std::endl; @@ -476,7 +472,7 @@ int main(int argc, char *argv[]) { } // Add "Calibration in progress" message - int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, + int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior, max_repr_error); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; @@ -485,7 +481,7 @@ int main(int argc, char *argv[]) { zed_camera.close(); - return 0; + return EXIT_SUCCESS; } // Function to perform linear interpolation @@ -574,28 +570,28 @@ bool writeRotText(cv::Mat& image, float rot_x, float rot_y, float rot_z, float d } bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, std::vector> pts) { - int count = 0; + int count = 0; - 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++; + 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++; + } + } } - } } - } if (min) { - if (count > MinPts) - return true; - else - return false; + if (count > MinPts) + return true; + else + return false; } else { - if (count < MaxPts) - return true; - else - return false; + if (count < MaxPts) + return true; + else + return false; } } @@ -629,41 +625,79 @@ float CheckCoverage(const std::vector>& pts, const cv:: return error / tot; } -bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec) { +// Convert rotation vector (rvec) to Euler angles (roll, pitch, yaw) in radians +cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { + // sy is the magnitude in the X-Y plane for pitch calculation + double sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + + R.at(1, 0) * R.at(1, 0)); + bool singular = sy < 1e-6; + + double roll, pitch, yaw; + if (!singular) { + // Following OpenCV camera frame axis convention: + // roll around Z axis + roll = std::atan2(R.at(1, 0), + R.at(0, 0)); // rotation about Z (roll) + pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) + yaw = std::atan2(R.at(2, 1), + R.at(2, 2)); // rotation about Y (yaw) + } else { + // Gimbal lock case + roll = std::atan2(-R.at(1, 2), R.at(1, 1)); + pitch = std::atan2(-R.at(2, 0), sy); + yaw = 0; + } + return cv::Vec3d(roll, pitch, yaw); +} + +bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, bool first_time) { + + std::cout << "************************************************" << std::endl; + std::cout << " * Current rvec: [" << rvec.at(0) << ", " << rvec.at(1) << ", " << rvec.at(2) << "]" << std::endl; + std::cout << " * Current tvec: [" << tvec.at(0) << ", " << tvec.at(1) << ", " << tvec.at(2) << "]" << std::endl; // Convert rotation vector to rotation matrix cv::Mat R; cv::Rodrigues(rvec, R); - // Extract Euler angles (RPY) from rotation matrix - double sy = sqrt(R.at(0, 0) * R.at(0, 0) + - R.at(1, 0) * R.at(1, 0)); - bool singular = sy < 1e-6; - - double rx, ry, rz; // roll (z), pitch (x), yaw (y) - if (!singular) { - rx = atan2(R.at(2, 1), R.at(2, 2)); // Pitch - ry = atan2(-R.at(2, 0), sy); // Yaw - rz = atan2(R.at(1, 0), R.at(0, 0)); // Roll - } else { - rx = atan2(-R.at(1, 2), R.at(1, 1)); - ry = atan2(-R.at(2, 0), sy); - rz = 0; - } + cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); // Convert radians to degrees - rx = rx * 180.0 / M_PI; - ry = ry * 180.0 / M_PI; - rz = rz * 180.0 / M_PI; + double rz = eulerAngles[0] * 180.0 / M_PI; + double rx = eulerAngles[1] * 180.0 / M_PI; + double ry = eulerAngles[2] * 180.0 / M_PI; std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry << std::endl; + double distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); + std::cout << "Distance: " << distance << " mm" << std::endl; + if(fabs(rz)>45.0) { std::cerr << " * Images ignored: Rot Z > 45° ["<< rz <<"°]" << std::endl; return false; } + if(first_time) { + checker_.rot_x_min = rx; + checker_.rot_y_min = ry; + checker_.rot_z_min = rz; + + checker_.rot_x_max = rx; + checker_.rot_y_max = ry; + checker_.rot_z_max = rz; + + checker_.d_min = distance; + checker_.d_max = distance; + + checker_.rot_x_delta = 0; + checker_.rot_y_delta = 0; + checker_.rot_z_delta = 0; + checker_.distance_tot = 0; + + return true; + } + // check min if (checker_.rot_x_min > rx) checker_.rot_x_min = rx; @@ -680,10 +714,12 @@ bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec) { 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)); + if (checker_.d_min > distance) { + checker_.d_min = distance; + } + if (checker_.d_max < distance) { + checker_.d_max = distance; + } // compute deltas checker_.rot_x_delta = checker_.rot_x_max - checker_.rot_x_min; diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 1defc87..6619a9d 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -16,8 +16,11 @@ int calibrate( int img_count, const std::string& folder, StereoCalib &calib_data 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; + std::cout << "Image number " << i + << " 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); From 91eaad33030d227cc0e9b0002556da8f032325cf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 12 Nov 2025 20:19:32 +0100 Subject: [PATCH 06/34] Improve target detection --- .../include/opencv_calibration.hpp | 158 +++--- stereo_calibration/src/main.cpp | 472 ++++++++++-------- stereo_calibration/src/opencv_calibration.cpp | 292 ++++++----- 3 files changed, 494 insertions(+), 428 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index b3eee22..0b3eb75 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -20,65 +20,73 @@ struct CameraCalib{ void initDefault(bool radtan){ disto_model_RadTan = radtan; - K = cv::Mat::eye(3, 3, CV_64FC1); + 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_64FC1); + 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_64FC1); + D = cv::Mat::zeros(1, 4, CV_32FC1); } } void setFrom(const 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]; - } + 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; + + // 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]; + } } - 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; + 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; } - 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; + float calibrate(const std::vector> &object_points, + const std::vector> &image_points, + const cv::Size &image_size, int flags) { + float 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; } }; @@ -92,9 +100,9 @@ struct StereoCalib{ 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); + 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) { @@ -102,36 +110,38 @@ struct StereoCalib{ 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; - + 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; + 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; + float 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) { + float 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; } }; diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index b08d0d9..07ba857 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -11,7 +11,7 @@ namespace fs = std::filesystem; constexpr int target_w = 9; // number of horizontal inner edges constexpr int target_h = 6; // number of vertical inner edges -constexpr float square_size = 25.4f; // mm +constexpr float square_size = 25.4; // mm // Default parameters are good for this checkerboard: // https://github.com/opencv/opencv/blob/4.x/doc/pattern.png/ @@ -22,37 +22,39 @@ std::string image_folder = "zed-images/"; int verbose = 0; 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; - - float rot_x_delta; - float rot_y_delta; - float rot_z_delta; - - float d_min; - float d_max; - float distance_tot; + float rot_x_min; + float rot_y_min; + float rot_z_min; + float rot_x_max; + float rot_y_max; + float rot_z_max; + + float rot_x_delta; + float rot_y_delta; + float rot_z_delta; + + float d_min; + float d_max; + float distance_tot; }; 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 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, bool first_time); /// Rendering constexpr int text_area_height = 200; /// Calibration condition -const float min_coverage = 65; // in percentage -const float min_rotation = 60; // in degrees -const float acceptable_rotation = 50; // in degrees -const float min_distance = 300; // in mm -const float acceptable_distance = 200; // in mm -const float max_repr_error = 0.5f; // in pixels +const float min_coverage = 90; // in percentage +const float min_rotation = 60; // in degrees +const float acceptable_rotation = 50; // in degrees +const float min_distance = 300; // in mm +const float acceptable_distance = 200; // in mm +const float max_repr_error = 0.5; // in pixels std::vector> pts_detected; @@ -65,15 +67,14 @@ const cv::Scalar warn_color = cv::Scalar(0, 128, 255); const bool image_stack_horizontal = true; // true for horizontal, false for vertical -void scaleKP(std::vector &pts, cv::Size in, cv::Size out){ +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 { @@ -158,10 +159,12 @@ int main(int argc, char *argv[]) { 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::HD1080; // Use the camera's native resolution + 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 = verbose; // Configure the Virtual Stereo Camera if '--zedxone' argument is provided if (args.is_zed_x_one_virtual_stereo) { @@ -250,17 +253,18 @@ int main(int argc, char *argv[]) { bool frames_rot_good=true; cv::Mat blank = cv::Mat::zeros(camera_resolution.height, camera_resolution.width, CV_8UC1); - cv::Mat rgb_d, rgb2_d, rgb_d_fill, display, rendering_image; + cv::Mat rgb_d, rgb2_d, rgb_d_fill, rgb2_d_fill, display, rendering_image; extrinsic_checker checker; - float cov_left = 1.0f; + float cov_left = 1.0; bool angle_clb = false; bool first_angle_check = true; bool acquisition_completed = 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)); + pts_obj_.push_back( + cv::Point3f(square_size * j, square_size * i, 0.0)); } } @@ -275,7 +279,7 @@ int main(int argc, char *argv[]) { std::cerr << "Error creating storage folder!"; return 1; } - + std::vector> pts_init_im_l, pts_init_im_r; std::vector> pts_init_obj; @@ -284,8 +288,7 @@ int main(int argc, char *argv[]) { 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; + bool missing_target_on_last_pics = false; const std::string window_name = "ZED Calibration"; cv::namedWindow(window_name, cv::WINDOW_KEEPRATIO); @@ -311,7 +314,7 @@ int main(int argc, char *argv[]) { std::vector channels; cv::split(rgb_l, channels); blank.setTo(0); - float x_end,y_end; + 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]) @@ -334,9 +337,19 @@ int main(int argc, char *argv[]) { cv::resize(rgb_l, rgb_d_fill, display_size); } - std::vector pts; + /*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); + drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), cv::Mat(pts), found);*/ + + std::vector pts_l, pts_r; + bool found_l = false; + bool found_r = false; + found_l = cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts_l); + drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), cv::Mat(pts_l), found_l); + if (found_l) { + found_r = cv::findChessboardCorners(rgb2_d, cv::Size(target_w, target_h), pts_r); + drawChessboardCorners(rgb2_d, cv::Size(target_w, target_h), cv::Mat(pts_r), found_r); + } if(image_stack_horizontal) cv::hconcat(rgb_d_fill, rgb2_d, display); @@ -349,16 +362,18 @@ int main(int argc, char *argv[]) { 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) { - acquisition_completed = true; - } else { - std::stringstream ss; - ss << "Not enough images for calibration saved. Missing " << (MIN_IMAGE - image_count) << " images."; - cv::putText(text_info, ss.str(), - cv::Point(10, display.size[0] + 140), - cv::FONT_HERSHEY_SIMPLEX, 0.8, - warn_color, 2); - } + if (image_count >= + MIN_IMAGE + + 10) { // Add 10 extra images for better results + acquisition_completed = true; + } else { + std::stringstream ss; + ss << "Not enough images for calibration saved. Missing " + << ((MIN_IMAGE + 10) - image_count) << " images."; + cv::putText(text_info, ss.str(), + cv::Point(10, display.size[0] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); + } } } @@ -368,12 +383,10 @@ int main(int argc, char *argv[]) { 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.8, info_color, 2); } else { - if (missing_right_target_on_last_pic) - cv::putText(rendering_image, "Missing target on image right", cv::Point(display.size[1]/2+10, display.size[0]+170), 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]+170), 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 (missing_target_on_last_pics) + cv::putText(rendering_image, "Missing target on one of the images.", cv::Point(30, display.size[0]+170), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); + + cv::putText(rendering_image, "Press 's' or the spacebar to save the current frames when the target is visible in both images.", cv::Point(10, display.size[0]+25), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); if(coverage_mode) { std::stringstream ss_cov; ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << (1-cov_left)*100 << "%/" << min_coverage << "%"; @@ -384,8 +397,12 @@ int main(int argc, char *argv[]) { 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); } + + std::stringstream ss_img_count; + ss_img_count << "Frame saved: " << image_count; + cv::putText(rendering_image, ss_img_count.str(), cv::Point((display.size[1]+display_size.width)/2, display.size[0]+170), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); } - + cv::imshow(window_name, rendering_image); key = cv::waitKey(10); @@ -393,27 +410,16 @@ int main(int argc, char *argv[]) { break; } - if ((key == 's' || key == 'S')) { + if ((key == 's' || key == 'S') || key == ' ') { 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; + 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)); - + if(need_intrinsic_estimation) { pts_init_im_l.push_back(pts_l); @@ -421,6 +427,7 @@ int main(int argc, char *argv[]) { 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)); @@ -432,18 +439,26 @@ int main(int argc, char *argv[]) { 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 << "%/" << min_coverage << "%" << std::endl; - if (cov_left < ((100-min_coverage)/100.0f)) { - // Coverage Complete. Start angle calibration - std::cout << "Coverage complete. Start angle calibration." << std::endl; + if (cov_left < ((100.0 - min_coverage) / 100.0)) { + // Coverage Complete. Start angle calibration + std::cout << "Coverage complete. Start angle " + "calibration." + << std::endl; - angle_clb = true; + angle_clb = true; } } else { - cv::Mat rvec(1, 3, CV_64FC1); - cv::Mat tvec(1, 3, CV_64FC1); + cv::Mat rvec(1, 3, CV_32FC1); + cv::Mat tvec(1, 3, CV_32FC1); + std::cout << "Before undistort" << pts_l + << std::endl; auto undist_pts = calib.left.undistortPoints(pts_l); + std::cout << "After undistort" << undist_pts + << std::endl; cv::Mat empty_dist; - bool found_ = cv::solvePnP(pts_obj_, undist_pts, calib.left.K, empty_dist, rvec, tvec, false, cv::SOLVEPNP_EPNP); + 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, first_angle_check); if(frames_rot_good) { @@ -485,166 +500,179 @@ int main(int argc, char *argv[]) { } // 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; +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; } -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); - - return status; +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); + + return status; } -bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, std::vector> pts) { - int count = 0; +bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, + std::vector> pts) { + int count = 0; - 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++; - } - } + 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++; } + } } + } - if (min) { - if (count > MinPts) - return true; - else - return false; - } else { - if (count < MaxPts) - return true; - else - return false; - } - + if (min) { + if (count > MinPts) + return true; + else + return false; + } else { + if (count < MaxPts) + return true; + else + return false; + } } -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; +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++; } - return error / tot; + min_h_ += bucketsize; + max_h_ += bucketsize; + } + return error / tot; } // Convert rotation vector (rvec) to Euler angles (roll, pitch, yaw) in radians cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { // sy is the magnitude in the X-Y plane for pitch calculation - double sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + - R.at(1, 0) * R.at(1, 0)); + float sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + + R.at(1, 0) * R.at(1, 0)); bool singular = sy < 1e-6; - double roll, pitch, yaw; + float roll, pitch, yaw; if (!singular) { // Following OpenCV camera frame axis convention: // roll around Z axis - roll = std::atan2(R.at(1, 0), - R.at(0, 0)); // rotation about Z (roll) - pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) - yaw = std::atan2(R.at(2, 1), - R.at(2, 2)); // rotation about Y (yaw) + roll = std::atan2(R.at(1, 0), + R.at(0, 0)); // rotation about Z (roll) + pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) + yaw = std::atan2(R.at(2, 1), + R.at(2, 2)); // rotation about Y (yaw) } else { // Gimbal lock case - roll = std::atan2(-R.at(1, 2), R.at(1, 1)); - pitch = std::atan2(-R.at(2, 0), sy); + roll = std::atan2(-R.at(1, 2), R.at(1, 1)); + pitch = std::atan2(-R.at(2, 0), sy); yaw = 0; } return cv::Vec3d(roll, pitch, yaw); @@ -653,8 +681,8 @@ cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, bool first_time) { std::cout << "************************************************" << std::endl; - std::cout << " * Current rvec: [" << rvec.at(0) << ", " << rvec.at(1) << ", " << rvec.at(2) << "]" << std::endl; - std::cout << " * Current tvec: [" << tvec.at(0) << ", " << tvec.at(1) << ", " << tvec.at(2) << "]" << std::endl; + std::cout << " * Current rvec: [" << rvec.at(0) << ", " << rvec.at(1) << ", " << rvec.at(2) << "]" << std::endl; + std::cout << " * Current tvec: [" << tvec.at(0) << ", " << tvec.at(1) << ", " << tvec.at(2) << "]" << std::endl; // Convert rotation vector to rotation matrix cv::Mat R; @@ -663,13 +691,13 @@ bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, bool firs cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); // Convert radians to degrees - double rz = eulerAngles[0] * 180.0 / M_PI; - double rx = eulerAngles[1] * 180.0 / M_PI; - double ry = eulerAngles[2] * 180.0 / M_PI; + float rz = eulerAngles[0] * 180.0 / M_PI; + float rx = eulerAngles[1] * 180.0 / M_PI; + float ry = eulerAngles[2] * 180.0 / M_PI; std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry << std::endl; - double distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); + float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); std::cout << "Distance: " << distance << " mm" << std::endl; if(fabs(rz)>45.0) diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 6619a9d..65d180f 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,142 +1,170 @@ #include "opencv_calibration.hpp" -int calibrate( int img_count, 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, float max_repr_error){ - - std::vector left_images, right_images; - - /// Read images - cv::Size imageSize = cv::Size(0, 0); - - for(int i = 0; i < img_count; i++) { - 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::cout << "Image number " << i - << " 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); - } - } - - std::cout << std::endl << "\t" << left_images.size() << " images opened" - << std::endl; +int calibrate(int img_count, 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, + float max_repr_error) { + std::vector left_images, right_images; + + /// Read images + cv::Size imageSize = cv::Size(0, 0); + + for (int i = 0; i < img_count; i++) { + 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::cout << "Image number " << i + << " does not have the same size as the previous ones" + << imageSize << " vs " << left_images.back().size() + << std::endl; + break; + } - // 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)); - } + left_images.push_back(grey_l); + right_images.push_back(grey_r); } + } - std::vector> object_points; - std::vector> pts_l, pts_r; - - cv::Size t_size(target_w, target_h); - - 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); - - 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 << "No target detected on image " << i << std::endl; - } - } + std::cout << std::endl + << "\t" << left_images.size() << " images opened" << std::endl; - /// Compute calibration - std::cout << std::endl << "*** Calibration Report ***" << std::endl; - - 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; + // 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::vector> object_points; + std::vector> pts_l, pts_r; + + cv::Size t_size(target_w, target_h); + + 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); + + 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 << " * 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: " << std::endl; - std::cout << " * Left " << rms_l << (rms_l > max_repr_error ? " !!! TOO HIGH !!!" : "") - << std::endl; - std::cout << " * Right " << rms_r << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") - << std::endl; + std::cout << "No target detected on image " << i << std::endl; + } + } + + /// Compute calibration + std::cout << std::endl << "*** Calibration Report ***" << std::endl; + + 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; + } 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: " << std::endl; + std::cout << " * Left " << rms_l + << (rms_l > max_repr_error ? " !!! TOO HIGH !!!" : "") + << std::endl; + std::cout << " * Right " << rms_r + << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") + << std::endl; + + auto err = calib_data.calibrate( + object_points, pts_l, pts_r, imageSize, + cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY); + std::cout << " * Stereo " << err + << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; + + if (rms_l > 0.5f || rms_r > 0.5f || err > 0.5f) { + std::cout << std::endl + << "\t !! Warning !!" << std::endl + << " The reprojection error looks too high (>" << max_repr_error + << "), check that the lens are clean (sharp images)" + " and that the pattern is printed/mounted on a not flexible " + "and flat surface." + << std::endl; + + return EXIT_FAILURE; + } - auto err = calib_data.calibrate(object_points, pts_l, pts_r, imageSize, cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY); - std::cout << " * Stereo " << err << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; - - if(rms_l > 0.5f || rms_r > 0.5f || err > 0.5f) { - std::cout << std::endl << "\t !! Warning !!" << std::endl << " The reprojection error looks too high (>" - << max_repr_error << "), check that the lens are clean (sharp images)" - " and that the pattern is printed/mounted on a not flexible and flat surface." << std::endl; - - return EXIT_FAILURE; - } - - std::cout << std::endl; - - 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; - - std::cout << std::endl << "*** Calibration file ***" << std::endl; - std::string calib_filename = "zed_calibration_" + std::to_string(serial) + ".yml"; - - std::cout << " * Saving calibration file: " << calib_filename << std::endl; - std::cout << " * If you want to use this calibration with the ZED SDK, you can load it by using sl::InitParameters::optional_opencv_calibration_file" << std::endl; - std::cout << std::endl; - - cv::FileStorage fs(calib_filename, cv::FileStorage::WRITE); - if (fs.isOpened()) { - fs << "Size" << imageSize; - fs << "K_LEFT" << calib_data.left.K<< "K_RIGHT" << calib_data.right.K; - - if(calib_data.left.disto_model_RadTan) { - fs << "D_LEFT" << calib_data.left.D << "D_RIGHT" << calib_data.right.D; - } else { - fs << "D_LEFT_FE" << calib_data.left.D << "D_RIGHT_FE" << calib_data.right.D; - } - - fs << "R" << calib_data.Rv << "T" << calib_data.T; - fs.release(); - } else { - std::cout << "Error: can not save the extrinsic parameters" << std::endl; - return EXIT_FAILURE; - } + std::cout << std::endl; + + 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; + + std::cout << std::endl << "*** Calibration file ***" << std::endl; + std::string calib_filename = + "zed_calibration_" + std::to_string(serial) + ".yml"; + + std::cout << " * Saving calibration file: " << calib_filename << std::endl; + std::cout << " * If you want to use this calibration with the ZED SDK, you " + "can load it by using " + "sl::InitParameters::optional_opencv_calibration_file" + << std::endl; + std::cout << std::endl; + + cv::FileStorage fs(calib_filename, cv::FileStorage::WRITE); + if (fs.isOpened()) { + fs << "Size" << imageSize; + fs << "K_LEFT" << calib_data.left.K << "K_RIGHT" << calib_data.right.K; + + if (calib_data.left.disto_model_RadTan) { + fs << "D_LEFT" << calib_data.left.D << "D_RIGHT" << calib_data.right.D; + } else { + fs << "D_LEFT_FE" << calib_data.left.D << "D_RIGHT_FE" + << calib_data.right.D; + } + + fs << "R" << calib_data.Rv << "T" << calib_data.T; + fs.release(); + } else { + std::cout << "Error: can not save the extrinsic parameters" << std::endl; + return EXIT_FAILURE; } - return EXIT_SUCCESS; + } + return EXIT_SUCCESS; } \ No newline at end of file From cb92b29f9b58879549a0848f4c8044baebbe2adb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 13 Nov 2025 18:50:28 +0100 Subject: [PATCH 07/34] Add coverage Checker code --- .../include/calibration_checker.hpp | 64 ++++++ .../src/calibration_checker.cpp | 215 ++++++++++++++++++ stereo_calibration/src/main.cpp | 106 +++++---- 3 files changed, 337 insertions(+), 48 deletions(-) create mode 100644 stereo_calibration/include/calibration_checker.hpp create mode 100644 stereo_calibration/src/calibration_checker.cpp diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp new file mode 100644 index 0000000..86a561c --- /dev/null +++ b/stereo_calibration/include/calibration_checker.hpp @@ -0,0 +1,64 @@ +#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 _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 +} BoardParams; + +class CalibrationChecker { +public: + CalibrationChecker( cv::Size board_size, float square_size ); + ~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 + void calculateSampleCollectionStatus( float& size_score, float& skew_score, + float& pos_score_x, float& pos_score_y ) const; + +private: + // Calculate the parameter of a detected checkerboard + BoardParams getDetectedBoarParams(const std::vector& corners, cv::Size image_size); + + // Check if the detected corners are valid + bool isGoodSample(const BoardParams& params, + const std::vector& corners, + const std::vector& prev_corners); + + // 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_ + + const BoardParams idealParams_ = {cv::Point2f(0.7f, 0.7f), 0.4f, 0.5f}; // Ideal parameters for a good sample database +}; + +#endif // CALIBRATION_CHECKER_HPP diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp new file mode 100644 index 0000000..1931e55 --- /dev/null +++ b/stereo_calibration/src/calibration_checker.cpp @@ -0,0 +1,215 @@ +#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 ) +{ + // 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) { + BoardParams params = getDetectedBoarParams(corners, image_size); + + if(params.size < 0 || params.skew < 0) { + return false; // Invalid parameters + } + + bool is_good = isGoodSample(params, corners, validCorners_.empty() ? std::vector() : validCorners_.back()); + + if(is_good) { + // Store the valid parameters and associated corners + paramDb_.push_back(params); + validCorners_.push_back(corners); + } + + std::cout << std::setprecision(3) << "Sample: Pos(" << params.pos.x << ", " << params.pos.y << "), Size: " << params.size << ", Skew: " << params.skew << " => " << (is_good ? "Accepted" : "Rejected") << std::endl; + + return is_good; +} + +float CalibrationChecker::compute_skew(const std::vector& outside_corners) { + 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); + return std::acos(dot / (norm_ab * norm_cb)); + }; + + // Calculate skew as deviation from 90 degrees + 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]))); + + return skew; +} + +float CalibrationChecker::compute_area(const std::vector& outside_corners) { + 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; +} + +BoardParams CalibrationChecker::getDetectedBoarParams(const std::vector& corners, cv::Size image_size) { + BoardParams params; + + auto outside_corners = get_outside_corners(corners); + float area = compute_area(outside_corners); + float skew = compute_skew(outside_corners); + + if(area < 0 || 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))); + + 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 BoardParams& params, const std::vector& corners, + const std::vector& prev_corners) { + + if(paramDb_.empty()) { + return true; // First sample is always good + } + + auto param_distance = [](const BoardParams& p1, + const BoardParams& 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); + }; + + 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 existing samples (dist=" + << dist << ")" << std::endl; + return false; + } + } + + return true; +} + +void CalibrationChecker::calculateSampleCollectionStatus(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; + } + + 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_size = 0.0f; + min_skew = 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 Score : " << std::setprecision(3) << pos_score_x + << std::endl; + std::cout << " * PosY Score : " << std::setprecision(3) << pos_score_y + << std::endl; + std::cout << " * Size Score : " << std::setprecision(3) << size_score + << std::endl; + std::cout << " * Skew Score : " << std::setprecision(3) << skew_score + << std::endl; +} \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 07ba857..b2eb49f 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -247,8 +247,8 @@ int main(int argc, char *argv[]) { 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()); - // Number of area to fill 4 horizontally - bucketsize = camera_resolution.width/4; + // Number of area to fill horizontally + bucketsize = camera_resolution.width/6; bool frames_rot_good=true; cv::Mat blank = cv::Mat::zeros(camera_resolution.height, camera_resolution.width, CV_8UC1); @@ -267,6 +267,7 @@ int main(int argc, char *argv[]) { cv::Point3f(square_size * j, square_size * i, 0.0)); } } + // Check if the temp image folder exists and clear it if (fs::exists(image_folder)) { @@ -280,9 +281,11 @@ int main(int argc, char *argv[]) { return 1; } + // Data for initial intrinsic estimation std::vector> pts_init_im_l, pts_init_im_r; std::vector> pts_init_obj; + // Size of the rendered images const cv::Size display_size(720, 404); char key = ' '; @@ -395,7 +398,7 @@ int main(int argc, char *argv[]) { } 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::Point(600, display.size[0]+50), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); } std::stringstream ss_img_count; @@ -419,54 +422,61 @@ int main(int argc, char *argv[]) { 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; + 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_); + + if (need_intrinsic_estimation) { + // wait 5 images before running the estimate + if (pts_init_im_l.size() >= 5) { + 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)); + std::cout << "Intrinsic estimation done:" << std::endl; + std::cout << "Left K: " << std::endl << calib.left.K << std::endl; + std::cout << "Right K: " << std::endl << calib.right.K << std::endl; + 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 + << "%/" << min_coverage << "%" << std::endl; + if (cov_left < ((100.0 - min_coverage) / 100.0)) { + // Coverage Complete. Start angle calibration + std::cout << "Coverage complete. Start angle " + "calibration." + << std::endl; + + // Estimate intrinsic with a better image coverage + need_intrinsic_estimation = true; + + angle_clb = true; } - } else { - if (!angle_clb) { + } else { + cv::Mat rvec(1, 3, CV_32FC1); + cv::Mat tvec(1, 3, CV_32FC1); + std::cout << "Before undistort" << pts_l << std::endl; + auto undist_pts = calib.left.undistortPoints(pts_l); + std::cout << "After undistort" << undist_pts + << std::endl; + 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, first_angle_check); + if (frames_rot_good) { 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 << "%/" << min_coverage << "%" << std::endl; - if (cov_left < ((100.0 - min_coverage) / 100.0)) { - // Coverage Complete. Start angle calibration - std::cout << "Coverage complete. Start angle " - "calibration." - << std::endl; - - angle_clb = true; - } - } else { - cv::Mat rvec(1, 3, CV_32FC1); - cv::Mat tvec(1, 3, CV_32FC1); - std::cout << "Before undistort" << pts_l - << std::endl; - auto undist_pts = calib.left.undistortPoints(pts_l); - std::cout << "After undistort" << undist_pts - << std::endl; - 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, first_angle_check); - if(frames_rot_good) { - pts_detected.push_back(pts_l); - first_angle_check = false; - } - } + first_angle_check = false; + } } + } } if (frames_rot_good) { From 7a40e46a512725133970d753565fafb463893d1e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 14 Nov 2025 19:23:43 +0100 Subject: [PATCH 08/34] Test calibration checker --- .../include/calibration_checker.hpp | 4 +- .../src/calibration_checker.cpp | 19 +- stereo_calibration/src/main.cpp | 912 ++++++++++-------- 3 files changed, 512 insertions(+), 423 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 86a561c..1d4dad7 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -58,7 +58,9 @@ class CalibrationChecker { std::vector paramDb_; // Database of previously detected board parameters std::vector> validCorners_; // All the corners associated to the single parameters in paramDb_ - const BoardParams idealParams_ = {cv::Point2f(0.7f, 0.7f), 0.4f, 0.5f}; // Ideal parameters for a good sample database + const BoardParams idealParams_ = {cv::Point2f(0.65f, 0.65f), 0.4f, 0.f}; // Ideal parameters for a good sample database + const size_t min_samples_ = 20; // Minimum number of samples to consider the database complete + const size_t max_samples_ = 50; // Maximum number of samples to consider the database complete }; #endif // CALIBRATION_CHECKER_HPP diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 1931e55..5ea7376 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -195,8 +195,8 @@ void CalibrationChecker::calculateSampleCollectionStatus(float& size_score, floa } // Don't reward small size or skew - min_size = 0.0f; - min_skew = 0.0f; + //min_size = 0.0f; + //min_skew = 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); @@ -204,12 +204,13 @@ void CalibrationChecker::calculateSampleCollectionStatus(float& size_score, floa skew_score = std::min((max_skew - min_skew)/idealParams_.skew,1.0f); std::cout << "Sample Collection Status:" << std::endl; - std::cout << " * PosX Score : " << std::setprecision(3) << pos_score_x - << std::endl; - std::cout << " * PosY Score : " << std::setprecision(3) << pos_score_y - << std::endl; - std::cout << " * Size Score : " << std::setprecision(3) << size_score - << std::endl; - std::cout << " * Skew Score : " << std::setprecision(3) << skew_score + 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; } \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index b2eb49f..15bc54b 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -1,7 +1,9 @@ -#include "opencv_calibration.hpp" #include #include +#include "calibration_checker.hpp" +#include "opencv_calibration.hpp" + namespace fs = std::filesystem; // ********************************************************************************* @@ -9,9 +11,9 @@ namespace fs = std::filesystem; // Learn more: // * 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 = 25.4; // mm +constexpr int target_w = 9; // number of horizontal inner edges +constexpr int target_h = 6; // number of vertical inner edges +constexpr float square_size = 25.4; // mm // Default parameters are good for this checkerboard: // https://github.com/opencv/opencv/blob/4.x/doc/pattern.png/ @@ -42,8 +44,9 @@ 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, bool first_time); + const cv::Size& imgSize); +bool updateRT(extrinsic_checker& checker_, cv::Mat r, cv::Mat t, + bool first_time); /// Rendering constexpr int text_area_height = 200; @@ -62,10 +65,11 @@ 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 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 +const bool image_stack_horizontal = + true; // true for horizontal, false for vertical void scaleKP(std::vector& pts, cv::Size in, cv::Size out) { float rx = out.width / static_cast(in.width); @@ -113,17 +117,23 @@ struct Args { 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; + "stereo pair" + << std::endl; std::cout << " --left_id Id of the left camera if using " - "virtual stereo" << std::endl; + "virtual stereo" + << std::endl; std::cout << " --right_id Id of the right camera if using " - "virtual stereo" << std::endl; + "virtual stereo" + << std::endl; std::cout << " --left_sn S/N of the left camera if using " - "virtual stereo" << std::endl; + "virtual stereo" + << std::endl; std::cout << " --right_sn S/N of the right camera if using " - "virtual stereo" << std::endl; + "virtual stereo" + << std::endl; std::cout << " --zed_sdk_format Save calibration file in " - "ZED SDK format" << std::endl; + "ZED SDK format" + << std::endl; std::cout << " --help, -h Show this help message" << std::endl << std::endl; std::cout << "Examples:" << std::endl; @@ -133,9 +143,11 @@ struct Args { std::cout << " " << argv[0] << " --zedxone --left_id 0 --right_id 1" << std::endl; std::cout << "* Virtual Stereo Camera with fisheye lenses using camera " - "serial numbers:" << std::endl; - std::cout << " " << argv[0] - << " --fisheye --zedxone --left_sn 301528071 --right_sn 300473441" + "serial numbers:" + << std::endl; + std::cout + << " " << argv[0] + << " --fisheye --zedxone --left_sn 301528071 --right_sn 300473441" << std::endl; std::cout << std::endl; exit(0); @@ -144,374 +156,449 @@ struct Args { } }; +int main(int argc, char* argv[]) { + // Setup the calibration checker + CalibrationChecker checker(cv::Size(target_w, target_h), square_size); -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 = 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 = verbose; - - // Configure the Virtual Stereo Camera if '--zedxone' argument is provided - if (args.is_zed_x_one_virtual_stereo) { - if(args.left_camera_sn != -1 && args.right_camera_sn != -1) { - std::cout << "Using serial numbers for left and right cameras: " - << args.left_camera_sn << ", " << args.right_camera_sn - << std::endl; - - int sn_stereo = sl::generateVirtualStereoSerialNumber( args.left_camera_sn, args.right_camera_sn); - std::cout << "Virtual SN: " << sn_stereo << std::endl; - init_params.input.setVirtualStereoFromSerialNumbers( args.left_camera_sn, args.right_camera_sn, 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 1; + Args args; + args.parse(argc, argv); - } - - std::cout << "Using camera IDs for left and right cameras: " - << args.left_camera_id << ", " << args.right_camera_id + 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 = 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 = verbose; + + // Configure the Virtual Stereo Camera if '--zedxone' argument is provided + if (args.is_zed_x_one_virtual_stereo) { + if (args.left_camera_sn != -1 && args.right_camera_sn != -1) { + std::cout << "Using serial numbers for left and right cameras: " + << args.left_camera_sn << ", " << args.right_camera_sn + << std::endl; + + int sn_stereo = sl::generateVirtualStereoSerialNumber( + args.left_camera_sn, args.right_camera_sn); + std::cout << "Virtual SN: " << sn_stereo << std::endl; + init_params.input.setVirtualStereoFromSerialNumbers( + args.left_camera_sn, args.right_camera_sn, 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 1; + } - auto cams = sl::CameraOne::getDeviceList(); - int sn_left = -1; - int sn_right = -1; + std::cout << "Using camera IDs for left and right cameras: " + << args.left_camera_id << ", " << args.right_camera_id + << std::endl; - 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; - } - } + auto cams = sl::CameraOne::getDeviceList(); + int sn_left = -1; + int sn_right = -1; - 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 1; + 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 1; + } - int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); - std::cout << "Virtual Stereo SN: " << sn_stereo << std::endl; + int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); + std::cout << "Virtual Stereo SN: " << sn_stereo << std::endl; - init_params.input.setVirtualStereoFromCameraIDs(args.left_camera_id, - args.right_camera_id,sn_stereo); - } + init_params.input.setVirtualStereoFromCameraIDs( + args.left_camera_id, args.right_camera_id, sn_stereo); } + } - auto status = zed_camera.open(init_params); + auto status = zed_camera.open(init_params); - // 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; - } - - // 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; + // 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; + } - std::cout << "Using prior calibration: " << (can_use_calib_prior ? "Yes" : "No") << std::endl; + // 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; - StereoCalib calib; - calib.initDefault(args.is_radtan_lens); + std::cout << "Using prior calibration: " + << (can_use_calib_prior ? "Yes" : "No") << std::endl; - auto zed_info = zed_camera.getCameraInformation(); + StereoCalib calib; + calib.initDefault(args.is_radtan_lens); - if(can_use_calib_prior) - calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); + auto zed_info = zed_camera.getCameraInformation(); - sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; + if (can_use_calib_prior) + calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); - 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()); + sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; - 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()); + 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()); - // Number of area to fill horizontally - bucketsize = camera_resolution.width/6; + 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()); - bool frames_rot_good=true; - cv::Mat blank = cv::Mat::zeros(camera_resolution.height, camera_resolution.width, CV_8UC1); + // Number of area to fill horizontally + bucketsize = camera_resolution.width / 6; - cv::Mat rgb_d, rgb2_d, rgb_d_fill, rgb2_d_fill, display, rendering_image; + // bool frames_rot_good=true; + cv::Mat blank = cv::Mat::zeros(camera_resolution.height, + camera_resolution.width, CV_8UC1); - extrinsic_checker checker; - float cov_left = 1.0; - bool angle_clb = false; - bool first_angle_check = true; - bool acquisition_completed = 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.0)); - } - } - + cv::Mat rgb_d, rgb2_d, rgb_d_fill, rgb2_d_fill, display, rendering_image; - // Check if the temp image folder exists and clear it - if (fs::exists(image_folder)) { - std::uintmax_t n{fs::remove_all(image_folder)}; - std::cout << " * Removed " << n << " temporary files or directories from previous calibration." << std::endl; + // extrinsic_checker checker; + // float cov_left = 1.0; + // bool angle_clb = false; + // bool first_angle_check = true; + bool acquisition_completed = 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.0)); } - // Create the temp image folder - if(!fs::create_directories(image_folder)) - { - std::cerr << "Error creating storage folder!"; - return 1; + } + + // Check if the temp image folder exists and clear it + if (fs::exists(image_folder)) { + std::uintmax_t n{fs::remove_all(image_folder)}; + std::cout << " * Removed " << n + << " temporary files or directories from previous calibration." + << std::endl; + } + // Create the temp image folder + if (!fs::create_directories(image_folder)) { + std::cerr << "Error creating storage folder!"; + return 1; + } + + // Data for initial intrinsic estimation + std::vector> pts_init_im_l, pts_init_im_r; + std::vector> pts_init_obj; + + // Size of the rendered images + const cv::Size display_size(720, 404); + + char key = ' '; + int image_count = 0; + // bool coverage_mode = false; + bool missing_target_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; } - // Data for initial intrinsic estimation - std::vector> pts_init_im_l, pts_init_im_r; - std::vector> pts_init_obj; + 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_l, pts_r; + bool found_l = false; + bool found_r = false; + found_l = + cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts_l); + drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), + cv::Mat(pts_l), found_l); + if (found_l) { + found_r = cv::findChessboardCorners( + rgb2_d, cv::Size(target_w, target_h), pts_r); + drawChessboardCorners(rgb2_d, cv::Size(target_w, target_h), + cv::Mat(pts_r), found_r); + } - // Size of the rendered images - const cv::Size display_size(720, 404); + if (image_stack_horizontal) { + cv::hconcat(rgb_d_fill, rgb2_d, display); + } else { + cv::vconcat(rgb_d_fill, rgb2_d, display); + } - char key = ' '; - int image_count = 0; - bool coverage_mode = false; - bool missing_target_on_last_pics = false; + cv::Mat text_info = cv::Mat::ones( + cv::Size(display.size[1], text_area_height), 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 + + // 10) { // Add 10 extra images for better results + // acquisition_completed = true; + // } else { + // std::stringstream ss; + // ss << "Not enough images for calibration saved. Missing " + // << ((MIN_IMAGE + 10) - image_count) << " images."; + // cv::putText(text_info, ss.str(), + // cv::Point(10, display.size[0] + 140), + // cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); + // } + // } + // } + + 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.8, info_color, 2); + } else { + if (missing_target_on_last_pics) + cv::putText(rendering_image, "Missing target on one of the images.", + cv::Point(30, display.size[0] + 170), + cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); + + cv::putText(rendering_image, + "Press 's' or the spacebar to save the current frames when " + "the target is visible in both images.", + cv::Point(10, display.size[0] + 25), + cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + // if(coverage_mode) { + // std::stringstream ss_cov; + // ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << + // (1-cov_left)*100 << "%/" << min_coverage << "%"; + // cv::putText(rendering_image, ss_cov.str(), cv::Point(10, + // display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, + // 1); cv::putText(rendering_image, "Keep going until the green + // covers the image, it represents coverage", cv::Point(10, + // display.size[0]+85), 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]+50), cv::FONT_HERSHEY_SIMPLEX, + // 0.8, warn_color, 2); + // } + + std::stringstream ss_img_count; + ss_img_count << "Frame saved: " << image_count; + cv::putText(rendering_image, ss_img_count.str(), + cv::Point((display.size[1] + display_size.width) / 2, + display.size[0] + 170), + cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + } - 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); + cv::imshow(window_name, rendering_image); + key = cv::waitKey(10); - while (1) { - if(key == 'q' || key == 'Q' || key == 27) { - std::cout << "Calibration aborted by user." << std::endl; - zed_camera.close(); - return EXIT_SUCCESS; - } + if (acquisition_completed) { + break; + } - 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);*/ - - std::vector pts_l, pts_r; - bool found_l = false; - bool found_r = false; - found_l = cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts_l); - drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), cv::Mat(pts_l), found_l); - if (found_l) { - found_r = cv::findChessboardCorners(rgb2_d, cv::Size(target_w, target_h), pts_r); - drawChessboardCorners(rgb2_d, cv::Size(target_w, target_h), cv::Mat(pts_r), found_r); - } - - 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()); - - 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 + - 10) { // Add 10 extra images for better results - acquisition_completed = true; - } else { - std::stringstream ss; - ss << "Not enough images for calibration saved. Missing " - << ((MIN_IMAGE + 10) - image_count) << " images."; - cv::putText(text_info, ss.str(), - cv::Point(10, display.size[0] + 140), - cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - } - } - } - - 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.8, info_color, 2); - } else { - if (missing_target_on_last_pics) - cv::putText(rendering_image, "Missing target on one of the images.", cv::Point(30, display.size[0]+170), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - - cv::putText(rendering_image, "Press 's' or the spacebar to save the current frames when the target is visible in both images.", cv::Point(10, display.size[0]+25), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); - if(coverage_mode) { - std::stringstream ss_cov; - ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << (1-cov_left)*100 << "%/" << min_coverage << "%"; - cv::putText(rendering_image, ss_cov.str(), cv::Point(10, display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, 1); - cv::putText(rendering_image, "Keep going until the green covers the image, it represents coverage", cv::Point(10, display.size[0]+85), 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]+50), cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - } - - std::stringstream ss_img_count; - ss_img_count << "Frame saved: " << image_count; - cv::putText(rendering_image, ss_img_count.str(), cv::Point((display.size[1]+display_size.width)/2, display.size[0]+170), cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); - } - - cv::imshow(window_name, rendering_image); - key = cv::waitKey(10); - - if (acquisition_completed) { - break; - } - - if ((key == 's' || key == 'S') || key == ' ') { - if (!angle_clb) { - coverage_mode = true; - } - - 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)); - 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_); - - if (need_intrinsic_estimation) { - // wait 5 images before running the estimate - if (pts_init_im_l.size() >= 5) { - 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)); - std::cout << "Intrinsic estimation done:" << std::endl; - std::cout << "Left K: " << std::endl << calib.left.K << std::endl; - std::cout << "Right K: " << std::endl << calib.right.K << std::endl; - 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 - << "%/" << min_coverage << "%" << std::endl; - if (cov_left < ((100.0 - min_coverage) / 100.0)) { - // Coverage Complete. Start angle calibration - std::cout << "Coverage complete. Start angle " - "calibration." - << std::endl; - - // Estimate intrinsic with a better image coverage - need_intrinsic_estimation = true; - - angle_clb = true; - } - } else { - cv::Mat rvec(1, 3, CV_32FC1); - cv::Mat tvec(1, 3, CV_32FC1); - std::cout << "Before undistort" << pts_l << std::endl; - auto undist_pts = calib.left.undistortPoints(pts_l); - std::cout << "After undistort" << undist_pts - << std::endl; - 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, first_angle_check); - if (frames_rot_good) { - pts_detected.push_back(pts_l); - first_angle_check = false; - } - } - } - } - - if (frames_rot_good) { - // 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 ((key == 's' || key == 'S') || key == ' ') { + // if (!angle_clb) { + // coverage_mode = true; + // } + + 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)); + 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_); + + if (checker.testSample(pts_l, cv::Size(camera_resolution.width, + camera_resolution.height))) { + 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++; + + float size_score, skew_score, pos_score_x, pos_score_y; + checker.calculateSampleCollectionStatus(size_score, skew_score, + pos_score_x, pos_score_y); + + } else { + std::cout << " ! Sample detected but not valid. Please check the " + "checkerboard position and angle." + << std::endl; + } + + // if (need_intrinsic_estimation) { + // // wait 5 images before running the estimate + // if (pts_init_im_l.size() >= 5) { + // 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)); + // std::cout << "Intrinsic estimation done:" << std::endl; + // std::cout << "Left K: " << std::endl << calib.left.K << + // std::endl; std::cout << "Right K: " << std::endl << + // calib.right.K << std::endl; 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 + // << "%/" << min_coverage << "%" << std::endl; + // if (cov_left < ((100.0 - min_coverage) / 100.0)) { + // // Coverage Complete. Start angle calibration + // std::cout << "Coverage complete. Start angle " + // "calibration." + // << std::endl; + + // // Estimate intrinsic with a better image coverage + // need_intrinsic_estimation = true; + + // angle_clb = true; + // } + // } else { + // cv::Mat rvec(1, 3, CV_32FC1); + // cv::Mat tvec(1, 3, CV_32FC1); + // std::cout << "Before undistort" << pts_l << std::endl; + // auto undist_pts = calib.left.undistortPoints(pts_l); + // std::cout << "After undistort" << undist_pts + // << std::endl; + // 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, first_angle_check); + // if (frames_rot_good) { + // pts_detected.push_back(pts_l); + // first_angle_check = false; + // } + // } + // } + // } + + // if (frames_rot_good) { + // // 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++; + // } } - //sl::sleep_ms(10); + } } + // sl::sleep_ms(10); + } - // Add "Calibration in progress" message - int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, - zed_info.serial_number, false, can_use_calib_prior, max_repr_error); - if (err == EXIT_SUCCESS) - std::cout << "CALIBRATION success" << std::endl; - else - std::cout << "CALIBRATION failed" << std::endl; + // Add "Calibration in progress" message + int err = calibrate(image_count, image_folder, calib, target_w, target_h, + square_size, zed_info.serial_number, false, + can_use_calib_prior, max_repr_error); + if (err == EXIT_SUCCESS) + std::cout << "CALIBRATION success" << std::endl; + else + std::cout << "CALIBRATION failed" << std::endl; - zed_camera.close(); + zed_camera.close(); - return EXIT_SUCCESS; + return EXIT_SUCCESS; } // Function to perform linear interpolation inline float interpolate(float x, float x0, float x1, float y0 = 0, - float y1 = 100) { + float y1 = 100) { float interpolatedValue = y0 + (x - x0) * (y1 - y0) / (x1 - x0); interpolatedValue = (interpolatedValue < y0) @@ -637,7 +724,7 @@ bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, } float CheckCoverage(const std::vector>& pts, - const cv::Size& imgSize) { + const cv::Size& imgSize) { int min_h_ = 0; int max_h_ = bucketsize; float tot = 0; @@ -667,7 +754,7 @@ float CheckCoverage(const std::vector>& pts, cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { // sy is the magnitude in the X-Y plane for pitch calculation float sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + - R.at(1, 0) * R.at(1, 0)); + R.at(1, 0) * R.at(1, 0)); bool singular = sy < 1e-6; float roll, pitch, yaw; @@ -688,87 +775,86 @@ cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { return cv::Vec3d(roll, pitch, yaw); } -bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, bool first_time) { +bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, + bool first_time) { + std::cout << "************************************************" << std::endl; + std::cout << " * Current rvec: [" << rvec.at(0) << ", " + << rvec.at(1) << ", " << rvec.at(2) << "]" + << std::endl; + std::cout << " * Current tvec: [" << tvec.at(0) << ", " + << tvec.at(1) << ", " << tvec.at(2) << "]" + << std::endl; + + // Convert rotation vector to rotation matrix + cv::Mat R; + cv::Rodrigues(rvec, R); - std::cout << "************************************************" << std::endl; - std::cout << " * Current rvec: [" << rvec.at(0) << ", " << rvec.at(1) << ", " << rvec.at(2) << "]" << std::endl; - std::cout << " * Current tvec: [" << tvec.at(0) << ", " << tvec.at(1) << ", " << tvec.at(2) << "]" << std::endl; + cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); - // Convert rotation vector to rotation matrix - cv::Mat R; - cv::Rodrigues(rvec, R); + // Convert radians to degrees + float rz = eulerAngles[0] * 180.0 / M_PI; + float rx = eulerAngles[1] * 180.0 / M_PI; + float ry = eulerAngles[2] * 180.0 / M_PI; - cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); + std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry + << std::endl; - // Convert radians to degrees - float rz = eulerAngles[0] * 180.0 / M_PI; - float rx = eulerAngles[1] * 180.0 / M_PI; - float ry = eulerAngles[2] * 180.0 / M_PI; + float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + + pow(tvec.at(2), 2)); + std::cout << "Distance: " << distance << " mm" << std::endl; - std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry << std::endl; + if (fabs(rz) > 45.0) { + std::cerr << " * Images ignored: Rot Z > 45° [" << rz << "°]" << std::endl; + return false; + } - float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + pow(tvec.at(2), 2)); - std::cout << "Distance: " << distance << " mm" << std::endl; + if (first_time) { + checker_.rot_x_min = rx; + checker_.rot_y_min = ry; + checker_.rot_z_min = rz; - if(fabs(rz)>45.0) - { - std::cerr << " * Images ignored: Rot Z > 45° ["<< rz <<"°]" << std::endl; - return false; - } + checker_.rot_x_max = rx; + checker_.rot_y_max = ry; + checker_.rot_z_max = rz; - if(first_time) { - checker_.rot_x_min = rx; - checker_.rot_y_min = ry; - checker_.rot_z_min = rz; + checker_.d_min = distance; + checker_.d_max = distance; - checker_.rot_x_max = rx; - checker_.rot_y_max = ry; - checker_.rot_z_max = rz; + checker_.rot_x_delta = 0; + checker_.rot_y_delta = 0; + checker_.rot_z_delta = 0; + checker_.distance_tot = 0; - checker_.d_min = distance; - checker_.d_max = distance; + return true; + } - checker_.rot_x_delta = 0; - checker_.rot_y_delta = 0; - checker_.rot_z_delta = 0; - checker_.distance_tot = 0; + // 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; - return true; - } + // 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; - // 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 > distance) { - checker_.d_min = distance; - } - if (checker_.d_max < distance) { - checker_.d_max = distance; - } + if (checker_.d_min > distance) { + checker_.d_min = distance; + } + if (checker_.d_max < distance) { + checker_.d_max = distance; + } - // 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; + // 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; + 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; + return true; } From 14b6b4e8358ee8d82741eaca51155d7060049044 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 14 Nov 2025 19:29:47 +0100 Subject: [PATCH 09/34] Test sample collection completeness --- .../include/calibration_checker.hpp | 7 +- .../src/calibration_checker.cpp | 375 ++++++++++-------- stereo_calibration/src/main.cpp | 4 +- 3 files changed, 216 insertions(+), 170 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 1d4dad7..dd40760 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -35,10 +35,11 @@ class CalibrationChecker { } // Calculate the sample collection status according to the stored samples - void calculateSampleCollectionStatus( float& size_score, float& skew_score, - float& pos_score_x, float& pos_score_y ) const; + bool evaluateSampleCollectionStatus(float& size_score, float& skew_score, + float& pos_score_x, + float& pos_score_y) const; -private: + private: // Calculate the parameter of a detected checkerboard BoardParams getDetectedBoarParams(const std::vector& corners, cv::Size image_size); diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 5ea7376..2168ef4 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -7,210 +7,255 @@ 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 ) -{ - // 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)); - } +CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size) { + // 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) { - BoardParams params = getDetectedBoarParams(corners, image_size); +bool CalibrationChecker::testSample(const std::vector& corners, + cv::Size image_size) { + BoardParams params = getDetectedBoarParams(corners, image_size); - if(params.size < 0 || params.skew < 0) { - return false; // Invalid parameters - } + if (params.size < 0 || params.skew < 0) { + return false; // Invalid parameters + } - bool is_good = isGoodSample(params, corners, validCorners_.empty() ? std::vector() : validCorners_.back()); + bool is_good = isGoodSample(params, corners, + validCorners_.empty() ? std::vector() + : validCorners_.back()); - if(is_good) { - // Store the valid parameters and associated corners - paramDb_.push_back(params); - validCorners_.push_back(corners); - } + if (is_good) { + // Store the valid parameters and associated corners + paramDb_.push_back(params); + validCorners_.push_back(corners); + } - std::cout << std::setprecision(3) << "Sample: Pos(" << params.pos.x << ", " << params.pos.y << "), Size: " << params.size << ", Skew: " << params.skew << " => " << (is_good ? "Accepted" : "Rejected") << std::endl; + std::cout << std::setprecision(3) << "Sample: Pos(" << params.pos.x << ", " + << params.pos.y << "), Size: " << params.size + << ", Skew: " << params.skew << " => " + << (is_good ? "Accepted" : "Rejected") << std::endl; - return is_good; + return is_good; } -float CalibrationChecker::compute_skew(const std::vector& outside_corners) { - 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); - return std::acos(dot / (norm_ab * norm_cb)); - }; - - // Calculate skew as deviation from 90 degrees - 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]))); - - return skew; +float CalibrationChecker::compute_skew( + const std::vector& outside_corners) { + 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); + return std::acos(dot / (norm_ab * norm_cb)); + }; + + // Calculate skew as deviation from 90 degrees + 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]))); + + return skew; } -float CalibrationChecker::compute_area(const std::vector& outside_corners) { - if (outside_corners.size() != 4) { - return -1.0f; // Invalid input - } +float CalibrationChecker::compute_area( + const std::vector& outside_corners) { + 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]; + // 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; + cv::Point2f p = b + c; + cv::Point2f q = a + b; - float area = 0.5f * std::abs(p.x * q.y - p.y * q.x); + float area = 0.5f * std::abs(p.x * q.y - p.y * q.x); - return area; + return area; } std::vector CalibrationChecker::get_outside_corners( - const std::vector& corners){ - std::vector outside_corners; + const std::vector& corners) { + std::vector outside_corners; - if(corners.size() != board_.board_size.area()){ - return 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; + size_t x_dim = board_.board_size.width; + size_t y_dim = board_.board_size.height; - outside_corners.resize(4); + 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; -} + 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 -BoardParams CalibrationChecker::getDetectedBoarParams(const std::vector& corners, cv::Size image_size) { - BoardParams params; - - auto outside_corners = get_outside_corners(corners); - float area = compute_area(outside_corners); - float skew = compute_skew(outside_corners); - - if(area < 0 || 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()); + return outside_corners; +} - 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))); +BoardParams CalibrationChecker::getDetectedBoarParams( + const std::vector& corners, cv::Size image_size) { + BoardParams params; - params.pos = cv::Point2f(p_x, p_y); - params.size = std::sqrt(area / (image_size.width * image_size.height)); - params.skew = skew; + auto outside_corners = get_outside_corners(corners); + float area = compute_area(outside_corners); + float skew = compute_skew(outside_corners); + if (area < 0 || 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))); + + 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 BoardParams& params, const std::vector& corners, const std::vector& prev_corners) { + if (paramDb_.empty()) { + return true; // First sample is always good + } - if(paramDb_.empty()) { - return true; // First sample is always good - } - - auto param_distance = [](const BoardParams& p1, - const BoardParams& 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); - }; - - 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 existing samples (dist=" - << dist << ")" << std::endl; - return false; - } + auto param_distance = [](const BoardParams& p1, + const BoardParams& 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); + }; + + 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 existing samples (dist=" << dist + << ")" << std::endl; + return false; } + } - return true; + return true; } -void CalibrationChecker::calculateSampleCollectionStatus(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; - } +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; - } + 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_size = 0.0f; + // min_skew = 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 true; + } + + if (paramDb_.size() >= max_samples_) { + std::cout + << "Sample collection complete: Reached the maximum sample count (" + << paramDb_.size() << "/" << max_samples_ << ")" << std::endl; + return true; + } - // Don't reward small size or skew - //min_size = 0.0f; - //min_skew = 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 + 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; } \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 15bc54b..52765ad 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -502,8 +502,8 @@ int main(int argc, char* argv[]) { image_count++; float size_score, skew_score, pos_score_x, pos_score_y; - checker.calculateSampleCollectionStatus(size_score, skew_score, - pos_score_x, pos_score_y); + checker.evaluateSampleCollectionStatus(size_score, skew_score, + pos_score_x, pos_score_y); } else { std::cout << " ! Sample detected but not valid. Please check the " From cf522f5c6300efe0ffefeabd129b3590511b7c37 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 18 Nov 2025 17:17:56 +0100 Subject: [PATCH 10/34] Calibration validated --- .../include/calibration_checker.hpp | 128 ++-- .../include/opencv_calibration.hpp | 33 +- .../src/calibration_checker.cpp | 7 +- stereo_calibration/src/main.cpp | 582 +++++++++--------- stereo_calibration/src/opencv_calibration.cpp | 33 +- 5 files changed, 422 insertions(+), 361 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index dd40760..565920c 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -4,64 +4,86 @@ #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 + 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 _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 + 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 } BoardParams; class CalibrationChecker { -public: - CalibrationChecker( cv::Size board_size, float square_size ); - ~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; - - private: - // Calculate the parameter of a detected checkerboard - BoardParams getDetectedBoarParams(const std::vector& corners, cv::Size image_size); - - // Check if the detected corners are valid - bool isGoodSample(const BoardParams& params, - const std::vector& corners, - const std::vector& prev_corners); - - // 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_ - - const BoardParams idealParams_ = {cv::Point2f(0.65f, 0.65f), 0.4f, 0.f}; // Ideal parameters for a good sample database - const size_t min_samples_ = 20; // Minimum number of samples to consider the database complete - const size_t max_samples_ = 50; // Maximum number of samples to consider the database complete + public: + CalibrationChecker(cv::Size board_size, float square_size, bool verbose); + ~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; + + private: + // Calculate the parameter of a detected checkerboard + BoardParams getDetectedBoarParams(const std::vector& corners, + cv::Size image_size); + + // Check if the detected corners are valid + bool isGoodSample(const BoardParams& params, + const std::vector& corners, + const std::vector& prev_corners); + + // 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_ + + const BoardParams idealParams_ = { + 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.7f // Checkerboard skew variation should be at least 70% + }; // Ideal parameters for a good sample database + const size_t min_samples_ = + 20; // Minimum number of samples to consider the database complete + const size_t max_samples_ = + 50; // Maximum number of samples to consider the database complete (used if it's not possible to reach the ideal parameters) + + bool verbose_ = false; }; -#endif // CALIBRATION_CHECKER_HPP +#endif // CALIBRATION_CHECKER_HPP diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 0b3eb75..f5ed5c1 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -74,18 +74,27 @@ struct CameraCalib{ float calibrate(const std::vector> &object_points, const std::vector> &image_points, - const cv::Size &image_size, int flags) { - float rms = 0.0; + 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; rms = cv::calibrateCamera(object_points, image_points, image_size, K, D, rvec, tvec, flags); - } else + } 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); + } + + if (verbose) { + std::cout << " * Intrinsic matrix K:" << std::endl << K << std::endl; + std::cout << " * Distortion coefficients D:" << std::endl + << D << std::endl; + std::cout << " * Re-projection error (RMS): " << rms << std::endl; + } + return rms; } }; @@ -128,19 +137,29 @@ struct StereoCalib{ const std::vector> &object_points, const std::vector> &image_points_left, const std::vector> &image_points_right, - const cv::Size &image_size, int flags) { + const cv::Size &image_size, int flags, bool verbose) { float rms = 0.0; cv::Mat E, F; - if (left.disto_model_RadTan && right.disto_model_RadTan) + 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 + } 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); + + if (verbose) { + std::cout << " * Rotation matrix R:" << std::endl << R << std::endl; + std::cout << " * Rotation vector Rv:" << std::endl << Rv << std::endl; + std::cout << " * Translation vector T:" << std::endl << T << std::endl; + std::cout << " * Re-projection error (RMS): " << rms << std::endl; + } + return rms; } }; @@ -148,4 +167,4 @@ struct StereoCalib{ int calibrate(int img_count, 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, - float max_repr_error = 0.5f); \ No newline at end of file + 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 index 2168ef4..aad9302 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -7,7 +7,10 @@ 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) { +CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, + bool verbose) { + verbose_ = verbose; + // Initialize the board parameters board_.board_size = board_size; board_.square_size = square_size; @@ -239,7 +242,7 @@ bool CalibrationChecker::evaluateSampleCollectionStatus( std::cout << "Sample collection incomplete: not reached the minimum sample " "count (" << paramDb_.size() << "/" << min_samples_ << ")" << std::endl; - return true; + return false; } if (paramDb_.size() >= max_samples_) { diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 52765ad..e2d7c59 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -21,45 +21,47 @@ constexpr float square_size = 25.4; // mm std::string image_folder = "zed-images/"; -int verbose = 0; - -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; - - float rot_x_delta; - float rot_y_delta; - float rot_z_delta; - - float d_min; - float d_max; - float distance_tot; -}; +// 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; + +// float rot_x_delta; +// float rot_y_delta; +// float rot_z_delta; + +// float d_min; +// float d_max; +// float distance_tot; +// }; 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, - bool first_time); +// 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, +// bool first_time); /// Rendering constexpr int text_area_height = 200; /// Calibration condition -const float min_coverage = 90; // in percentage -const float min_rotation = 60; // in degrees -const float acceptable_rotation = 50; // in degrees -const float min_distance = 300; // in mm -const float acceptable_distance = 200; // in mm +// const float min_coverage = 90; // in percentage +// const float min_rotation = 60; // in degrees +// const float acceptable_rotation = 50; // in degrees +// const float min_distance = 300; // in mm +// const float acceptable_distance = 200; // in mm const float max_repr_error = 0.5; // in pixels -std::vector> pts_detected; +// Debug +bool verbose = true; +int sdk_verbose = 1; + +//std::vector> pts_detected; std::vector square_valid; int bucketsize = 480; @@ -158,7 +160,7 @@ struct Args { int main(int argc, char* argv[]) { // Setup the calibration checker - CalibrationChecker checker(cv::Size(target_w, target_h), square_size); + CalibrationChecker checker(cv::Size(target_w, target_h), square_size, verbose); Args args; args.parse(argc, argv); @@ -184,7 +186,7 @@ int main(int argc, char* argv[]) { init_params.enable_image_validity_check = false; // Disable image validity check for performance init_params.camera_disable_self_calib = true; - init_params.sdk_verbose = verbose; + init_params.sdk_verbose = sdk_verbose; // Configure the Virtual Stereo Camera if '--zedxone' argument is provided if (args.is_zed_x_one_virtual_stereo) { @@ -314,8 +316,8 @@ int main(int argc, char* argv[]) { } // Data for initial intrinsic estimation - std::vector> pts_init_im_l, pts_init_im_r; - std::vector> pts_init_obj; + //std::vector> pts_init_im_l, pts_init_im_r; + //std::vector> pts_init_obj; // Size of the rendered images const cv::Size display_size(720, 404); @@ -480,7 +482,7 @@ int main(int argc, char* argv[]) { if (found_l && found_r) { scaleKP(pts_l, display_size, cv::Size(camera_resolution.width, camera_resolution.height)); - pts_init_im_l.push_back(pts_l); + // 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); @@ -502,8 +504,10 @@ int main(int argc, char* argv[]) { image_count++; float size_score, skew_score, pos_score_x, pos_score_y; - checker.evaluateSampleCollectionStatus(size_score, skew_score, - pos_score_x, pos_score_y); + if( checker.evaluateSampleCollectionStatus(size_score, skew_score, + pos_score_x, pos_score_y) ) { + std::cout << ">>> Sample collection status: COMPLETE <<<" << std::endl; + acquisition_completed = true;} } else { std::cout << " ! Sample detected but not valid. Please check the " @@ -585,7 +589,7 @@ int main(int argc, char* argv[]) { // Add "Calibration in progress" message int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, - can_use_calib_prior, max_repr_error); + can_use_calib_prior, max_repr_error, true); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION success" << std::endl; else @@ -607,254 +611,254 @@ inline float interpolate(float x, float x0, float x1, float y0 = 0, return interpolatedValue; } -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); - - return status; -} - -bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, - std::vector> pts) { - int count = 0; - - 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++; - } - } - } - } - - if (min) { - if (count > MinPts) - return true; - else - return false; - } else { - if (count < MaxPts) - return true; - else - return false; - } -} - -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; -} - -// Convert rotation vector (rvec) to Euler angles (roll, pitch, yaw) in radians -cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { - // sy is the magnitude in the X-Y plane for pitch calculation - float sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + - R.at(1, 0) * R.at(1, 0)); - bool singular = sy < 1e-6; - - float roll, pitch, yaw; - if (!singular) { - // Following OpenCV camera frame axis convention: - // roll around Z axis - roll = std::atan2(R.at(1, 0), - R.at(0, 0)); // rotation about Z (roll) - pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) - yaw = std::atan2(R.at(2, 1), - R.at(2, 2)); // rotation about Y (yaw) - } else { - // Gimbal lock case - roll = std::atan2(-R.at(1, 2), R.at(1, 1)); - pitch = std::atan2(-R.at(2, 0), sy); - yaw = 0; - } - return cv::Vec3d(roll, pitch, yaw); -} - -bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, - bool first_time) { - std::cout << "************************************************" << std::endl; - std::cout << " * Current rvec: [" << rvec.at(0) << ", " - << rvec.at(1) << ", " << rvec.at(2) << "]" - << std::endl; - std::cout << " * Current tvec: [" << tvec.at(0) << ", " - << tvec.at(1) << ", " << tvec.at(2) << "]" - << std::endl; - - // Convert rotation vector to rotation matrix - cv::Mat R; - cv::Rodrigues(rvec, R); - - cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); - - // Convert radians to degrees - float rz = eulerAngles[0] * 180.0 / M_PI; - float rx = eulerAngles[1] * 180.0 / M_PI; - float ry = eulerAngles[2] * 180.0 / M_PI; - - std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry - << std::endl; - - float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + - pow(tvec.at(2), 2)); - std::cout << "Distance: " << distance << " mm" << std::endl; - - if (fabs(rz) > 45.0) { - std::cerr << " * Images ignored: Rot Z > 45° [" << rz << "°]" << std::endl; - return false; - } - - if (first_time) { - checker_.rot_x_min = rx; - checker_.rot_y_min = ry; - checker_.rot_z_min = rz; - - checker_.rot_x_max = rx; - checker_.rot_y_max = ry; - checker_.rot_z_max = rz; - - checker_.d_min = distance; - checker_.d_max = distance; - - checker_.rot_x_delta = 0; - checker_.rot_y_delta = 0; - checker_.rot_z_delta = 0; - checker_.distance_tot = 0; - - return true; - } - - // 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 > distance) { - checker_.d_min = distance; - } - if (checker_.d_max < distance) { - checker_.d_max = distance; - } - - // 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; -} +// 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); + +// return status; +// } + +// bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, +// std::vector> pts) { +// int count = 0; + +// 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++; +// } +// } +// } +// } + +// if (min) { +// if (count > MinPts) +// return true; +// else +// return false; +// } else { +// if (count < MaxPts) +// return true; +// else +// return false; +// } +// } + +// 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; +// } + +// // Convert rotation vector (rvec) to Euler angles (roll, pitch, yaw) in radians +// cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { +// // sy is the magnitude in the X-Y plane for pitch calculation +// float sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + +// R.at(1, 0) * R.at(1, 0)); +// bool singular = sy < 1e-6; + +// float roll, pitch, yaw; +// if (!singular) { +// // Following OpenCV camera frame axis convention: +// // roll around Z axis +// roll = std::atan2(R.at(1, 0), +// R.at(0, 0)); // rotation about Z (roll) +// pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) +// yaw = std::atan2(R.at(2, 1), +// R.at(2, 2)); // rotation about Y (yaw) +// } else { +// // Gimbal lock case +// roll = std::atan2(-R.at(1, 2), R.at(1, 1)); +// pitch = std::atan2(-R.at(2, 0), sy); +// yaw = 0; +// } +// return cv::Vec3d(roll, pitch, yaw); +// } + +// bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, +// bool first_time) { +// std::cout << "************************************************" << std::endl; +// std::cout << " * Current rvec: [" << rvec.at(0) << ", " +// << rvec.at(1) << ", " << rvec.at(2) << "]" +// << std::endl; +// std::cout << " * Current tvec: [" << tvec.at(0) << ", " +// << tvec.at(1) << ", " << tvec.at(2) << "]" +// << std::endl; + +// // Convert rotation vector to rotation matrix +// cv::Mat R; +// cv::Rodrigues(rvec, R); + +// cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); + +// // Convert radians to degrees +// float rz = eulerAngles[0] * 180.0 / M_PI; +// float rx = eulerAngles[1] * 180.0 / M_PI; +// float ry = eulerAngles[2] * 180.0 / M_PI; + +// std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry +// << std::endl; + +// float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + +// pow(tvec.at(2), 2)); +// std::cout << "Distance: " << distance << " mm" << std::endl; + +// if (fabs(rz) > 45.0) { +// std::cerr << " * Images ignored: Rot Z > 45° [" << rz << "°]" << std::endl; +// return false; +// } + +// if (first_time) { +// checker_.rot_x_min = rx; +// checker_.rot_y_min = ry; +// checker_.rot_z_min = rz; + +// checker_.rot_x_max = rx; +// checker_.rot_y_max = ry; +// checker_.rot_z_max = rz; + +// checker_.d_min = distance; +// checker_.d_max = distance; + +// checker_.rot_x_delta = 0; +// checker_.rot_y_delta = 0; +// checker_.rot_z_delta = 0; +// checker_.distance_tot = 0; + +// return true; +// } + +// // 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 > distance) { +// checker_.d_min = distance; +// } +// if (checker_.d_max < distance) { +// checker_.d_max = distance; +// } + +// // 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; +// } diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 65d180f..9ea4048 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -3,7 +3,7 @@ int calibrate(int img_count, 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, - float max_repr_error) { + float max_repr_error, bool verbose) { std::vector left_images, right_images; /// Read images @@ -33,8 +33,10 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, } } - std::cout << std::endl - << "\t" << left_images.size() << " images opened" << std::endl; + if (verbose) { + std::cout << std::endl + << "\t" << left_images.size() << " images opened" << std::endl; + } // Define object points of the target std::vector pattern_points; @@ -72,7 +74,7 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, pts_r.push_back(pts_r_); object_points.push_back(pattern_points); } else { - std::cout << "No target detected on image " << i << std::endl; + std::cout << "No target detected on image " << i << std::endl; } } @@ -89,11 +91,19 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, 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: " << std::endl; + if (verbose) { + std::cout << "Left camera calibration: " << std::endl; + } + auto rms_l = calib_data.left.calibrate(object_points, pts_l, imageSize, + flags, verbose); + + if (verbose) { + std::cout << "Right camera calibration: " << std::endl; + } + auto rms_r = calib_data.right.calibrate(object_points, pts_r, imageSize, + flags, verbose); + + std::cout << " * Reprojection errors: " << std::endl; std::cout << " * Left " << rms_l << (rms_l > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; @@ -101,9 +111,12 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; + if (verbose) { + std::cout << "Stereo calibration: " << std::endl; + } auto err = calib_data.calibrate( object_points, pts_l, pts_r, imageSize, - cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY); + cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); std::cout << " * Stereo " << err << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; From 5342e6a4c8c1462727f10a38cf7b1988d62c758f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 18 Nov 2025 18:36:33 +0100 Subject: [PATCH 11/34] Change params diff check --- .../include/calibration_checker.hpp | 26 +++++++------- .../src/calibration_checker.cpp | 36 ++++++++++++------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 565920c..f55a607 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -12,12 +12,12 @@ typedef struct _board { cv::Size2f board_size_mm = {0.0f, 0.0f}; // Physical size of the board in mm } Board; -typedef struct _board_params { +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 -} BoardParams; +} DetectedBoardParams; class CalibrationChecker { public: @@ -42,11 +42,11 @@ class CalibrationChecker { private: // Calculate the parameter of a detected checkerboard - BoardParams getDetectedBoarParams(const std::vector& corners, - cv::Size image_size); + DetectedBoardParams getDetectedBoarParams( + const std::vector& corners, cv::Size image_size); // Check if the detected corners are valid - bool isGoodSample(const BoardParams& params, + bool isGoodSample(const DetectedBoardParams& params, const std::vector& corners, const std::vector& prev_corners); @@ -64,20 +64,20 @@ class CalibrationChecker { private: Board board_; - std::vector + std::vector paramDb_; // Database of previously detected board parameters std::vector> validCorners_; // All the corners associated to the single parameters in // paramDb_ - const BoardParams idealParams_ = { + const DetectedBoardParams idealParams_ = { 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.7f // Checkerboard skew variation should be at least 70% - }; // Ideal parameters for a good sample database + 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 const size_t min_samples_ = 20; // Minimum number of samples to consider the database complete const size_t max_samples_ = diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index aad9302..e607462 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -28,7 +28,7 @@ CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, bool CalibrationChecker::testSample(const std::vector& corners, cv::Size image_size) { - BoardParams params = getDetectedBoarParams(corners, image_size); + DetectedBoardParams params = getDetectedBoarParams(corners, image_size); if (params.size < 0 || params.skew < 0) { return false; // Invalid parameters @@ -118,9 +118,9 @@ std::vector CalibrationChecker::get_outside_corners( return outside_corners; } -BoardParams CalibrationChecker::getDetectedBoarParams( +DetectedBoardParams CalibrationChecker::getDetectedBoarParams( const std::vector& corners, cv::Size image_size) { - BoardParams params; + DetectedBoardParams params; auto outside_corners = get_outside_corners(corners); float area = compute_area(outside_corners); @@ -160,23 +160,35 @@ BoardParams CalibrationChecker::getDetectedBoarParams( } bool CalibrationChecker::isGoodSample( - const BoardParams& params, const std::vector& corners, + const DetectedBoardParams& params, const std::vector& corners, const std::vector& prev_corners) { if (paramDb_.empty()) { return true; // First sample is always good } - auto param_distance = [](const BoardParams& p1, - const BoardParams& 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); + auto check_distance = [](const DetectedBoardParams& p1, + const DetectedBoardParams& p2) -> bool { + // Check that at least one parameter differs by at least 10% + float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); + float skew_diff = std::abs(p1.skew - p2.skew) / std::max(p1.skew, p2.skew); + float pos_x_diff = + std::abs(p1.pos.x - p2.pos.x) / std::max(p1.pos.x, p2.pos.x); + float pos_y_diff = + std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); + + const float threshold = 0.1f; + + if (size_diff < threshold && skew_diff < threshold && + pos_x_diff < threshold && pos_y_diff < threshold) { + return false; // Too similar + } + return true; }; 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 existing samples (dist=" << dist - << ")" << std::endl; + bool is_different = check_distance(params, stored_params); + if (!is_different) { // TODO tune the threshold + std::cout << " Rejected: Too similar to existing samples" << std::endl; return false; } } From 5b9ea5000b8a5357b643a289a94bda34b2958cec Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 Nov 2025 20:04:30 +0100 Subject: [PATCH 12/34] Better user feedback --- .../include/calibration_checker.hpp | 38 +- .../include/opencv_calibration.hpp | 293 ++++----- .../src/calibration_checker.cpp | 22 +- stereo_calibration/src/main.cpp | 612 ++++-------------- stereo_calibration/src/opencv_calibration.cpp | 19 +- 5 files changed, 335 insertions(+), 649 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index f55a607..5ccd359 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -19,9 +19,25 @@ typedef struct _detected_board_params { 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 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, bool verbose); + CalibrationChecker(cv::Size board_size, float square_size, + size_t min_samples = DEFAULT_MIN_SAMPLES, + size_t max_samples = DEFAULT_MAX_SAMPLES, + DetectedBoardParams idealParams = DEFAULT_IDEAL_PARAMS, + bool verbose = false); ~CalibrationChecker() = default; // Test if the detected corners form a valid sample @@ -40,6 +56,9 @@ class CalibrationChecker { 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 getDetectedBoarParams( @@ -70,18 +89,11 @@ class CalibrationChecker { validCorners_; // All the corners associated to the single parameters in // paramDb_ - const DetectedBoardParams idealParams_ = { - 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 - const size_t min_samples_ = - 20; // Minimum number of samples to consider the database complete - const size_t max_samples_ = - 50; // Maximum number of samples to consider the database complete (used if it's not possible to reach the ideal parameters) + 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 bool verbose_ = false; }; diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index f5ed5c1..2b95753 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -1,170 +1,171 @@ #pragma once +#include +#include +#include #include #include -#include -#include -#include constexpr int MIN_IMAGE = 20; -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; +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 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 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; + + // 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(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; - - // 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]; - } + } + + 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); } - - 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; + return undistorted_points; + } + + float 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; + 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); } - float 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; - 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); - } - - if (verbose) { - std::cout << " * Intrinsic matrix K:" << std::endl << K << std::endl; - std::cout << " * Distortion coefficients D:" << std::endl - << D << std::endl; - std::cout << " * Re-projection error (RMS): " << rms << std::endl; - } - - return rms; + if (verbose) { + std::cout << " * Intrinsic matrix K:" << std::endl << K << std::endl; + std::cout << " * Distortion coefficients D:" << std::endl + << D << std::endl; + std::cout << " * 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 - - 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); +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 + + 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); + std::cout << " Lens disto model " + << (left.disto_model_RadTan && right.disto_model_RadTan) + << std::endl; + } + + float 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) { + float 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); } - 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); - 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, bool verbose) { - float 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); - - if (verbose) { - std::cout << " * Rotation matrix R:" << std::endl << R << std::endl; - std::cout << " * Rotation vector Rv:" << std::endl << Rv << std::endl; - std::cout << " * Translation vector T:" << std::endl << T << std::endl; - std::cout << " * Re-projection error (RMS): " << rms << std::endl; - } - - return rms; + if (verbose) { + std::cout << " * Rotation matrix R:" << std::endl << R << std::endl; + std::cout << " * Rotation vector Rv:" << std::endl << Rv << std::endl; + std::cout << " * Translation vector T:" << std::endl << T << std::endl; + std::cout << " * Re-projection error (RMS): " << rms << std::endl; } + + return rms; + } }; int calibrate(int img_count, 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, - float max_repr_error = 0.5f, bool verbose=false); \ No newline at end of file + 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 index e607462..c77444c 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -8,9 +8,15 @@ 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, + DetectedBoardParams idealParams, bool verbose) { verbose_ = verbose; + // Calibration parameters + min_samples_ = min_samples; + max_samples_ = max_samples; + // Initialize the board parameters board_.board_size = board_size; board_.square_size = square_size; @@ -176,7 +182,7 @@ bool CalibrationChecker::isGoodSample( float pos_y_diff = std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); - const float threshold = 0.1f; + const float threshold = 0.05f; if (size_diff < threshold && skew_diff < threshold && pos_x_diff < threshold && pos_y_diff < threshold) { @@ -223,10 +229,6 @@ bool CalibrationChecker::evaluateSampleCollectionStatus( if (params.skew > max_skew) max_skew = params.skew; } - // Don't reward small size or skew - // min_size = 0.0f; - // min_skew = 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); @@ -273,4 +275,14 @@ bool CalibrationChecker::evaluateSampleCollectionStatus( 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 e2d7c59..3bf2471 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -21,50 +21,36 @@ constexpr float square_size = 25.4; // mm std::string image_folder = "zed-images/"; -// 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; - -// float rot_x_delta; -// float rot_y_delta; -// float rot_z_delta; - -// float d_min; -// float d_max; -// float distance_tot; -// }; - +// Argument parsing helper 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, -// bool first_time); + +// Coverage indicator fill helpers +void addNewCheckerboardPosition(cv::Mat& coverage_indicator, float norm_x, + float norm_y, float norm_size); +void applyCoverageIndicatorOverlay(cv::Mat& image, + const cv::Mat& coverage_indicator); /// Rendering -constexpr int text_area_height = 200; +constexpr int text_area_height = 210; +const cv::Size display_size(720, 404); // Size of the rendered images /// Calibration condition -// const float min_coverage = 90; // in percentage -// const float min_rotation = 60; // in degrees -// const float acceptable_rotation = 50; // in degrees -// const float min_distance = 300; // in mm -// const float acceptable_distance = 200; // in mm -const float max_repr_error = 0.5; // in pixels +const float max_repr_error = 0.5; // in pixels +const int min_samples = 30; +const int max_samples = 60; +const float min_x_coverage = + 0.65f; // Checkerboard X position should cover 65% of the image width +const float min_y_coverage = + 0.65f; // Checkerboard Y position should cover 65% of the image height +const float min_size_variation = + 0.4f; // Checkerboard size variation should be at least 40% +const float min_skew_variation = + 0.6f; // Checkerboard skew variation should be at least 70% // Debug -bool verbose = true; -int sdk_verbose = 1; - -//std::vector> pts_detected; +bool verbose = false; +int sdk_verbose = 0; -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); @@ -160,7 +146,14 @@ struct Args { int main(int argc, char* argv[]) { // Setup the calibration checker - CalibrationChecker checker(cv::Size(target_w, target_h), square_size, verbose); + const DetectedBoardParams idealParams = { + cv::Point2f(min_x_coverage, min_y_coverage), min_size_variation, + min_skew_variation}; + CalibrationChecker checker(cv::Size(target_w, target_h), square_size, + min_samples, max_samples, idealParams, verbose); + // Coverage scores + float size_score = 0.0f, skew_score = 0.0f, pos_score_x = 0.0f, + pos_score_y = 0.0f; Args args; args.parse(argc, argv); @@ -281,19 +274,11 @@ int main(int argc, char* argv[]) { auto rgb_r = cv::Mat(camera_resolution.height, camera_resolution.width, CV_8UC4, zed_imageR.getPtr()); - // Number of area to fill horizontally - bucketsize = camera_resolution.width / 6; - - // bool frames_rot_good=true; - cv::Mat blank = cv::Mat::zeros(camera_resolution.height, - camera_resolution.width, CV_8UC1); + cv::Mat coverage_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; - // extrinsic_checker checker; - // float cov_left = 1.0; - // bool angle_clb = false; - // bool first_angle_check = true; bool acquisition_completed = false; std::vector pts_obj_; for (int i = 0; i < target_h; i++) { @@ -315,17 +300,11 @@ int main(int argc, char* argv[]) { return 1; } - // Data for initial intrinsic estimation - //std::vector> pts_init_im_l, pts_init_im_r; - //std::vector> pts_init_obj; - - // Size of the rendered images - const cv::Size display_size(720, 404); - 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); @@ -345,38 +324,10 @@ int main(int argc, char* argv[]) { 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); + applyCoverageIndicatorOverlay(rgb_d_fill, coverage_indicator); + std::vector pts_l, pts_r; bool found_l = false; bool found_r = false; @@ -399,27 +350,6 @@ int main(int argc, char* argv[]) { cv::Mat text_info = cv::Mat::ones( cv::Size(display.size[1], text_area_height), 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 + - // 10) { // Add 10 extra images for better results - // acquisition_completed = true; - // } else { - // std::stringstream ss; - // ss << "Not enough images for calibration saved. Missing " - // << ((MIN_IMAGE + 10) - image_count) << " images."; - // cv::putText(text_info, ss.str(), - // cv::Point(10, display.size[0] + 140), - // cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); - // } - // } - // } - cv::vconcat(display, text_info, rendering_image); if (acquisition_completed) { @@ -427,42 +357,82 @@ int main(int argc, char* argv[]) { "Acquisition completed! Wait for the calibration " "computation to complete...", cv::Point(20, display.size[0] + 50), - cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + 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) - cv::putText(rendering_image, "Missing target on one of the images.", - cv::Point(30, display.size[0] + 170), - cv::FONT_HERSHEY_SIMPLEX, 0.8, warn_color, 2); + 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] + 110), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); + } - cv::putText(rendering_image, - "Press 's' or the spacebar to save the current frames when " - "the target is visible in both images.", - cv::Point(10, display.size[0] + 25), - cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); - // if(coverage_mode) { - // std::stringstream ss_cov; - // ss_cov << "Coverage: " << std::fixed << std::setprecision(2) << - // (1-cov_left)*100 << "%/" << min_coverage << "%"; - // cv::putText(rendering_image, ss_cov.str(), cv::Point(10, - // display.size[0]+55), cv::FONT_HERSHEY_SIMPLEX, 0.6, info_color, - // 1); cv::putText(rendering_image, "Keep going until the green - // covers the image, it represents coverage", cv::Point(10, - // display.size[0]+85), 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]+50), cv::FONT_HERSHEY_SIMPLEX, - // 0.8, warn_color, 2); - // } + 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] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); + } + + if (low_target_variability_on_last_pics) { + cv::putText(rendering_image, + " * Target too similar to a previous acquisition.", + cv::Point(display.size[1] / 2, display.size[0] + 140), + cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); + } + + 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 << "Frame saved: " << image_count; + ss_img_count << " * Sample saved: " << image_count; cv::putText(rendering_image, ss_img_count.str(), - cv::Point((display.size[1] + display_size.width) / 2, - display.size[0] + 170), - cv::FONT_HERSHEY_SIMPLEX, 0.8, info_color, 2); + 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); } cv::imshow(window_name, rendering_image); @@ -473,23 +443,18 @@ int main(int argc, char* argv[]) { } if ((key == 's' || key == 'S') || key == ' ') { - // if (!angle_clb) { - // coverage_mode = true; - // } - 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)); - // 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_); 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; @@ -503,87 +468,27 @@ int main(int argc, char* argv[]) { std::cout << " * Images saved" << std::endl; image_count++; - float size_score, skew_score, pos_score_x, pos_score_y; - if( checker.evaluateSampleCollectionStatus(size_score, skew_score, - pos_score_x, pos_score_y) ) { - std::cout << ">>> Sample collection status: COMPLETE <<<" << std::endl; - acquisition_completed = true;} - + if (checker.evaluateSampleCollectionStatus( + size_score, skew_score, pos_score_x, pos_score_y)) { + std::cout << ">>> Sample collection status: COMPLETE <<<" + << std::endl; + acquisition_completed = true; + } + + // 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, norm_x, norm_y, norm_size); } else { std::cout << " ! Sample detected but not valid. Please check the " "checkerboard position and angle." << std::endl; + low_target_variability_on_last_pics = true; } - - // if (need_intrinsic_estimation) { - // // wait 5 images before running the estimate - // if (pts_init_im_l.size() >= 5) { - // 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)); - // std::cout << "Intrinsic estimation done:" << std::endl; - // std::cout << "Left K: " << std::endl << calib.left.K << - // std::endl; std::cout << "Right K: " << std::endl << - // calib.right.K << std::endl; 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 - // << "%/" << min_coverage << "%" << std::endl; - // if (cov_left < ((100.0 - min_coverage) / 100.0)) { - // // Coverage Complete. Start angle calibration - // std::cout << "Coverage complete. Start angle " - // "calibration." - // << std::endl; - - // // Estimate intrinsic with a better image coverage - // need_intrinsic_estimation = true; - - // angle_clb = true; - // } - // } else { - // cv::Mat rvec(1, 3, CV_32FC1); - // cv::Mat tvec(1, 3, CV_32FC1); - // std::cout << "Before undistort" << pts_l << std::endl; - // auto undist_pts = calib.left.undistortPoints(pts_l); - // std::cout << "After undistort" << undist_pts - // << std::endl; - // 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, first_angle_check); - // if (frames_rot_good) { - // pts_detected.push_back(pts_l); - // first_angle_check = false; - // } - // } - // } - // } - - // if (frames_rot_good) { - // // 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++; - // } } } } - // sl::sleep_ms(10); } // Add "Calibration in progress" message @@ -600,265 +505,20 @@ int main(int argc, char* argv[]) { return EXIT_SUCCESS; } -// 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; +void addNewCheckerboardPosition(cv::Mat& coverage_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(coverage_indicator, cv::Point(x, y), size, + cv::Scalar(255, 255, 255), -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); - -// return status; -// } - -// bool CheckBucket(int min_h, int max_h, int min_w, int max_w, bool min, -// std::vector> pts) { -// int count = 0; - -// 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++; -// } -// } -// } -// } - -// if (min) { -// if (count > MinPts) -// return true; -// else -// return false; -// } else { -// if (count < MaxPts) -// return true; -// else -// return false; -// } -// } - -// 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; -// } - -// // Convert rotation vector (rvec) to Euler angles (roll, pitch, yaw) in radians -// cv::Vec3d rotationMatrixToEulerAngles(const cv::Mat& R) { -// // sy is the magnitude in the X-Y plane for pitch calculation -// float sy = std::sqrt(R.at(0, 0) * R.at(0, 0) + -// R.at(1, 0) * R.at(1, 0)); -// bool singular = sy < 1e-6; - -// float roll, pitch, yaw; -// if (!singular) { -// // Following OpenCV camera frame axis convention: -// // roll around Z axis -// roll = std::atan2(R.at(1, 0), -// R.at(0, 0)); // rotation about Z (roll) -// pitch = std::atan2(-R.at(2, 0), sy); // rotation about X (pitch) -// yaw = std::atan2(R.at(2, 1), -// R.at(2, 2)); // rotation about Y (yaw) -// } else { -// // Gimbal lock case -// roll = std::atan2(-R.at(1, 2), R.at(1, 1)); -// pitch = std::atan2(-R.at(2, 0), sy); -// yaw = 0; -// } -// return cv::Vec3d(roll, pitch, yaw); -// } - -// bool updateRT(extrinsic_checker& checker_, cv::Mat rvec, cv::Mat tvec, -// bool first_time) { -// std::cout << "************************************************" << std::endl; -// std::cout << " * Current rvec: [" << rvec.at(0) << ", " -// << rvec.at(1) << ", " << rvec.at(2) << "]" -// << std::endl; -// std::cout << " * Current tvec: [" << tvec.at(0) << ", " -// << tvec.at(1) << ", " << tvec.at(2) << "]" -// << std::endl; - -// // Convert rotation vector to rotation matrix -// cv::Mat R; -// cv::Rodrigues(rvec, R); - -// cv::Vec3d eulerAngles = rotationMatrixToEulerAngles(R); - -// // Convert radians to degrees -// float rz = eulerAngles[0] * 180.0 / M_PI; -// float rx = eulerAngles[1] * 180.0 / M_PI; -// float ry = eulerAngles[2] * 180.0 / M_PI; - -// std::cout << "Roll: " << rz << " Pitch: " << rx << " Yaw: " << ry -// << std::endl; - -// float distance = sqrt(pow(tvec.at(0), 2) + pow(tvec.at(1), 2) + -// pow(tvec.at(2), 2)); -// std::cout << "Distance: " << distance << " mm" << std::endl; - -// if (fabs(rz) > 45.0) { -// std::cerr << " * Images ignored: Rot Z > 45° [" << rz << "°]" << std::endl; -// return false; -// } - -// if (first_time) { -// checker_.rot_x_min = rx; -// checker_.rot_y_min = ry; -// checker_.rot_z_min = rz; - -// checker_.rot_x_max = rx; -// checker_.rot_y_max = ry; -// checker_.rot_z_max = rz; - -// checker_.d_min = distance; -// checker_.d_max = distance; - -// checker_.rot_x_delta = 0; -// checker_.rot_y_delta = 0; -// checker_.rot_z_delta = 0; -// checker_.distance_tot = 0; - -// return true; -// } - -// // 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 > distance) { -// checker_.d_min = distance; -// } -// if (checker_.d_max < distance) { -// checker_.d_max = distance; -// } - -// // 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[2] = channels[2] - coverage_indicator; + channels[1] = channels[1] - coverage_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 9ea4048..860e59d 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -35,7 +35,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, if (verbose) { std::cout << std::endl - << "\t" << left_images.size() << " images opened" << std::endl; + << "\t" << left_images.size() << " samples collected" + << std::endl; } // Define object points of the target @@ -88,7 +89,7 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, << std::endl; return EXIT_FAILURE; } else { - std::cout << " * Enough points detected" << std::endl; + std::cout << " * Enough valid samples: " << pts_l.size() << std::endl; auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; if (verbose) { @@ -103,6 +104,13 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, auto rms_r = calib_data.right.calibrate(object_points, pts_r, imageSize, flags, verbose); + if (verbose) { + std::cout << "Stereo calibration: " << std::endl; + } + auto err = calib_data.calibrate( + object_points, pts_l, pts_r, imageSize, + cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); + std::cout << " * Reprojection errors: " << std::endl; std::cout << " * Left " << rms_l << (rms_l > max_repr_error ? " !!! TOO HIGH !!!" : "") @@ -110,13 +118,6 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << " * Right " << rms_r << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; - - if (verbose) { - std::cout << "Stereo calibration: " << std::endl; - } - auto err = calib_data.calibrate( - object_points, pts_l, pts_r, imageSize, - cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); std::cout << " * Stereo " << err << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; From d462be5737eb8b3e39eb94de5efc546d2b5bf437 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 Nov 2025 20:45:02 +0100 Subject: [PATCH 13/34] Fix sample diff checker --- .../src/calibration_checker.cpp | 26 ++++++++++++------- stereo_calibration/src/main.cpp | 2 +- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index c77444c..4c086fe 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -172,9 +172,9 @@ bool CalibrationChecker::isGoodSample( return true; // First sample is always good } - auto check_distance = [](const DetectedBoardParams& p1, + auto check_distance = [this](const DetectedBoardParams& p1, const DetectedBoardParams& p2) -> bool { - // Check that at least one parameter differs by at least 10% + // Check that at least one parameter differs by at least 10% from all the stored samples float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); float skew_diff = std::abs(p1.skew - p2.skew) / std::max(p1.skew, p2.skew); float pos_x_diff = @@ -182,23 +182,29 @@ bool CalibrationChecker::isGoodSample( float pos_y_diff = std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); - const float threshold = 0.05f; + if (verbose_) { + std::cout << " Differences: SizeDiff=" << size_diff + << ", SkewDiff=" << skew_diff << ", PosXDiff=" << pos_x_diff + << ", PosYDiff=" << pos_y_diff << std::endl; + } + + const float diff_thresh = 0.1f; - if (size_diff < threshold && skew_diff < threshold && - pos_x_diff < threshold && pos_y_diff < threshold) { - return false; // Too similar + if (size_diff > diff_thresh || skew_diff > diff_thresh || + pos_x_diff > diff_thresh || pos_y_diff > diff_thresh) { + return true; } - return true; + return false; // Too similar to all existing samples }; for (auto& stored_params : paramDb_) { bool is_different = check_distance(params, stored_params); - if (!is_different) { // TODO tune the threshold - std::cout << " Rejected: Too similar to existing samples" << std::endl; + if (!is_different) { + std::cout << " Rejected: Too similar to an existing sample" << std::endl; return false; } } - + return true; } diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 3bf2471..949d16d 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -48,7 +48,7 @@ const float min_skew_variation = 0.6f; // Checkerboard skew variation should be at least 70% // Debug -bool verbose = false; +bool verbose = true; int sdk_verbose = 0; const int MinPts = 10; From 9131b8cb034068e03a21b32ad34879e4c51ef050 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 Nov 2025 20:51:25 +0100 Subject: [PATCH 14/34] TODO Check sample differences --- .../src/calibration_checker.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 4c086fe..0c14fce 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -172,7 +172,7 @@ bool CalibrationChecker::isGoodSample( return true; // First sample is always good } - auto check_distance = [this](const DetectedBoardParams& p1, + 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 float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); @@ -181,25 +181,25 @@ bool CalibrationChecker::isGoodSample( std::abs(p1.pos.x - p2.pos.x) / std::max(p1.pos.x, p2.pos.x); float pos_y_diff = std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); - - if (verbose_) { - std::cout << " Differences: SizeDiff=" << size_diff - << ", SkewDiff=" << skew_diff << ", PosXDiff=" << pos_x_diff - << ", PosYDiff=" << pos_y_diff << std::endl; - } - - const float diff_thresh = 0.1f; + + const float diff_thresh = 0.1f; // 10% difference threshold if (size_diff > diff_thresh || skew_diff > diff_thresh || pos_x_diff > diff_thresh || pos_y_diff > diff_thresh) { - return true; + return true; // At least one parameter is sufficiently different } - return false; // Too similar to all existing samples + + std::cout << std::setprecision(3) + << "Size diff: " << size_diff << ", " + << "Skew diff: " << skew_diff << ", " + << "PosX diff: " << pos_x_diff << ", " + << "PosY diff: " << pos_y_diff << "" << std::endl; + + return false; // All parameters are too similar }; for (auto& stored_params : paramDb_) { - bool is_different = check_distance(params, stored_params); - if (!is_different) { + if (!is_different(params, stored_params)) { std::cout << " Rejected: Too similar to an existing sample" << std::endl; return false; } From 110f7cdc84f85db4d5aabbeba7404ff8c6fc4cce Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 11:34:32 +0100 Subject: [PATCH 15/34] Add 4-zone coverage indicator --- .../src/calibration_checker.cpp | 46 +++++--- stereo_calibration/src/main.cpp | 100 ++++++++++++++---- 2 files changed, 109 insertions(+), 37 deletions(-) diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 0c14fce..369fb7a 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -60,6 +60,11 @@ bool CalibrationChecker::testSample(const std::vector& corners, 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 } @@ -76,7 +81,7 @@ float CalibrationChecker::compute_skew( // Calculate skew as deviation from 90 degrees float skew = std::min( - 1.0f, 2.0f * std::abs(PI / 2 - angle(outside_corners[up_left], + 1.0f, 2.0f * std::abs((PI / 2) - angle(outside_corners[up_left], outside_corners[up_right], outside_corners[down_right]))); @@ -85,6 +90,12 @@ float CalibrationChecker::compute_skew( 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 } @@ -175,25 +186,32 @@ bool CalibrationChecker::isGoodSample( 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 - float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); - float skew_diff = std::abs(p1.skew - p2.skew) / std::max(p1.skew, p2.skew); - float pos_x_diff = - std::abs(p1.pos.x - p2.pos.x) / std::max(p1.pos.x, p2.pos.x); - float pos_y_diff = - std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); - + float size_diff = std::abs(p1.size - p2.size); // / std::max(p1.size, p2.size); + float skew_diff = std::abs(p1.skew - p2.skew); // / std::max(p1.skew, p2.skew); + float pos_x_diff = std::abs(p1.pos.x - p2.pos.x); // / std::max(p1.pos.x, p2.pos.x); + float pos_y_diff = std::abs(p1.pos.y - p2.pos.y); // / std::max(p1.pos.y, p2.pos.y); + const float diff_thresh = 0.1f; // 10% difference threshold + if(verbose_) { + std::cout << std::setprecision(3) + << "PosX diff: " << pos_x_diff << ", " + << "PosY diff: " << pos_y_diff << ", " + << "Size diff: " << size_diff << ", " + << "Skew diff: " << skew_diff; + } + if (size_diff > diff_thresh || skew_diff > diff_thresh || pos_x_diff > diff_thresh || pos_y_diff > diff_thresh) { - return true; // At least one parameter is sufficiently different + if(verbose_) { + std::cout << " => Different enough." << std::endl; + } + return true; // At least one parameter is sufficiently different } - std::cout << std::setprecision(3) - << "Size diff: " << size_diff << ", " - << "Skew diff: " << skew_diff << ", " - << "PosX diff: " << pos_x_diff << ", " - << "PosY diff: " << pos_y_diff << "" << std::endl; + if(verbose_) { + std::cout << " => Too similar." << std::endl; + } return false; // All parameters are too similar }; diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 949d16d..783ca81 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -25,10 +25,12 @@ std::string image_folder = "zed-images/"; std::map parseArguments(int argc, char* argv[]); // Coverage indicator fill helpers -void addNewCheckerboardPosition(cv::Mat& coverage_indicator, float norm_x, +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); /// Rendering constexpr int text_area_height = 210; @@ -36,29 +38,30 @@ const cv::Size display_size(720, 404); // Size of the rendered images /// Calibration condition const float max_repr_error = 0.5; // in pixels -const int min_samples = 30; -const int max_samples = 60; +const int min_samples = 25; +const int max_samples = 45; const float min_x_coverage = - 0.65f; // Checkerboard X position should cover 65% of the image width + 0.7f; // Checkerboard X position should cover 70% of the image width const float min_y_coverage = - 0.65f; // Checkerboard Y position should cover 65% of the image height -const float min_size_variation = - 0.4f; // Checkerboard size variation should be at least 40% -const float min_skew_variation = - 0.6f; // Checkerboard skew variation should be at least 70% + 0.7f; // Checkerboard Y position should cover 70% of the image height +const float min_area_range = + 0.4f; // Checkerboard area range size should be at least 0.4 [min_area-max_area] +const float min_skew_range = + 0.5f; // Checkerboard skew ange size should be at least 0.5 [min_skew-max_skew] // Debug bool verbose = true; int sdk_verbose = 0; -const int MinPts = 10; -const int MaxPts = 90; +// Text colors const cv::Scalar info_color = cv::Scalar(50, 205, 50); const cv::Scalar warn_color = cv::Scalar(0, 128, 255); +// SIDE-by-SIDE or TOP-BOTTOM image stacking for display const bool image_stack_horizontal = true; // true for horizontal, false for vertical +// 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); @@ -147,8 +150,8 @@ struct Args { int main(int argc, char* argv[]) { // Setup the calibration checker const DetectedBoardParams idealParams = { - cv::Point2f(min_x_coverage, min_y_coverage), min_size_variation, - min_skew_variation}; + cv::Point2f(min_x_coverage, min_y_coverage), min_area_range, + min_skew_range}; CalibrationChecker checker(cv::Size(target_w, target_h), square_size, min_samples, max_samples, idealParams, verbose); // Coverage scores @@ -277,6 +280,9 @@ int main(int argc, char* argv[]) { 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; @@ -327,6 +333,7 @@ int main(int argc, char* argv[]) { 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; @@ -366,14 +373,14 @@ int main(int argc, char* argv[]) { 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] + 110), + cv::Point(display.size[1] / 2, display.size[0] + 80), cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); } 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] + 140), + cv::Point(display.size[1] / 2, display.size[0] + 110), cv::FONT_HERSHEY_SIMPLEX, 0.7, warn_color, 2); } @@ -443,6 +450,8 @@ int main(int argc, char* argv[]) { } if ((key == 's' || key == 'S') || key == ' ') { + std::cout << "************************************************" << std::endl; + missing_target_on_last_pics = !found_r || !found_l; if (found_l && found_r) { @@ -479,7 +488,7 @@ int main(int argc, char* argv[]) { float norm_x = checker.getLastDetectedBoardParams().pos.x; float norm_y = checker.getLastDetectedBoardParams().pos.y; float norm_size = checker.getLastDetectedBoardParams().size; - addNewCheckerboardPosition(coverage_indicator, norm_x, norm_y, norm_size); + addNewCheckerboardPosition(coverage_indicator, pos_indicator, norm_x, norm_y, norm_size); } else { std::cout << " ! Sample detected but not valid. Please check the " "checkerboard position and angle." @@ -491,12 +500,12 @@ int main(int argc, char* argv[]) { } } - // Add "Calibration in progress" message + // Start the calibration process int err = calibrate(image_count, image_folder, calib, target_w, target_h, square_size, zed_info.serial_number, false, can_use_calib_prior, max_repr_error, true); if (err == EXIT_SUCCESS) - std::cout << "CALIBRATION success" << std::endl; + std::cout << "CALIBRATION successful" << std::endl; else std::cout << "CALIBRATION failed" << std::endl; @@ -505,20 +514,65 @@ int main(int argc, char* argv[]) { return EXIT_SUCCESS; } -void addNewCheckerboardPosition(cv::Mat& coverage_indicator, float norm_x, - float norm_y, float norm_size) { +int top_left_count = 0; +int top_right_count = 0; +int bottom_left_count = 0; +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(coverage_indicator, cv::Point(x, y), size, - cv::Scalar(255, 255, 255), -1); + 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 (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); + } } 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; - channels[1] = channels[1] - 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 From db871e40098c0dffccfbeb710f4dfdac9d740178 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 12:42:52 +0100 Subject: [PATCH 16/34] Check ZED X One model --- .../include/opencv_calibration.hpp | 12 ++- stereo_calibration/src/main.cpp | 57 ++++++++++---- stereo_calibration/src/opencv_calibration.cpp | 77 ++++++++++++------- 3 files changed, 99 insertions(+), 47 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 2b95753..62d9ea7 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -102,9 +102,12 @@ struct CameraCalib { 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); @@ -139,8 +142,12 @@ struct StereoCalib { const std::vector> &image_points_left, const std::vector> &image_points_right, const cv::Size &image_size, int flags, bool verbose) { + + imageSize = image_size; + float 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, @@ -163,9 +170,12 @@ struct StereoCalib { return rms; } + + std::string saveCalibOpenCV(int serial); + std::string saveCalibZED(int serial, bool is_4k = false); }; int calibrate(int img_count, const std::string &folder, StereoCalib &raw_data, - int target_w, int target_h, float square_size, int serial, + int target_w, int target_h, float square_size, int serial, 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/main.cpp b/stereo_calibration/src/main.cpp index 783ca81..5ad2805 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -158,6 +158,9 @@ int main(int argc, char* argv[]) { float size_score = 0.0f, skew_score = 0.0f, pos_score_x = 0.0f, pos_score_y = 0.0f; + // Flags + bool is_4k_camera = false; + Args args; args.parse(argc, argv); @@ -176,8 +179,7 @@ int main(int argc, char* argv[]) { 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_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 @@ -186,16 +188,18 @@ int main(int argc, char* argv[]) { // Configure the Virtual Stereo Camera if '--zedxone' argument is provided if (args.is_zed_x_one_virtual_stereo) { - if (args.left_camera_sn != -1 && args.right_camera_sn != -1) { + 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: " - << args.left_camera_sn << ", " << args.right_camera_sn - << std::endl; + << sn_left << ", " << sn_right << std::endl; int sn_stereo = sl::generateVirtualStereoSerialNumber( - args.left_camera_sn, args.right_camera_sn); + sn_left, sn_right); std::cout << "Virtual SN: " << sn_stereo << std::endl; init_params.input.setVirtualStereoFromSerialNumbers( - args.left_camera_sn, args.right_camera_sn, sn_stereo); + 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 " @@ -210,9 +214,7 @@ int main(int argc, char* argv[]) { << args.left_camera_id << ", " << args.right_camera_id << std::endl; - auto cams = sl::CameraOne::getDeviceList(); - int sn_left = -1; - int sn_right = -1; + auto cams = sl::CameraOne::getDeviceList(); for (auto& cam : cams) { if (cam.id == args.left_camera_id) { @@ -238,8 +240,29 @@ int main(int argc, char* argv[]) { init_params.input.setVirtualStereoFromCameraIDs( args.left_camera_id, args.right_camera_id, sn_stereo); } - } + int left_model = sn_left / 10000000; + int right_model = sn_right / 10000000; + + if(left_model != right_model) { + std::cerr << "Error: Left and Right cameras must be of the same model." + << std::endl; + return EXIT_FAILURE; + } + + 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); // in case of a virtual stereo camera, the calibration file can be not @@ -502,8 +525,8 @@ int main(int argc, char* argv[]) { // Start the calibration process int err = calibrate(image_count, image_folder, calib, target_w, target_h, - square_size, zed_info.serial_number, false, - can_use_calib_prior, max_repr_error, true); + square_size, zed_info.serial_number, is_4k_camera, false, + can_use_calib_prior, max_repr_error, verbose); if (err == EXIT_SUCCESS) std::cout << "CALIBRATION successful" << std::endl; else @@ -514,10 +537,10 @@ int main(int argc, char* argv[]) { return EXIT_SUCCESS; } -int top_left_count = 0; -int top_right_count = 0; -int bottom_left_count = 0; -int bottom_right_count = 0; +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) { diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 860e59d..0c7c870 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -2,7 +2,7 @@ int calibrate(int img_count, 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, + bool is_4k, bool save_calib_mono, bool use_intrinsic_prior, float max_repr_error, bool verbose) { std::vector left_images, right_images; @@ -124,10 +124,10 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, if (rms_l > 0.5f || rms_r > 0.5f || err > 0.5f) { std::cout << std::endl << "\t !! Warning !!" << std::endl - << " The reprojection error looks too high (>" << max_repr_error - << "), check that the lens are clean (sharp images)" - " and that the pattern is printed/mounted on a not flexible " - "and flat surface." + << "The max reprojection error looks too high (>" << max_repr_error + << "), check that the lenses are clean (sharp images)" + " and that the pattern is printed/mounted on a RIGID " + "and FLAT surface." << std::endl; return EXIT_FAILURE; @@ -150,35 +150,54 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << "* Rotation:" << std::endl << calib_data.Rv << std::endl; std::cout << std::endl; - std::cout << std::endl << "*** Calibration file ***" << std::endl; - std::string calib_filename = - "zed_calibration_" + std::to_string(serial) + ".yml"; + std::cout << std::endl << "*** Save Calibration files ***" << std::endl; - std::cout << " * Saving calibration file: " << calib_filename << std::endl; - std::cout << " * If you want to use this calibration with the ZED SDK, you " - "can load it by using " - "sl::InitParameters::optional_opencv_calibration_file" - << std::endl; - std::cout << std::endl; + 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; + } + + 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::FileStorage fs(calib_filename, cv::FileStorage::WRITE); - if (fs.isOpened()) { - fs << "Size" << imageSize; - fs << "K_LEFT" << calib_data.left.K << "K_RIGHT" << calib_data.right.K; + return EXIT_SUCCESS; +} - if (calib_data.left.disto_model_RadTan) { - fs << "D_LEFT" << calib_data.left.D << "D_RIGHT" << calib_data.right.D; - } else { - fs << "D_LEFT_FE" << calib_data.left.D << "D_RIGHT_FE" - << calib_data.right.D; - } +std::string StereoCalib::saveCalibOpenCV(int serial) { + std::string calib_filename = + "zed_calibration_" + std::to_string(serial) + ".yml"; - fs << "R" << calib_data.Rv << "T" << calib_data.T; - fs.release(); + cv::FileStorage fs(calib_filename, cv::FileStorage::WRITE); + if (fs.isOpened()) { + fs << "Size" << imageSize; + fs << "K_LEFT" << left.K << "K_RIGHT" << right.K; + + if (left.disto_model_RadTan) { + fs << "D_LEFT" << left.D << "D_RIGHT" << right.D; } else { - std::cout << "Error: can not save the extrinsic parameters" << std::endl; - return EXIT_FAILURE; + fs << "D_LEFT_FE" << left.D << "D_RIGHT_FE" + << right.D; } + + fs << "R" << Rv << "T" << T; + fs.release(); + + return calib_filename; } - return EXIT_SUCCESS; + + return std::string(); +} + +std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { + + + return std::string(); } \ No newline at end of file From 309b34df70f089e1081c628fbcefb54d3159f5f2 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 13:02:06 +0100 Subject: [PATCH 17/34] Add function to save SDK conf file --- .../include/opencv_calibration.hpp | 7 +- stereo_calibration/src/main.cpp | 102 ++++++++++-------- stereo_calibration/src/opencv_calibration.cpp | 15 +-- 3 files changed, 70 insertions(+), 54 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 62d9ea7..b2f6bf0 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -176,6 +176,7 @@ struct StereoCalib { }; int calibrate(int img_count, const std::string &folder, StereoCalib &raw_data, - int target_w, int target_h, float square_size, int serial, 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 + int target_w, int target_h, 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/main.cpp b/stereo_calibration/src/main.cpp index 5ad2805..458d171 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -30,7 +30,7 @@ void addNewCheckerboardPosition(cv::Mat& coverage_indicator, 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); +void applyPosIndicatorOverlay(cv::Mat& image, const cv::Mat& pos_indicator); /// Rendering constexpr int text_area_height = 210; @@ -44,10 +44,10 @@ const float min_x_coverage = 0.7f; // Checkerboard X position should cover 70% of the image width const float min_y_coverage = 0.7f; // Checkerboard Y position should cover 70% of the image height -const float min_area_range = - 0.4f; // Checkerboard area range size should be at least 0.4 [min_area-max_area] -const float min_skew_range = - 0.5f; // Checkerboard skew ange size should be at least 0.5 [min_skew-max_skew] +const float min_area_range = 0.4f; // Checkerboard area range size should be at + // least 0.4 [min_area-max_area] +const float min_skew_range = 0.5f; // Checkerboard skew ange size should be at + // least 0.5 [min_skew-max_skew] // Debug bool verbose = true; @@ -159,6 +159,7 @@ int main(int argc, char* argv[]) { pos_score_y = 0.0f; // Flags + bool is_dual_mono_camera = false; bool is_4k_camera = false; Args args; @@ -179,7 +180,8 @@ int main(int argc, char* argv[]) { 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_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 @@ -188,6 +190,7 @@ int main(int argc, char* argv[]) { // Configure the Virtual Stereo Camera if '--zedxone' argument is provided if (args.is_zed_x_one_virtual_stereo) { + is_dual_mono_camera = true; int sn_left = args.left_camera_sn; int sn_right = args.right_camera_sn; @@ -195,11 +198,10 @@ int main(int argc, char* argv[]) { 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); + int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); std::cout << "Virtual SN: " << sn_stereo << std::endl; - init_params.input.setVirtualStereoFromSerialNumbers( - sn_left, sn_right, sn_stereo); + 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 " @@ -214,7 +216,7 @@ int main(int argc, char* argv[]) { << args.left_camera_id << ", " << args.right_camera_id << std::endl; - auto cams = sl::CameraOne::getDeviceList(); + auto cams = sl::CameraOne::getDeviceList(); for (auto& cam : cams) { if (cam.id == args.left_camera_id) { @@ -244,25 +246,26 @@ int main(int argc, char* argv[]) { int left_model = sn_left / 10000000; int right_model = sn_right / 10000000; - if(left_model != right_model) { + if (left_model != right_model) { std::cerr << "Error: Left and Right cameras must be of the same model." << std::endl; return EXIT_FAILURE; - } + } 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 { + 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; + std::cout << " * ZED X One GS Virtual Stereo Camera detected." + << std::endl; } } - + auto status = zed_camera.open(init_params); // in case of a virtual stereo camera, the calibration file can be not @@ -460,7 +463,8 @@ int main(int argc, char* argv[]) { 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.", + "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); } @@ -473,7 +477,8 @@ int main(int argc, char* argv[]) { } if ((key == 's' || key == 'S') || key == ' ') { - std::cout << "************************************************" << std::endl; + std::cout << "************************************************" + << std::endl; missing_target_on_last_pics = !found_r || !found_l; @@ -511,7 +516,8 @@ int main(int argc, char* argv[]) { 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); + addNewCheckerboardPosition(coverage_indicator, pos_indicator, + norm_x, norm_y, norm_size); } else { std::cout << " ! Sample detected but not valid. Please check the " "checkerboard position and angle." @@ -525,8 +531,9 @@ int main(int argc, char* argv[]) { // Start the calibration process int err = calibrate(image_count, image_folder, calib, target_w, target_h, - square_size, zed_info.serial_number, is_4k_camera, false, - can_use_calib_prior, max_repr_error, verbose); + 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 << "CALIBRATION successful" << std::endl; else @@ -542,15 +549,16 @@ 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) { +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) { + 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++; @@ -560,26 +568,31 @@ void addNewCheckerboardPosition(cv::Mat& coverage_indicator, cv::Mat& pos_indica bottom_right_count++; } - 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_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 (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_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); } - 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); - } } void applyCoverageIndicatorOverlay(cv::Mat& image, @@ -591,8 +604,7 @@ void applyCoverageIndicatorOverlay(cv::Mat& image, cv::merge(channels, image); } -void applyPosIndicatorOverlay(cv::Mat& image, - const cv::Mat& pos_indicator) { +void applyPosIndicatorOverlay(cv::Mat& image, const cv::Mat& pos_indicator) { std::vector channels; cv::split(image, channels); channels[2] = channels[2] - pos_indicator; diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 0c7c870..a6fa9ce 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -2,7 +2,7 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, int target_w, int target_h, float square_size, int serial, - bool is_4k, bool save_calib_mono, bool use_intrinsic_prior, + 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; @@ -160,11 +160,14 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, << opencv_file << " !!!" << std::endl; } - 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; + // 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; + } } } From 8dd778a796747c7250973b7a6c7973bd62e581ab Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 13:11:51 +0100 Subject: [PATCH 18/34] Save SDK calib file --- stereo_calibration/src/opencv_calibration.cpp | 228 +++++++++++++++++- 1 file changed, 217 insertions(+), 11 deletions(-) diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index a6fa9ce..cb5cd5e 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -2,8 +2,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, int target_w, int target_h, 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) { + 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 @@ -124,7 +124,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, if (rms_l > 0.5f || rms_r > 0.5f || err > 0.5f) { std::cout << std::endl << "\t !! Warning !!" << std::endl - << "The max reprojection error looks too high (>" << max_repr_error + << "The max reprojection error looks too high (>" + << max_repr_error << "), check that the lenses are clean (sharp images)" " and that the pattern is printed/mounted on a RIGID " "and FLAT surface." @@ -154,19 +155,22 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::string opencv_file = calib_data.saveCalibOpenCV(serial); if (!opencv_file.empty()) { - std::cout << " * OpenCV calibration file saved: " << opencv_file << std::endl; + std::cout << " * OpenCV calibration file saved: " << opencv_file + << std::endl; } else { - std::cout << " !!! Failed to save OpenCV calibration file " - << opencv_file << " !!!" << std::endl; + std::cout << " !!! Failed to save OpenCV calibration file " << opencv_file + << " !!!" << std::endl; } // 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; + 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; + std::cout << " !!! Failed to save ZED SDK calibration file " << zed_file + << " !!!" << std::endl; } } } @@ -186,8 +190,7 @@ std::string StereoCalib::saveCalibOpenCV(int serial) { if (left.disto_model_RadTan) { fs << "D_LEFT" << left.D << "D_RIGHT" << right.D; } else { - fs << "D_LEFT_FE" << left.D << "D_RIGHT_FE" - << right.D; + fs << "D_LEFT_FE" << left.D << "D_RIGHT_FE" << right.D; } fs << "R" << Rv << "T" << T; @@ -200,7 +203,210 @@ std::string StereoCalib::saveCalibOpenCV(int serial) { } std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { + std::string calib_filename = "SN" + std::to_string(serial) + ".conf"; + + // Write parameters to a text file + std::ofstream outfile(calib_filename); + if (!outfile.is_open()) { + std::cerr + << " !!! Cannot save the calibration file: 'Unable to open output file'" + << std::endl; + return std::string(); + } + + if (!is_4k) { // AR0234 + outfile << "[LEFT_CAM_FHD1200]\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + + outfile << "[RIGHT_CAM_FHD1200]\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + + outfile << "[LEFT_CAM_FHD]\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) - 60 << "\n\n"; + + outfile << "[RIGHT_CAM_FHD]\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; + + outfile << "[LEFT_CAM_SVGA]\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + + outfile << "[RIGHT_CAM_SVGA]\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + + // Add other parameters for other cameras... + + outfile << "[LEFT_DISTO]\n"; + outfile << "k1 = " << left.D.at(0) << "\n"; + outfile << "k2 = " << left.D.at(1) << "\n"; + outfile << "p1 = " << left.D.at(2) << "\n"; + outfile << "p2 = " << left.D.at(3) << "\n"; + outfile << "k3 = " << left.D.at(4) << "\n"; + outfile << "k4 = " << left.D.at(5) << "\n"; + outfile << "k5 = " << left.D.at(6) << "\n"; + outfile << "k6 = " << left.D.at(7) << "\n\n"; + + outfile << "[RIGHT_DISTO]\n"; + outfile << "k1 = " << right.D.at(0) << "\n"; + outfile << "k2 = " << right.D.at(1) << "\n"; + outfile << "p1 = " << right.D.at(2) << "\n"; + outfile << "p2 = " << right.D.at(3) << "\n"; + outfile << "k3 = " << right.D.at(4) << "\n"; + outfile << "k4 = " << right.D.at(5) << "\n"; + outfile << "k5 = " << right.D.at(6) << "\n"; + outfile << "k6 = " << right.D.at(7) << "\n\n"; + + outfile << "[STEREO]\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << R.at(1) << "\n"; + outfile << "CV_SVGA = " << R.at(1) << "\n"; + outfile << "CV_FHD1200 = " << R.at(1) << "\n"; + outfile << "RX_FHD = " << R.at(0) << "\n"; + outfile << "RX_SVGA = " << R.at(0) << "\n"; + outfile << "RX_FHD1200 = " << R.at(0) << "\n"; + outfile << "RZ_FHD = " << R.at(2) << "\n"; + outfile << "RZ_SVGA = " << R.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << R.at(2) << "\n\n"; + + // Add other parameters for other stereo parameters if needed... + + outfile << "[MISC]\n"; + outfile << "Sensor_ID = 1\n\n"; + + outfile.close(); + std::cout << " * Parameter file written successfully: '" << calib_filename + << "'" << std::endl; + return calib_filename; + } else { // IMX678 + outfile << "[LEFT_CAM_4k]\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + + outfile << "[RIGHT_CAM_4k]\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + + outfile << "[LEFT_CAM_QHDPLUS]\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 3200) / 2 + << "\n"; + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1800) / 2 + << "\n\n"; + + outfile << "[RIGHT_CAM_QHDPLUS]\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 3200) / 2 + << "\n"; + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1800) / 2 + << "\n\n"; + + outfile << "[LEFT_CAM_FHD]\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + + outfile << "[RIGHT_CAM_FHD]\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + + outfile << "[LEFT_CAM_FHD1200]\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 1920) / 2 + << "\n"; + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1200) / 2 + << "\n\n"; + + outfile << "[RIGHT_CAM_FHD1200]\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 1920) / 2 + << "\n"; + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 + << "\n\n"; + + // Add other parameters for other cameras... + + outfile << "[LEFT_DISTO]\n"; + outfile << "k1 = " << left.D.at(0) << "\n"; + outfile << "k2 = " << left.D.at(1) << "\n"; + outfile << "p1 = " << left.D.at(2) << "\n"; + outfile << "p2 = " << left.D.at(3) << "\n"; + outfile << "k3 = " << left.D.at(4) << "\n"; + outfile << "k4 = " << left.D.at(5) << "\n"; + outfile << "k5 = " << left.D.at(6) << "\n"; + outfile << "k6 = " << left.D.at(7) << "\n\n"; + + outfile << "[RIGHT_DISTO]\n"; + outfile << "k1 = " << right.D.at(0) << "\n"; + outfile << "k2 = " << right.D.at(1) << "\n"; + outfile << "p1 = " << right.D.at(2) << "\n"; + outfile << "p2 = " << right.D.at(3) << "\n"; + outfile << "k3 = " << right.D.at(4) << "\n"; + outfile << "k4 = " << right.D.at(5) << "\n"; + outfile << "k5 = " << right.D.at(6) << "\n"; + outfile << "k6 = " << right.D.at(7) << "\n\n"; + + outfile << "[STEREO]\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << R.at(1) << "\n"; + outfile << "CV_FHD1200 = " << R.at(1) << "\n"; + outfile << "CV_4k = " << R.at(1) << "\n"; + outfile << "CV_QHDPLUS = " << R.at(1) << "\n"; + outfile << "RX_FHD = " << R.at(0) << "\n"; + outfile << "RX_FHD1200 = " << R.at(0) << "\n"; + outfile << "RX_4k = " << R.at(0) << "\n"; + outfile << "RX_QHDPLUS = " << R.at(0) << "\n"; + outfile << "RZ_FHD = " << R.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << R.at(2) << "\n"; + outfile << "RZ_4k = " << R.at(2) << "\n\n"; + outfile << "RZ_QHDPLUS = " << R.at(2) << "\n\n"; + + // Add other parameters for other stereo parameters if needed... + + outfile << "[MISC]\n"; + outfile << "Sensor_ID = 2\n\n"; + + outfile.close(); + std::cout << " * Parameter file written successfully: '" << calib_filename + << "'" << std::endl; + return calib_filename; + } + std::cout + << "The resolution for the calibration is not valid\n\nUse 4k " + "(3840x2160) for ZED X One 4K and FHD1200 (1920x1200) for ZED X One GS" + << std::endl; - return std::string(); + return std::string(); } \ No newline at end of file From 4f2108ba7ba15e03f5e83f05322660e735976f31 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 13:27:48 +0100 Subject: [PATCH 19/34] Fix Rotation vector for SDK calib file --- stereo_calibration/src/main.cpp | 2 +- stereo_calibration/src/opencv_calibration.cpp | 24 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 458d171..cce61c6 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -454,7 +454,7 @@ int main(int argc, char* argv[]) { (skew_score > 1.0f ? info_color : warn_color), 1); std::stringstream ss_img_count; - ss_img_count << " * Sample saved: " << image_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, diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index cb5cd5e..2ea7e7c 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -379,18 +379,18 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "Baseline = " << -T.at(0) << "\n"; outfile << "TY = " << T.at(1) << "\n"; outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << R.at(1) << "\n"; - outfile << "CV_FHD1200 = " << R.at(1) << "\n"; - outfile << "CV_4k = " << R.at(1) << "\n"; - outfile << "CV_QHDPLUS = " << R.at(1) << "\n"; - outfile << "RX_FHD = " << R.at(0) << "\n"; - outfile << "RX_FHD1200 = " << R.at(0) << "\n"; - outfile << "RX_4k = " << R.at(0) << "\n"; - outfile << "RX_QHDPLUS = " << R.at(0) << "\n"; - outfile << "RZ_FHD = " << R.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << R.at(2) << "\n"; - outfile << "RZ_4k = " << R.at(2) << "\n\n"; - outfile << "RZ_QHDPLUS = " << R.at(2) << "\n\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "CV_4k = " << Rv.at(1) << "\n"; + outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RX_4k = " << Rv.at(0) << "\n"; + outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; + outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; + outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... From aa2f42d2f6473841ffa78bfa86d9d1f40653ba3a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 13:31:14 +0100 Subject: [PATCH 20/34] Minor Fix --- stereo_calibration/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index cce61c6..894d298 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -454,7 +454,7 @@ int main(int argc, char* argv[]) { (skew_score > 1.0f ? info_color : warn_color), 1); std::stringstream ss_img_count; - ss_img_count << " * Sample saved: " << image_count << "[min. " << min_samples << "]"; + 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, From f5430ee0a9fea3bc0cee3307f7ef6fc40507b561 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 17:12:36 +0100 Subject: [PATCH 21/34] Improve skew calc and fix SDK calib saving --- .../include/calibration_checker.hpp | 4 +- .../src/calibration_checker.cpp | 127 ++++++++++++------ stereo_calibration/src/main.cpp | 10 +- stereo_calibration/src/opencv_calibration.cpp | 88 ++++++------ 4 files changed, 140 insertions(+), 89 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 5ccd359..802a7ec 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -65,9 +65,7 @@ class CalibrationChecker { const std::vector& corners, cv::Size image_size); // Check if the detected corners are valid - bool isGoodSample(const DetectedBoardParams& params, - const std::vector& corners, - const std::vector& prev_corners); + bool isGoodSample(const DetectedBoardParams& params); // Helper functions std::vector get_outside_corners( diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 369fb7a..b1df713 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -16,6 +16,7 @@ CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, // Calibration parameters min_samples_ = min_samples; max_samples_ = max_samples; + idealParams_ = idealParams; // Initialize the board parameters board_.board_size = board_size; @@ -40,22 +41,21 @@ bool CalibrationChecker::testSample(const std::vector& corners, return false; // Invalid parameters } - bool is_good = isGoodSample(params, corners, - validCorners_.empty() ? std::vector() - : validCorners_.back()); + std::cout << std::setprecision(3) << " * New Sample: Pos(" << params.pos.x + << ", " << params.pos.y << "), Size: " << params.size + << ", Skew: " << params.skew << std::endl; - if (is_good) { + 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 << std::setprecision(3) << "Sample: Pos(" << params.pos.x << ", " - << params.pos.y << "), Size: " << params.size - << ", Skew: " << params.skew << " => " - << (is_good ? "Accepted" : "Rejected") << std::endl; - - return is_good; + std::cout << " * Sample rejected." << std::endl; + return false; } float CalibrationChecker::compute_skew( @@ -76,16 +76,32 @@ float CalibrationChecker::compute_skew( 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); - return std::acos(dot / (norm_ab * norm_cb)); + 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); }; - // Calculate skew as deviation from 90 degrees - 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]))); + // 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 skew; + return maxDeviation / (PI / 2); } float CalibrationChecker::compute_area( @@ -176,40 +192,64 @@ DetectedBoardParams CalibrationChecker::getDetectedBoarParams( return params; } -bool CalibrationChecker::isGoodSample( - const DetectedBoardParams& params, const std::vector& corners, - const std::vector& prev_corners) { +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); + // }; + + // 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 existing samples (dist=" << + // dist + // << ")" << std::endl; + // return false; + // } + // } + + // 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 - float size_diff = std::abs(p1.size - p2.size); // / std::max(p1.size, p2.size); - float skew_diff = std::abs(p1.skew - p2.skew); // / std::max(p1.skew, p2.skew); - float pos_x_diff = std::abs(p1.pos.x - p2.pos.x); // / std::max(p1.pos.x, p2.pos.x); - float pos_y_diff = std::abs(p1.pos.y - p2.pos.y); // / std::max(p1.pos.y, p2.pos.y); - - const float diff_thresh = 0.1f; // 10% difference threshold - - if(verbose_) { - std::cout << std::setprecision(3) - << "PosX diff: " << pos_x_diff << ", " - << "PosY diff: " << pos_y_diff << ", " - << "Size diff: " << size_diff << ", " - << "Skew diff: " << skew_diff; + const DetectedBoardParams& p2) -> bool { + // Check that at least one parameter differs by at least 10% from all the + // stored samples + float pos_x_diff = + std::abs(p1.pos.x - p2.pos.x) / std::max(p1.pos.x, p2.pos.x); + float pos_y_diff = + std::abs(p1.pos.y - p2.pos.y) / std::max(p1.pos.y, p2.pos.y); + float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); + float skew_diff = std::abs(p1.skew - p2.skew) / std::max(p1.skew, p2.skew); + + const float diff_thresh = 0.1f; // 10% difference threshold + + if (verbose_) { + std::cout << "Comparing to: Pos(" << p2.pos.x << ", " << p2.pos.y + << "), Size: " << p2.size << ", Skew: " << p2.skew << std::endl; + std::cout << std::setprecision(3) << "PosX diff: " << pos_x_diff * 100.0f + << "%, " + << "PosY diff: " << pos_y_diff * 100.0f << "%, " + << "Size diff: " << size_diff * 100.0f << "%, " + << "Skew diff: " << skew_diff * 100.0f << "%"; } 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 << " => Different enough." << std::endl; + } + return true; // At least one parameter is sufficiently different } - if(verbose_) { + if (verbose_) { std::cout << " => Too similar." << std::endl; } @@ -217,12 +257,13 @@ bool CalibrationChecker::isGoodSample( }; 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; } @@ -253,6 +294,10 @@ bool CalibrationChecker::evaluateSampleCollectionStatus( 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); diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 894d298..47b09bd 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -44,13 +44,13 @@ const float min_x_coverage = 0.7f; // Checkerboard X position should cover 70% of the image width const float min_y_coverage = 0.7f; // Checkerboard Y position should cover 70% of the image height -const float min_area_range = 0.4f; // Checkerboard area range size should be at +const float min_area_range = 0.45f; // Checkerboard area range size should be at // least 0.4 [min_area-max_area] -const float min_skew_range = 0.5f; // Checkerboard skew ange size should be at +const float min_skew_range = 0.45f; // Checkerboard skew ange size should be at // least 0.5 [min_skew-max_skew] // Debug -bool verbose = true; +bool verbose = false; int sdk_verbose = 0; // Text colors @@ -473,6 +473,8 @@ int main(int argc, char* argv[]) { key = cv::waitKey(10); if (acquisition_completed) { + std::cout << "Starting calibration process..." << std::endl + << std::endl; break; } @@ -508,7 +510,7 @@ int main(int argc, char* argv[]) { if (checker.evaluateSampleCollectionStatus( size_score, skew_score, pos_score_x, pos_score_y)) { std::cout << ">>> Sample collection status: COMPLETE <<<" - << std::endl; + << std::endl << std::endl; acquisition_completed = true; } diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 2ea7e7c..7d114b4 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -253,39 +253,42 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { // Add other parameters for other cameras... + size_t l_dist_size = left.D.total(); + size_t r_dist_size = right.D.total(); + outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << left.D.at(0) << "\n"; - outfile << "k2 = " << left.D.at(1) << "\n"; - outfile << "p1 = " << left.D.at(2) << "\n"; - outfile << "p2 = " << left.D.at(3) << "\n"; - outfile << "k3 = " << left.D.at(4) << "\n"; - outfile << "k4 = " << left.D.at(5) << "\n"; - outfile << "k5 = " << left.D.at(6) << "\n"; - outfile << "k6 = " << left.D.at(7) << "\n\n"; + outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << right.D.at(0) << "\n"; - outfile << "k2 = " << right.D.at(1) << "\n"; - outfile << "p1 = " << right.D.at(2) << "\n"; - outfile << "p2 = " << right.D.at(3) << "\n"; - outfile << "k3 = " << right.D.at(4) << "\n"; - outfile << "k4 = " << right.D.at(5) << "\n"; - outfile << "k5 = " << right.D.at(6) << "\n"; - outfile << "k6 = " << right.D.at(7) << "\n\n"; + outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; outfile << "[STEREO]\n"; outfile << "Baseline = " << -T.at(0) << "\n"; outfile << "TY = " << T.at(1) << "\n"; outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << R.at(1) << "\n"; - outfile << "CV_SVGA = " << R.at(1) << "\n"; - outfile << "CV_FHD1200 = " << R.at(1) << "\n"; - outfile << "RX_FHD = " << R.at(0) << "\n"; - outfile << "RX_SVGA = " << R.at(0) << "\n"; - outfile << "RX_FHD1200 = " << R.at(0) << "\n"; - outfile << "RZ_FHD = " << R.at(2) << "\n"; - outfile << "RZ_SVGA = " << R.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << R.at(2) << "\n\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_SVGA = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_SVGA = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... @@ -355,25 +358,28 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { // Add other parameters for other cameras... + size_t l_dist_size = left.D.total(); + size_t r_dist_size = right.D.total(); + outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << left.D.at(0) << "\n"; - outfile << "k2 = " << left.D.at(1) << "\n"; - outfile << "p1 = " << left.D.at(2) << "\n"; - outfile << "p2 = " << left.D.at(3) << "\n"; - outfile << "k3 = " << left.D.at(4) << "\n"; - outfile << "k4 = " << left.D.at(5) << "\n"; - outfile << "k5 = " << left.D.at(6) << "\n"; - outfile << "k6 = " << left.D.at(7) << "\n\n"; + outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << right.D.at(0) << "\n"; - outfile << "k2 = " << right.D.at(1) << "\n"; - outfile << "p1 = " << right.D.at(2) << "\n"; - outfile << "p2 = " << right.D.at(3) << "\n"; - outfile << "k3 = " << right.D.at(4) << "\n"; - outfile << "k4 = " << right.D.at(5) << "\n"; - outfile << "k5 = " << right.D.at(6) << "\n"; - outfile << "k6 = " << right.D.at(7) << "\n\n"; + outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; outfile << "[STEREO]\n"; outfile << "Baseline = " << -T.at(0) << "\n"; From 39d913de274b4f094cd89406a3b8435dfd4a007e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 17:22:36 +0100 Subject: [PATCH 22/34] Update stereo_calibration/src/opencv_calibration.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- stereo_calibration/src/opencv_calibration.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 7d114b4..e2b1c01 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -216,28 +216,28 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { if (!is_4k) { // AR0234 outfile << "[LEFT_CAM_FHD1200]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; outfile << "[RIGHT_CAM_FHD1200]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; outfile << "[LEFT_CAM_FHD]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) - 60 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) - 60 << "\n\n"; outfile << "[RIGHT_CAM_FHD]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; outfile << "[LEFT_CAM_SVGA]\n"; outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; From 1c82ed5b3f0dad650f85f8e087c17946ec5dfd75 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 17:24:04 +0100 Subject: [PATCH 23/34] Update stereo_calibration/src/calibration_checker.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- stereo_calibration/src/calibration_checker.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index b1df713..11cc096 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -222,12 +222,13 @@ bool CalibrationChecker::isGoodSample(const DetectedBoardParams& params) { 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(p1.pos.x, p2.pos.x); + 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(p1.pos.y, p2.pos.y); - float size_diff = std::abs(p1.size - p2.size) / std::max(p1.size, p2.size); - float skew_diff = std::abs(p1.skew - p2.skew) / std::max(p1.skew, p2.skew); + 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 From e521ca33310b1ca3c5b6848b371253f981c3613c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 17:41:22 +0100 Subject: [PATCH 24/34] Fixes by Copilot review --- .../include/calibration_checker.hpp | 2 +- .../src/calibration_checker.cpp | 4 +- stereo_calibration/src/main.cpp | 8 +- stereo_calibration/src/opencv_calibration.cpp | 223 +++++++++--------- 4 files changed, 124 insertions(+), 113 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 802a7ec..464d41c 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -61,7 +61,7 @@ class CalibrationChecker { private: // Calculate the parameter of a detected checkerboard - DetectedBoardParams getDetectedBoarParams( + DetectedBoardParams getDetectedBoardParams( const std::vector& corners, cv::Size image_size); // Check if the detected corners are valid diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 11cc096..3be1d36 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -35,7 +35,7 @@ CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, bool CalibrationChecker::testSample(const std::vector& corners, cv::Size image_size) { - DetectedBoardParams params = getDetectedBoarParams(corners, image_size); + DetectedBoardParams params = getDetectedBoardParams(corners, image_size); if (params.size < 0 || params.skew < 0) { return false; // Invalid parameters @@ -151,7 +151,7 @@ std::vector CalibrationChecker::get_outside_corners( return outside_corners; } -DetectedBoardParams CalibrationChecker::getDetectedBoarParams( +DetectedBoardParams CalibrationChecker::getDetectedBoardParams( const std::vector& corners, cv::Size image_size) { DetectedBoardParams params; diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 47b09bd..1ac7ac7 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -322,9 +322,11 @@ int main(int argc, char* argv[]) { // Check if the temp image folder exists and clear it if (fs::exists(image_folder)) { std::uintmax_t n{fs::remove_all(image_folder)}; - std::cout << " * Removed " << n - << " temporary files or directories from previous calibration." - << std::endl; + if(verbose) { + std::cout << " * Removed " << n + << " temporary files or directories from previous calibration." + << std::endl; + } } // Create the temp image folder if (!fs::create_directories(image_folder)) { diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index e2b1c01..6f7b755 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -75,7 +75,7 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, pts_r.push_back(pts_r_); object_points.push_back(pattern_points); } else { - std::cout << "No target detected on image " << i << std::endl; + std::cout << "No target detected on image " << i << std::endl; } } @@ -215,6 +215,14 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { } if (!is_4k) { // AR0234 + + if(imageSize.height!=1200) { + std::cout + << "The resolution for the calibration is not valid\n\nUse HD1200 (1920x1200) for ZED X One GS" + << std::endl; + return std::string(); + } + outfile << "[LEFT_CAM_FHD1200]\n"; outfile << "fx = " << left.K.at(0, 0) << "\n"; outfile << "fy = " << left.K.at(1, 1) << "\n"; @@ -240,16 +248,16 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; outfile << "[LEFT_CAM_SVGA]\n"; - outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; outfile << "[RIGHT_CAM_SVGA]\n"; - outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; // Add other parameters for other cameras... @@ -257,38 +265,38 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { size_t r_dist_size = right.D.total(); outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (l_dist_size > 0 ? left.D.at(0) : 0.0) << "\n"; + outfile << "k2 = " << (l_dist_size > 1 ? left.D.at(1) : 0.0) << "\n"; + outfile << "p1 = " << (l_dist_size > 2 ? left.D.at(2) : 0.0) << "\n"; + outfile << "p2 = " << (l_dist_size > 3 ? left.D.at(3) : 0.0) << "\n"; + outfile << "k3 = " << (l_dist_size > 4 ? left.D.at(4) : 0.0) << "\n"; + outfile << "k4 = " << (l_dist_size > 5 ? left.D.at(5) : 0.0) << "\n"; + outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_SVGA = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_SVGA = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_SVGA = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_SVGA = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... @@ -300,60 +308,68 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { << "'" << std::endl; return calib_filename; } else { // IMX678 + + if (imageSize.height != 2160) { + std::cout << "The resolution for the calibration is not valid\n\nUse " + "4K (3840x2160) for ZED X One 4K" + << std::endl; + return std::string(); + } + outfile << "[LEFT_CAM_4k]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; outfile << "[RIGHT_CAM_4k]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; outfile << "[LEFT_CAM_QHDPLUS]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) - (3840 - 3200) / 2 + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 3200) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) - (2160 - 1800) / 2 + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1800) / 2 << "\n\n"; outfile << "[RIGHT_CAM_QHDPLUS]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) - (3840 - 3200) / 2 + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 3200) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) - (2160 - 1800) / 2 + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1800) / 2 << "\n\n"; outfile << "[LEFT_CAM_FHD]\n"; - outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; outfile << "[RIGHT_CAM_FHD]\n"; - outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; outfile << "[LEFT_CAM_FHD1200]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) - (3840 - 1920) / 2 + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 1920) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) - (2160 - 1200) / 2 + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1200) / 2 << "\n\n"; outfile << "[RIGHT_CAM_FHD1200]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) - (3840 - 1920) / 2 + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 1920) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 << "\n\n"; // Add other parameters for other cameras... @@ -362,41 +378,41 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { size_t r_dist_size = right.D.total(); outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; + outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; + outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; + outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; + outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; + outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; + outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; + outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "CV_4k = " << Rv.at(1) << "\n"; - outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RX_4k = " << Rv.at(0) << "\n"; - outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; - outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; - outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "CV_4k = " << Rv.at(1) << "\n"; + outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RX_4k = " << Rv.at(0) << "\n"; + outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; + outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; + outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... @@ -408,11 +424,4 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { << "'" << std::endl; return calib_filename; } - - std::cout - << "The resolution for the calibration is not valid\n\nUse 4k " - "(3840x2160) for ZED X One 4K and FHD1200 (1920x1200) for ZED X One GS" - << std::endl; - - return std::string(); } \ No newline at end of file From 467e127486722b663f54a4be62d7967d582ac740 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Nov 2025 17:57:42 +0100 Subject: [PATCH 25/34] Data type fix and param tuning --- stereo_calibration/src/main.cpp | 2 +- stereo_calibration/src/opencv_calibration.cpp | 286 ++++++++++-------- 2 files changed, 162 insertions(+), 126 deletions(-) diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 1ac7ac7..b23e6a9 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -46,7 +46,7 @@ const float min_y_coverage = 0.7f; // Checkerboard Y position should cover 70% of the image height const float min_area_range = 0.45f; // Checkerboard area range size should be at // least 0.4 [min_area-max_area] -const float min_skew_range = 0.45f; // Checkerboard skew ange size should be at +const float min_skew_range = 0.4f; // Checkerboard skew ange size should be at // least 0.5 [min_skew-max_skew] // Debug diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 6f7b755..2763786 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -214,50 +214,60 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { return std::string(); } + if (left.K.type() == CV_64F) { + std::cout << " Data type: double" << std::endl; + } else if (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 std::string(); + } + if (!is_4k) { // AR0234 - if(imageSize.height!=1200) { - std::cout - << "The resolution for the calibration is not valid\n\nUse HD1200 (1920x1200) for ZED X One GS" - << std::endl; + if (imageSize.height != 1200) { + std::cout << "The resolution for the calibration is not valid\n\nUse " + "HD1200 (1920x1200) for ZED X One GS" + << std::endl; return std::string(); } outfile << "[LEFT_CAM_FHD1200]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; outfile << "[RIGHT_CAM_FHD1200]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; outfile << "[LEFT_CAM_FHD]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) - 60 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) - 60 << "\n\n"; outfile << "[RIGHT_CAM_FHD]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) - 60 << "\n\n"; outfile << "[LEFT_CAM_SVGA]\n"; - outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; outfile << "[RIGHT_CAM_SVGA]\n"; - outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; // Add other parameters for other cameras... @@ -265,38 +275,54 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { size_t r_dist_size = right.D.total(); outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size > 0 ? left.D.at(0) : 0.0) << "\n"; - outfile << "k2 = " << (l_dist_size > 1 ? left.D.at(1) : 0.0) << "\n"; - outfile << "p1 = " << (l_dist_size > 2 ? left.D.at(2) : 0.0) << "\n"; - outfile << "p2 = " << (l_dist_size > 3 ? left.D.at(3) : 0.0) << "\n"; - outfile << "k3 = " << (l_dist_size > 4 ? left.D.at(4) : 0.0) << "\n"; - outfile << "k4 = " << (l_dist_size > 5 ? left.D.at(5) : 0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (l_dist_size > 0 ? left.D.at(0) : 0.0) + << "\n"; + outfile << "k2 = " << (l_dist_size > 1 ? left.D.at(1) : 0.0) + << "\n"; + outfile << "p1 = " << (l_dist_size > 2 ? left.D.at(2) : 0.0) + << "\n"; + outfile << "p2 = " << (l_dist_size > 3 ? left.D.at(3) : 0.0) + << "\n"; + outfile << "k3 = " << (l_dist_size > 4 ? left.D.at(4) : 0.0) + << "\n"; + outfile << "k4 = " << (l_dist_size > 5 ? left.D.at(5) : 0.0) + << "\n"; + outfile << "k5 = " << (l_dist_size > 6 ? left.D.at(6) : 0.0) + << "\n"; + outfile << "k6 = " << (l_dist_size > 7 ? left.D.at(7) : 0.0) + << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (r_dist_size > 0 ? right.D.at(0) : 0.0) + << "\n"; + outfile << "k2 = " << (r_dist_size > 1 ? right.D.at(1) : 0.0) + << "\n"; + outfile << "p1 = " << (r_dist_size > 2 ? right.D.at(2) : 0.0) + << "\n"; + outfile << "p2 = " << (r_dist_size > 3 ? right.D.at(3) : 0.0) + << "\n"; + outfile << "k3 = " << (r_dist_size > 4 ? right.D.at(4) : 0.0) + << "\n"; + outfile << "k4 = " << (r_dist_size > 5 ? right.D.at(5) : 0.0) + << "\n"; + outfile << "k5 = " << (r_dist_size > 6 ? right.D.at(6) : 0.0) + << "\n"; + outfile << "k6 = " << (r_dist_size > 7 ? right.D.at(7) : 0.0) + << "\n\n"; outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_SVGA = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_SVGA = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_SVGA = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_SVGA = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... @@ -317,59 +343,53 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { } outfile << "[LEFT_CAM_4k]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) << "\n"; - outfile << "cy = " << left.K.at(1, 2) << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) << "\n"; + outfile << "cy = " << left.K.at(1, 2) << "\n\n"; outfile << "[RIGHT_CAM_4k]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) << "\n"; - outfile << "cy = " << right.K.at(1, 2) << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) << "\n"; + outfile << "cy = " << right.K.at(1, 2) << "\n\n"; outfile << "[LEFT_CAM_QHDPLUS]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) - (3840 - 3200) / 2 - << "\n"; - outfile << "cy = " << left.K.at(1, 2) - (2160 - 1800) / 2 - << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 3200) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1800) / 2 << "\n\n"; outfile << "[RIGHT_CAM_QHDPLUS]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) - (3840 - 3200) / 2 - << "\n"; - outfile << "cy = " << right.K.at(1, 2) - (2160 - 1800) / 2 + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 3200) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1800) / 2 << "\n\n"; outfile << "[LEFT_CAM_FHD]\n"; - outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << left.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << left.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; outfile << "[RIGHT_CAM_FHD]\n"; - outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; outfile << "[LEFT_CAM_FHD1200]\n"; - outfile << "fx = " << left.K.at(0, 0) << "\n"; - outfile << "fy = " << left.K.at(1, 1) << "\n"; - outfile << "cx = " << left.K.at(0, 2) - (3840 - 1920) / 2 - << "\n"; - outfile << "cy = " << left.K.at(1, 2) - (2160 - 1200) / 2 - << "\n\n"; + outfile << "fx = " << left.K.at(0, 0) << "\n"; + outfile << "fy = " << left.K.at(1, 1) << "\n"; + outfile << "cx = " << left.K.at(0, 2) - (3840 - 1920) / 2 << "\n"; + outfile << "cy = " << left.K.at(1, 2) - (2160 - 1200) / 2 << "\n\n"; outfile << "[RIGHT_CAM_FHD1200]\n"; - outfile << "fx = " << right.K.at(0, 0) << "\n"; - outfile << "fy = " << right.K.at(1, 1) << "\n"; - outfile << "cx = " << right.K.at(0, 2) - (3840 - 1920) / 2 - << "\n"; - outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 + outfile << "fx = " << right.K.at(0, 0) << "\n"; + outfile << "fy = " << right.K.at(1, 1) << "\n"; + outfile << "cx = " << right.K.at(0, 2) - (3840 - 1920) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 << "\n\n"; // Add other parameters for other cameras... @@ -378,41 +398,57 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { size_t r_dist_size = right.D.total(); outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (l_dist_size > 0 ? left.D.at(0) : 0.0) + << "\n"; + outfile << "k2 = " << (l_dist_size > 1 ? left.D.at(1) : 0.0) + << "\n"; + outfile << "p1 = " << (l_dist_size > 2 ? left.D.at(2) : 0.0) + << "\n"; + outfile << "p2 = " << (l_dist_size > 3 ? left.D.at(3) : 0.0) + << "\n"; + outfile << "k3 = " << (l_dist_size > 4 ? left.D.at(4) : 0.0) + << "\n"; + outfile << "k4 = " << (l_dist_size > 5 ? left.D.at(5) : 0.0) + << "\n"; + outfile << "k5 = " << (l_dist_size > 6 ? left.D.at(6) : 0.0) + << "\n"; + outfile << "k6 = " << (l_dist_size > 7 ? left.D.at(7) : 0.0) + << "\n\n"; outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + outfile << "k1 = " << (r_dist_size > 0 ? right.D.at(0) : 0.0) + << "\n"; + outfile << "k2 = " << (r_dist_size > 1 ? right.D.at(1) : 0.0) + << "\n"; + outfile << "p1 = " << (r_dist_size > 2 ? right.D.at(2) : 0.0) + << "\n"; + outfile << "p2 = " << (r_dist_size > 3 ? right.D.at(3) : 0.0) + << "\n"; + outfile << "k3 = " << (r_dist_size > 4 ? right.D.at(4) : 0.0) + << "\n"; + outfile << "k4 = " << (r_dist_size > 5 ? right.D.at(5) : 0.0) + << "\n"; + outfile << "k5 = " << (r_dist_size > 6 ? right.D.at(6) : 0.0) + << "\n"; + outfile << "k6 = " << (r_dist_size > 7 ? right.D.at(7) : 0.0) + << "\n\n"; outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "CV_4k = " << Rv.at(1) << "\n"; - outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RX_4k = " << Rv.at(0) << "\n"; - outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; - outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; - outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "CV_4k = " << Rv.at(1) << "\n"; + outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RX_4k = " << Rv.at(0) << "\n"; + outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; + outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; + outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; // Add other parameters for other stereo parameters if needed... From 6b0a9436143f9b1815359e1d3710853703f85406 Mon Sep 17 00:00:00 2001 From: pyver Date: Fri, 28 Nov 2025 09:58:04 +0100 Subject: [PATCH 26/34] fix fisheye disto export --- stereo_calibration/src/opencv_calibration.cpp | 76 ++++++------------- 1 file changed, 24 insertions(+), 52 deletions(-) diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 6f7b755..e80bd54 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -202,6 +202,26 @@ std::string StereoCalib::saveCalibOpenCV(int serial) { return std::string(); } +void printDisto(const CameraCalib& calib, std::ofstream &outfile) { + if(calib.disto_model_RadTan) { + size_t dist_size = calib.D.total(); + outfile << "k1 = " << calib.D.at(0) << "\n"; + outfile << "k2 = " << calib.D.at(1) << "\n"; + outfile << "p1 = " << calib.D.at(2) << "\n"; + outfile << "p2 = " << calib.D.at(3) << "\n"; + outfile << "k3 = " << calib.D.at(4) << "\n"; + outfile << "k4 = " << (dist_size > 5 ? calib.D.at(5) : 0.0) << "\n"; + outfile << "k5 = " << (dist_size > 6 ? calib.D.at(6) : 0.0) << "\n"; + outfile << "k6 = " << (dist_size > 7 ? calib.D.at(7) : 0.0) << "\n"; + }else{ + outfile << "k1 = " << calib.D.at(0) << "\n"; + outfile << "k2 = " << calib.D.at(1) << "\n"; + outfile << "k3 = " << calib.D.at(2) << "\n"; + outfile << "k4 = " << calib.D.at(3) << "\n"; + } + outfile<<"\n"; +} + std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { std::string calib_filename = "SN" + std::to_string(serial) + ".conf"; @@ -259,30 +279,11 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; - // Add other parameters for other cameras... - - size_t l_dist_size = left.D.total(); - size_t r_dist_size = right.D.total(); - outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size > 0 ? left.D.at(0) : 0.0) << "\n"; - outfile << "k2 = " << (l_dist_size > 1 ? left.D.at(1) : 0.0) << "\n"; - outfile << "p1 = " << (l_dist_size > 2 ? left.D.at(2) : 0.0) << "\n"; - outfile << "p2 = " << (l_dist_size > 3 ? left.D.at(3) : 0.0) << "\n"; - outfile << "k3 = " << (l_dist_size > 4 ? left.D.at(4) : 0.0) << "\n"; - outfile << "k4 = " << (l_dist_size > 5 ? left.D.at(5) : 0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + printDisto(left, outfile); outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + printDisto(right, outfile); outfile << "[STEREO]\n"; outfile << "Baseline = " << -T.at(0) << "\n"; @@ -298,11 +299,6 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; - // Add other parameters for other stereo parameters if needed... - - outfile << "[MISC]\n"; - outfile << "Sensor_ID = 1\n\n"; - outfile.close(); std::cout << " * Parameter file written successfully: '" << calib_filename << "'" << std::endl; @@ -372,30 +368,11 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "cy = " << right.K.at(1, 2) - (2160 - 1200) / 2 << "\n\n"; - // Add other parameters for other cameras... - - size_t l_dist_size = left.D.total(); - size_t r_dist_size = right.D.total(); - outfile << "[LEFT_DISTO]\n"; - outfile << "k1 = " << (l_dist_size>0?left.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (l_dist_size>1?left.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (l_dist_size>2?left.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (l_dist_size>3?left.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (l_dist_size>4?left.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (l_dist_size>5?left.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (l_dist_size>6?left.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (l_dist_size>7?left.D.at(7):0.0) << "\n\n"; + printDisto(left, outfile); outfile << "[RIGHT_DISTO]\n"; - outfile << "k1 = " << (r_dist_size>0?right.D.at(0):0.0) << "\n"; - outfile << "k2 = " << (r_dist_size>1?right.D.at(1):0.0) << "\n"; - outfile << "p1 = " << (r_dist_size>2?right.D.at(2):0.0) << "\n"; - outfile << "p2 = " << (r_dist_size>3?right.D.at(3):0.0) << "\n"; - outfile << "k3 = " << (r_dist_size>4?right.D.at(4):0.0) << "\n"; - outfile << "k4 = " << (r_dist_size>5?right.D.at(5):0.0) << "\n"; - outfile << "k5 = " << (r_dist_size>6?right.D.at(6):0.0) << "\n"; - outfile << "k6 = " << (r_dist_size>7?right.D.at(7):0.0) << "\n\n"; + printDisto(right, outfile); outfile << "[STEREO]\n"; outfile << "Baseline = " << -T.at(0) << "\n"; @@ -414,11 +391,6 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; - // Add other parameters for other stereo parameters if needed... - - outfile << "[MISC]\n"; - outfile << "Sensor_ID = 2\n\n"; - outfile.close(); std::cout << " * Parameter file written successfully: '" << calib_filename << "'" << std::endl; From c74a81c788daa4704315aafff46960ef1546eefc Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 14:33:43 +0100 Subject: [PATCH 27/34] Add baseline value check --- stereo_calibration/src/opencv_calibration.cpp | 41 ++++++++++++++++++- 1 file changed, 39 insertions(+), 2 deletions(-) diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 3bb3a79..04c4fdb 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -121,8 +121,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << " * Stereo " << err << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; - if (rms_l > 0.5f || rms_r > 0.5f || err > 0.5f) { - std::cout << std::endl + if (rms_l > max_repr_error || rms_r > max_repr_error || err > max_repr_error) { + std::cerr << std::endl << "\t !! Warning !!" << std::endl << "The max reprojection error looks too high (>" << max_repr_error @@ -134,6 +134,43 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, 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::cout << "** Camera parameters **" << std::endl; From 9e88f8df67f66d907bcc57e0f1eae531b13e0a31 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 14:46:08 +0100 Subject: [PATCH 28/34] Add checkerboard params --- .../include/opencv_calibration.hpp | 2 +- stereo_calibration/src/main.cpp | 78 +++++++++++++------ stereo_calibration/src/opencv_calibration.cpp | 8 +- 3 files changed, 58 insertions(+), 30 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index b2f6bf0..1fa78da 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -176,7 +176,7 @@ struct StereoCalib { }; int calibrate(int img_count, const std::string &folder, StereoCalib &raw_data, - int target_w, int target_h, float square_size, int serial, + 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/main.cpp b/stereo_calibration/src/main.cpp index b23e6a9..6e0f987 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -7,13 +7,13 @@ namespace fs = std::filesystem; // ********************************************************************************* -// CHANGE THIS PARAM BASED ON THE CHECKERBOARD USED +// CHANGE THIS PARAMS USING THE COMMAND LINE OPTIONS // Learn more: // * 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 = 25.4; // mm +int h_edges = 9; // number of horizontal inner edges +int v_edges = 6; // number of vertical inner edges +float square_size = 25.4; // mm // Default parameters are good for this checkerboard: // https://github.com/opencv/opencv/blob/4.x/doc/pattern.png/ @@ -44,8 +44,8 @@ const float min_x_coverage = 0.7f; // Checkerboard X position should cover 70% of the image width const float min_y_coverage = 0.7f; // Checkerboard Y position should cover 70% of the image height -const float min_area_range = 0.45f; // Checkerboard area range size should be at - // least 0.4 [min_area-max_area] +const float min_area_range = 0.45f; // Checkerboard area range size should be + // at least 0.4 [min_area-max_area] const float min_skew_range = 0.4f; // Checkerboard skew ange size should be at // least 0.5 [min_skew-max_skew] @@ -103,8 +103,23 @@ struct Args { 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 " @@ -128,12 +143,23 @@ struct Args { std::cout << " --help, -h Show this help message" << std::endl << std::endl; std::cout << "Examples:" << std::endl; - std::cout << "* ZED Stereo Camera using an SVO file:" << 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 << "* Virtual Stereo Camera using camera IDs:" << 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 << "* Virtual Stereo Camera with fisheye lenses using camera " + 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 @@ -152,7 +178,7 @@ int main(int argc, char* argv[]) { const DetectedBoardParams idealParams = { cv::Point2f(min_x_coverage, min_y_coverage), min_area_range, min_skew_range}; - CalibrationChecker checker(cv::Size(target_w, target_h), square_size, + CalibrationChecker checker(cv::Size(h_edges, v_edges), square_size, min_samples, max_samples, idealParams, verbose); // Coverage scores float size_score = 0.0f, skew_score = 0.0f, pos_score_x = 0.0f, @@ -169,7 +195,7 @@ int main(int argc, char* argv[]) { std::cout << "The calibration process requires a checkerboard of known " "characteristics." << std::endl; - std::cout << "Expected checkerboard size: " << target_w << "x" << target_h + std::cout << "Expected checkerboard size: " << h_edges << "x" << v_edges << " - " << square_size << "mm" << std::endl; std::cout << "Change those values in the code depending on the checkerboard " "you are using!" @@ -313,8 +339,8 @@ int main(int argc, char* argv[]) { bool acquisition_completed = false; std::vector pts_obj_; - for (int i = 0; i < target_h; i++) { - for (int j = 0; j < target_w; j++) { + 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)); } } @@ -322,7 +348,7 @@ int main(int argc, char* argv[]) { // 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) { + if (verbose) { std::cout << " * Removed " << n << " temporary files or directories from previous calibration." << std::endl; @@ -367,13 +393,13 @@ int main(int argc, char* argv[]) { bool found_l = false; bool found_r = false; found_l = - cv::findChessboardCorners(rgb_d, cv::Size(target_w, target_h), pts_l); - drawChessboardCorners(rgb_d_fill, cv::Size(target_w, target_h), + 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(target_w, target_h), pts_r); - drawChessboardCorners(rgb2_d, cv::Size(target_w, target_h), + 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); } @@ -456,7 +482,8 @@ int main(int argc, char* argv[]) { (skew_score > 1.0f ? info_color : warn_color), 1); std::stringstream ss_img_count; - ss_img_count << " * Sample saved: " << image_count << " [min. " << min_samples << "]"; + 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, @@ -512,7 +539,8 @@ int main(int argc, char* argv[]) { if (checker.evaluateSampleCollectionStatus( size_score, skew_score, pos_score_x, pos_score_y)) { std::cout << ">>> Sample collection status: COMPLETE <<<" - << std::endl << std::endl; + << std::endl + << std::endl; acquisition_completed = true; } @@ -534,10 +562,10 @@ int main(int argc, char* argv[]) { } // Start the calibration process - int err = calibrate(image_count, image_folder, calib, target_w, target_h, - square_size, zed_info.serial_number, is_dual_mono_camera, - is_4k_camera, false, can_use_calib_prior, max_repr_error, - verbose); + 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 << "CALIBRATION successful" << std::endl; else diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 04c4fdb..a936b06 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,7 +1,7 @@ #include "opencv_calibration.hpp" int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, - int target_w, int target_h, float square_size, int serial, + 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; @@ -41,8 +41,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, // 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++) { + 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)); } @@ -51,7 +51,7 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::vector> object_points; std::vector> pts_l, pts_r; - cv::Size t_size(target_w, target_h); + cv::Size t_size(h_edges, v_edges); for (int i = 0; i < left_images.size(); i++) { std::vector pts_l_, pts_r_; From ce2c0f613efcd003bef1a194448702e4630dbea3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 15:03:59 +0100 Subject: [PATCH 29/34] Improve user log feedback --- .../include/opencv_calibration.hpp | 3 -- stereo_calibration/src/main.cpp | 30 +++++++++++-------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index 1fa78da..a613045 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -132,9 +132,6 @@ struct StereoCalib { 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) - << std::endl; } float calibrate( diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 6e0f987..bd3f2f8 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -191,15 +191,17 @@ int main(int argc, char* argv[]) { 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 size: " << h_edges << "x" << v_edges - << " - " << square_size << "mm" << std::endl; - std::cout << "Change those values in the code depending on the checkerboard " - "you are using!" - << 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; @@ -216,16 +218,17 @@ int main(int argc, char* argv[]) { // 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: " + 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 << "Virtual SN: " << sn_stereo << std::endl; + 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 { @@ -235,7 +238,7 @@ int main(int argc, char* argv[]) { << std::endl; std::cerr << " * use the command " << args.app_name << " -h' for details." << std::endl; - return 1; + return EXIT_FAILURE; } std::cout << "Using camera IDs for left and right cameras: " @@ -259,11 +262,12 @@ int main(int argc, char* argv[]) { std::cerr << " * use the command 'ZED_Explore --all' to get the camera " "ID or the Serial Number of the connected cameras." << std::endl; - return 1; + return EXIT_FAILURE; } int sn_stereo = sl::generateVirtualStereoSerialNumber(sn_left, sn_right); - std::cout << "Virtual Stereo SN: " << sn_stereo << std::endl; + 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); @@ -306,13 +310,15 @@ int main(int argc, char* argv[]) { // 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: " + std::cout << " * Using prior calibration: " << (can_use_calib_prior ? "Yes" : "No") << std::endl; StereoCalib calib; calib.initDefault(args.is_radtan_lens); + std::cout << " * Lens distorsion model: " + << (args.is_radtan_lens ? "Radial-Tangential" : "Fisheye") + << std::endl; auto zed_info = zed_camera.getCameraInformation(); From 9ec055f73aaa6614ec998ce5348986707280b0ac Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 20:24:14 +0100 Subject: [PATCH 30/34] Improve user feedback --- .gitignore | 4 + reprojection_viewer/src/main.cpp | 2 +- stereo_calibration/.vscode/settings.json | 3 - stereo_calibration/src/main.cpp | 23 +- stereo_calibration/src/opencv_calibration.cpp | 340 +++++++++--------- 5 files changed, 195 insertions(+), 177 deletions(-) delete mode 100644 stereo_calibration/.vscode/settings.json 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/reprojection_viewer/src/main.cpp b/reprojection_viewer/src/main.cpp index 769430e..10f9121 100644 --- a/reprojection_viewer/src/main.cpp +++ b/reprojection_viewer/src/main.cpp @@ -208,7 +208,7 @@ int main(int argc, char **argv) { InitParameters init_parameters; init_parameters.depth_mode = DEPTH_MODE::NEURAL; init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed - init_parameters.sdk_verbose = 1; + init_parameters.sdk_verbose = sdk_verbose; init_parameters.maximum_working_resolution = sl::Resolution(0, 0); if (!args.svo_path.empty()) { diff --git a/stereo_calibration/.vscode/settings.json b/stereo_calibration/.vscode/settings.json deleted file mode 100644 index 70e34ec..0000000 --- a/stereo_calibration/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "C_Cpp.errorSquiggles": "disabled" -} \ No newline at end of file diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index bd3f2f8..13c8936 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -39,15 +39,13 @@ const cv::Size display_size(720, 404); // Size of the rendered images /// Calibration condition const float max_repr_error = 0.5; // in pixels const int min_samples = 25; -const int max_samples = 45; +const int max_samples = 40; const float min_x_coverage = - 0.7f; // Checkerboard X position should cover 70% of the image width + 0.7f; // Checkerboard X position covering percentage of the image width const float min_y_coverage = - 0.7f; // Checkerboard Y position should cover 70% of the image height -const float min_area_range = 0.45f; // Checkerboard area range size should be - // at least 0.4 [min_area-max_area] -const float min_skew_range = 0.4f; // Checkerboard skew ange size should be at - // least 0.5 [min_skew-max_skew] + 0.7f; // 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] // Debug bool verbose = false; @@ -199,7 +197,7 @@ int main(int argc, char* argv[]) { 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 << " - 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; @@ -236,7 +234,7 @@ int main(int argc, char* argv[]) { 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 + std::cerr << " * use the command '" << args.app_name << " -h' for details." << std::endl; return EXIT_FAILURE; } @@ -508,8 +506,7 @@ int main(int argc, char* argv[]) { key = cv::waitKey(10); if (acquisition_completed) { - std::cout << "Starting calibration process..." << std::endl - << std::endl; + std::cout << " *** Starting the calibration process ***" << std::endl; break; } @@ -573,9 +570,9 @@ int main(int argc, char* argv[]) { 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 << "CALIBRATION successful" << std::endl; + std::cout << std::endl << " +++++ Calibration successful +++++" << std::endl; else - std::cout << "CALIBRATION failed" << std::endl; + std::cout << std::endl << " ----- Calibration failed -----" << std::endl; zed_camera.close(); diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index a936b06..9a61a34 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -9,7 +9,11 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, /// 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); @@ -21,11 +25,13 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, if (imageSize.width == 0) imageSize = grey_l.size(); else if (imageSize != left_images.back().size()) { - std::cout << "Image number " << i - << " does not have the same size as the previous ones" + 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; - break; + return EXIT_FAILURE; } left_images.push_back(grey_l); @@ -33,11 +39,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, } } - if (verbose) { - std::cout << std::endl - << "\t" << left_images.size() << " samples collected" - << std::endl; - } + std::cout << std::endl + << " * " << left_images.size() << " samples collected" << std::endl; // Define object points of the target std::vector pattern_points; @@ -53,7 +56,10 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, cv::Size t_size(h_edges, v_edges); + 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); @@ -75,12 +81,12 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, pts_r.push_back(pts_r_); object_points.push_back(pattern_points); } else { - std::cout << "No target detected on image " << i << std::endl; + std::cout << std::endl + << "- No valid targets detected on frames #" << i << " -" << std::endl; } } /// Compute calibration - std::cout << std::endl << "*** Calibration Report ***" << std::endl; if (pts_l.size() < MIN_IMAGE) { std::cout << " !!! Not enough images with the target detected !!!" @@ -88,128 +94,142 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << " Please perform a new data acquisition." << std::endl << std::endl; return EXIT_FAILURE; - } else { - std::cout << " * Enough valid samples: " << pts_l.size() << std::endl; + } - auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; - if (verbose) { - std::cout << "Left camera calibration: " << std::endl; - } - auto rms_l = calib_data.left.calibrate(object_points, pts_l, imageSize, - flags, verbose); + std::cout << std::endl << " * Valid samples: " << pts_l.size() << "/" << img_count + << std::endl; - if (verbose) { - std::cout << "Right camera calibration: " << std::endl; - } - auto rms_r = calib_data.right.calibrate(object_points, pts_r, imageSize, - flags, verbose); + auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; - if (verbose) { - std::cout << "Stereo calibration: " << std::endl; - } - auto err = calib_data.calibrate( - object_points, pts_l, pts_r, imageSize, - cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); + std::cout << "Left camera calibration... " << std::flush; + + auto rms_l = calib_data.left.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.calibrate(object_points, pts_r, imageSize, + flags, verbose); + + std::cout << "Done." << std::endl; - std::cout << " * Reprojection errors: " << std::endl; - std::cout << " * Left " << rms_l - << (rms_l > max_repr_error ? " !!! TOO HIGH !!!" : "") + std::cout << "Stereo calibration... " << std::flush; + + auto err = calib_data.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 + << (rms_l > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; + std::cout << " * Right:\t" << rms_r + << (rms_r > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; + std::cout << " * Stereo:\t" << err + << (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 + << "), check that the lenses are clean (sharp images)" + " and that the pattern is printed/mounted on a RIGID " + "and FLAT surface." << std::endl; - std::cout << " * Right " << rms_r - << (rms_r > max_repr_error ? " !!! TOO HIGH !!!" : "") + + 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; - std::cout << " * Stereo " << err - << (err > max_repr_error ? " !!! TOO HIGH !!!" : "") << std::endl; - - if (rms_l > max_repr_error || rms_r > max_repr_error || err > max_repr_error) { - std::cerr << std::endl - << "\t !! Warning !!" << std::endl - << "The max reprojection error looks too high (>" - << max_repr_error - << "), check that the lenses are clean (sharp images)" - " and that the pattern is printed/mounted on a RIGID " - "and FLAT surface." - << std::endl; + return EXIT_FAILURE; + } - 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; - 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; + } - 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; - 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; + } - return EXIT_FAILURE; - } + constexpr float MIN_BASELINE = 30.0f; // Minimum possible baseline in mm - 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; - 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; + } - return EXIT_FAILURE; - } + std::cout << std::endl; + + 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; + + std::cout << std::endl << "*** Save Calibration files ***" << std::endl; + + 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; + } - std::cout << std::endl; - - 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; - - std::cout << std::endl << "*** Save Calibration files ***" << std::endl; - - std::string opencv_file = calib_data.saveCalibOpenCV(serial); - if (!opencv_file.empty()) { - std::cout << " * OpenCV calibration file saved: " << opencv_file + // 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 OpenCV calibration file " << opencv_file + std::cout << " !!! Failed to save ZED SDK calibration file " << zed_file << " !!!" << std::endl; } - - // 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; - } - } } return EXIT_SUCCESS; @@ -239,24 +259,24 @@ std::string StereoCalib::saveCalibOpenCV(int serial) { return std::string(); } -void printDisto(const CameraCalib& calib, std::ofstream &outfile) { - if(calib.disto_model_RadTan) { +void printDisto(const CameraCalib& calib, std::ofstream& outfile) { + if (calib.disto_model_RadTan) { size_t dist_size = calib.D.total(); - outfile << "k1 = " << calib.D.at(0) << "\n"; - outfile << "k2 = " << calib.D.at(1) << "\n"; - outfile << "p1 = " << calib.D.at(2) << "\n"; - outfile << "p2 = " << calib.D.at(3) << "\n"; - outfile << "k3 = " << calib.D.at(4) << "\n"; - outfile << "k4 = " << (dist_size > 5 ? calib.D.at(5) : 0.0) << "\n"; - outfile << "k5 = " << (dist_size > 6 ? calib.D.at(6) : 0.0) << "\n"; - outfile << "k6 = " << (dist_size > 7 ? calib.D.at(7) : 0.0) << "\n"; - }else{ - outfile << "k1 = " << calib.D.at(0) << "\n"; - outfile << "k2 = " << calib.D.at(1) << "\n"; - outfile << "k3 = " << calib.D.at(2) << "\n"; - outfile << "k4 = " << calib.D.at(3) << "\n"; + outfile << "k1 = " << calib.D.at(0) << "\n"; + outfile << "k2 = " << calib.D.at(1) << "\n"; + outfile << "p1 = " << calib.D.at(2) << "\n"; + outfile << "p2 = " << calib.D.at(3) << "\n"; + outfile << "k3 = " << calib.D.at(4) << "\n"; + outfile << "k4 = " << (dist_size > 5 ? calib.D.at(5) : 0.0) << "\n"; + outfile << "k5 = " << (dist_size > 6 ? calib.D.at(6) : 0.0) << "\n"; + outfile << "k6 = " << (dist_size > 7 ? calib.D.at(7) : 0.0) << "\n"; + } else { + outfile << "k1 = " << calib.D.at(0) << "\n"; + outfile << "k2 = " << calib.D.at(1) << "\n"; + outfile << "k3 = " << calib.D.at(2) << "\n"; + outfile << "k4 = " << calib.D.at(3) << "\n"; } - outfile<<"\n"; + outfile << "\n"; } std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { @@ -321,10 +341,10 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "cy = " << left.K.at(1, 2) / 2 << "\n\n"; outfile << "[RIGHT_CAM_SVGA]\n"; - outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; - outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; - outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; - outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; + outfile << "fx = " << right.K.at(0, 0) / 2 << "\n"; + outfile << "fy = " << right.K.at(1, 1) / 2 << "\n"; + outfile << "cx = " << right.K.at(0, 2) / 2 << "\n"; + outfile << "cy = " << right.K.at(1, 2) / 2 << "\n\n"; outfile << "[LEFT_DISTO]\n"; printDisto(left, outfile); @@ -333,18 +353,18 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { printDisto(right, outfile); outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_SVGA = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_SVGA = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_SVGA = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_SVGA = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_SVGA = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; outfile.close(); std::cout << " * Parameter file written successfully: '" << calib_filename @@ -416,21 +436,21 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { printDisto(right, outfile); outfile << "[STEREO]\n"; - outfile << "Baseline = " << -T.at(0) << "\n"; - outfile << "TY = " << T.at(1) << "\n"; - outfile << "TZ = " << T.at(2) << "\n"; - outfile << "CV_FHD = " << Rv.at(1) << "\n"; - outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; - outfile << "CV_4k = " << Rv.at(1) << "\n"; - outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; - outfile << "RX_FHD = " << Rv.at(0) << "\n"; - outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; - outfile << "RX_4k = " << Rv.at(0) << "\n"; - outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; - outfile << "RZ_FHD = " << Rv.at(2) << "\n"; - outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; - outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; - outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; + outfile << "Baseline = " << -T.at(0) << "\n"; + outfile << "TY = " << T.at(1) << "\n"; + outfile << "TZ = " << T.at(2) << "\n"; + outfile << "CV_FHD = " << Rv.at(1) << "\n"; + outfile << "CV_FHD1200 = " << Rv.at(1) << "\n"; + outfile << "CV_4k = " << Rv.at(1) << "\n"; + outfile << "CV_QHDPLUS = " << Rv.at(1) << "\n"; + outfile << "RX_FHD = " << Rv.at(0) << "\n"; + outfile << "RX_FHD1200 = " << Rv.at(0) << "\n"; + outfile << "RX_4k = " << Rv.at(0) << "\n"; + outfile << "RX_QHDPLUS = " << Rv.at(0) << "\n"; + outfile << "RZ_FHD = " << Rv.at(2) << "\n"; + outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n"; + outfile << "RZ_4k = " << Rv.at(2) << "\n\n"; + outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; outfile.close(); std::cout << " * Parameter file written successfully: '" << calib_filename From 7781ce4c31cbce64f2cb169e55c429127a2a6925 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 20:46:21 +0100 Subject: [PATCH 31/34] Improvements --- CMakeLists.txt | 11 ++++++++ reprojection_viewer/CMakeLists.txt | 2 +- reprojection_viewer/src/main.cpp | 2 +- stereo_calibration/CMakeLists.txt | 2 +- stereo_calibration/src/opencv_calibration.cpp | 26 +++++-------------- 5 files changed, 20 insertions(+), 23 deletions(-) create mode 100644 CMakeLists.txt 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/reprojection_viewer/src/main.cpp b/reprojection_viewer/src/main.cpp index 10f9121..769430e 100644 --- a/reprojection_viewer/src/main.cpp +++ b/reprojection_viewer/src/main.cpp @@ -208,7 +208,7 @@ int main(int argc, char **argv) { InitParameters init_parameters; init_parameters.depth_mode = DEPTH_MODE::NEURAL; init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed - init_parameters.sdk_verbose = sdk_verbose; + init_parameters.sdk_verbose = 1; init_parameters.maximum_working_resolution = sl::Resolution(0, 0); if (!args.svo_path.empty()) { 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/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 9a61a34..86677ad 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -125,20 +125,20 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << std::endl << "*** Calibration Report ***" << std::endl; std::cout << " * Reprojection errors: " << std::endl; - std::cout << " * Left:\t" << rms_l + 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 + std::cout << " * Right:\t" << rms_r << " px" << (rms_r > max_repr_error ? "\t!!! TOO HIGH !!!" : "\t-> GOOD") << std::endl; - std::cout << " * Stereo:\t" << err + 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 (>" + << "The max reprojection error looks too high (> " << max_repr_error - << "), check that the lenses are clean (sharp images)" - " and that the pattern is printed/mounted on a RIGID " + << " 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; @@ -291,16 +291,6 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { return std::string(); } - if (left.K.type() == CV_64F) { - std::cout << " Data type: double" << std::endl; - } else if (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 std::string(); - } - if (!is_4k) { // AR0234 if (imageSize.height != 1200) { @@ -367,8 +357,6 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "RZ_FHD1200 = " << Rv.at(2) << "\n\n"; outfile.close(); - std::cout << " * Parameter file written successfully: '" << calib_filename - << "'" << std::endl; return calib_filename; } else { // IMX678 @@ -453,8 +441,6 @@ std::string StereoCalib::saveCalibZED(int serial, bool is_4k) { outfile << "RZ_QHDPLUS = " << Rv.at(2) << "\n\n"; outfile.close(); - std::cout << " * Parameter file written successfully: '" << calib_filename - << "'" << std::endl; return calib_filename; } } \ No newline at end of file From ea5c49153de70c485d11cdd772285ab81814a18a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Dec 2025 21:09:09 +0100 Subject: [PATCH 32/34] Added minimum checkerboard area param --- stereo_calibration/include/calibration_checker.hpp | 4 ++++ stereo_calibration/src/calibration_checker.cpp | 4 +++- stereo_calibration/src/main.cpp | 7 +++++-- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/stereo_calibration/include/calibration_checker.hpp b/stereo_calibration/include/calibration_checker.hpp index 464d41c..ee686d2 100644 --- a/stereo_calibration/include/calibration_checker.hpp +++ b/stereo_calibration/include/calibration_checker.hpp @@ -22,6 +22,7 @@ typedef struct _detected_board_params { // 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 @@ -36,6 +37,7 @@ class CalibrationChecker { 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; @@ -93,6 +95,8 @@ class CalibrationChecker { 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; }; diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 3be1d36..46e0a6b 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -9,6 +9,7 @@ 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; @@ -16,6 +17,7 @@ CalibrationChecker::CalibrationChecker(cv::Size board_size, float square_size, // Calibration parameters min_samples_ = min_samples; max_samples_ = max_samples; + min_target_area_ = min_target_area; idealParams_ = idealParams; // Initialize the board parameters @@ -159,7 +161,7 @@ DetectedBoardParams CalibrationChecker::getDetectedBoardParams( float area = compute_area(outside_corners); float skew = compute_skew(outside_corners); - if (area < 0 || skew < 0) { + if (area < min_target_area_ || skew < 0) { // Return invalid params params.size = -1.0f; params.skew = -1.0f; diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index 13c8936..d50218e 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -47,8 +47,10 @@ const float min_y_coverage = 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 = false; +bool verbose = true; int sdk_verbose = 0; // Text colors @@ -177,7 +179,8 @@ int main(int argc, char* argv[]) { 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, idealParams, verbose); + 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; From f14f7fe2d1a9b096f5deaca04c4184e7fc0b9ed5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 3 Dec 2025 15:01:47 +0100 Subject: [PATCH 33/34] Better similarity check and debug logs --- .../include/opencv_calibration.hpp | 51 +++++-- .../src/calibration_checker.cpp | 131 +++++++++--------- stereo_calibration/src/main.cpp | 11 +- stereo_calibration/src/opencv_calibration.cpp | 11 +- 4 files changed, 117 insertions(+), 87 deletions(-) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index a613045..b594c12 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -72,16 +72,25 @@ struct CameraCalib { return undistorted_points; } - float calibrate(const std::vector> &object_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 + @@ -89,10 +98,11 @@ struct CameraCalib { } if (verbose) { - std::cout << " * Intrinsic matrix K:" << std::endl << K << std::endl; - std::cout << " * Distortion coefficients D:" << std::endl + 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 << " * Re-projection error (RMS): " << rms << std::endl; + std::cout << "[DEBUG][mono_calibrate] * Re-projection error (RMS): " << rms << std::endl; } return rms; @@ -134,7 +144,7 @@ struct StereoCalib { cv::Rodrigues(Rv, R); } - float calibrate( + float stereo_calibrate( const std::vector> &object_points, const std::vector> &image_points_left, const std::vector> &image_points_right, @@ -146,10 +156,20 @@ struct StereoCalib { 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, @@ -159,10 +179,23 @@ struct StereoCalib { cv::Rodrigues(R, Rv); if (verbose) { - std::cout << " * Rotation matrix R:" << std::endl << R << std::endl; - std::cout << " * Rotation vector Rv:" << std::endl << Rv << std::endl; - std::cout << " * Translation vector T:" << std::endl << T << std::endl; - std::cout << " * Re-projection error (RMS): " << rms << std::endl; + 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; diff --git a/stereo_calibration/src/calibration_checker.cpp b/stereo_calibration/src/calibration_checker.cpp index 46e0a6b..b3d2b29 100644 --- a/stereo_calibration/src/calibration_checker.cpp +++ b/stereo_calibration/src/calibration_checker.cpp @@ -168,7 +168,7 @@ DetectedBoardParams CalibrationChecker::getDetectedBoardParams( return params; } - float border = std::sqrt(area); + //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 @@ -182,10 +182,10 @@ DetectedBoardParams CalibrationChecker::getDetectedBoardParams( 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 - 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)); @@ -201,73 +201,76 @@ bool CalibrationChecker::isGoodSample(const DetectedBoardParams& params) { // 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); - // }; - - // 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 existing samples (dist=" << - // dist - // << ")" << std::endl; - // return false; - // } - // } - - // 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 << "Comparing to: Pos(" << p2.pos.x << ", " << p2.pos.y - << "), Size: " << p2.size << ", Skew: " << p2.skew << std::endl; - std::cout << std::setprecision(3) << "PosX diff: " << pos_x_diff * 100.0f - << "%, " - << "PosY diff: " << pos_y_diff * 100.0f << "%, " - << "Size diff: " << size_diff * 100.0f << "%, " - << "Skew diff: " << skew_diff * 100.0f << "%"; - } - - 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 + 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_) { - // Stop at the first similar sample found - if (!is_different(params, stored_params)) { - std::cout << " Rejected: Too similar to an existing sample" << std::endl; + 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( diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index d50218e..a858514 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -41,9 +41,9 @@ 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.7f; // Checkerboard X position covering percentage of the image width + 0.6f; // Checkerboard X position covering percentage of the image width const float min_y_coverage = - 0.7f; // Checkerboard Y position covering percentage of the image height + 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] @@ -356,7 +356,7 @@ int main(int argc, char* argv[]) { if (fs::exists(image_folder)) { std::uintmax_t n{fs::remove_all(image_folder)}; if (verbose) { - std::cout << " * Removed " << n + std::cout << "[DEBUG][main] * Removed " << n << " temporary files or directories from previous calibration." << std::endl; } @@ -447,7 +447,7 @@ int main(int argc, char* argv[]) { if (low_target_variability_on_last_pics) { cv::putText(rendering_image, - " * Target too similar to a previous acquisition.", + " * 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); } @@ -557,8 +557,7 @@ int main(int argc, char* argv[]) { addNewCheckerboardPosition(coverage_indicator, pos_indicator, norm_x, norm_y, norm_size); } else { - std::cout << " ! Sample detected but not valid. Please check the " - "checkerboard position and angle." + std::cout << " ! Sample detected but not valid. Please try again with a new position." << std::endl; low_target_variability_on_last_pics = true; } diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 86677ad..6eb7b44 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -102,21 +102,16 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; std::cout << "Left camera calibration... " << std::flush; - - auto rms_l = calib_data.left.calibrate(object_points, pts_l, imageSize, flags, - verbose); + 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.calibrate(object_points, pts_r, imageSize, - flags, verbose); - + 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.calibrate( + auto err = calib_data.stereo_calibrate( object_points, pts_l, pts_r, imageSize, cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_ZERO_DISPARITY, verbose); From 4cf04c67cc1cac44c22af5c4f9a7d1fd0e36f6f5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 3 Dec 2025 17:52:39 +0100 Subject: [PATCH 34/34] Trying to solve issues with Stereo Calibration --- CMakeLists.txt | 2 +- reprojection_viewer/CMakeLists.txt | 2 +- stereo_calibration/CMakeLists.txt | 2 +- .../include/opencv_calibration.hpp | 6 +- stereo_calibration/src/main.cpp | 677 +++++++++--------- stereo_calibration/src/opencv_calibration.cpp | 59 +- 6 files changed, 403 insertions(+), 345 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index de25784..89d1618 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(zed_opencv_calibration) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_BUILD_TYPE "Release") +#set(CMAKE_BUILD_TYPE "Release") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/reprojection_viewer/CMakeLists.txt b/reprojection_viewer/CMakeLists.txt index faf5455..54b5555 100644 --- a/reprojection_viewer/CMakeLists.txt +++ b/reprojection_viewer/CMakeLists.txt @@ -3,7 +3,7 @@ PROJECT(zed_reprojection_viewer) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) -SET(CMAKE_BUILD_TYPE "Release") +#SET(CMAKE_BUILD_TYPE "Release") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/stereo_calibration/CMakeLists.txt b/stereo_calibration/CMakeLists.txt index 042ebdb..00fa0f1 100644 --- a/stereo_calibration/CMakeLists.txt +++ b/stereo_calibration/CMakeLists.txt @@ -3,7 +3,7 @@ project(zed_stereo_calibration) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_BUILD_TYPE "Release") +#set(CMAKE_BUILD_TYPE "Release") set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/stereo_calibration/include/opencv_calibration.hpp b/stereo_calibration/include/opencv_calibration.hpp index b594c12..05c7f49 100644 --- a/stereo_calibration/include/opencv_calibration.hpp +++ b/stereo_calibration/include/opencv_calibration.hpp @@ -122,9 +122,9 @@ struct StereoCalib { 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); + R = cv::Mat::eye(3, 3, CV_64FC1); + Rv = cv::Mat::zeros(3, 1, CV_64FC1); + T = cv::Mat::zeros(3, 1, CV_64FC1); } void setFrom(const sl::CalibrationParameters &calib_params) { diff --git a/stereo_calibration/src/main.cpp b/stereo_calibration/src/main.cpp index a858514..ff2c15e 100644 --- a/stereo_calibration/src/main.cpp +++ b/stereo_calibration/src/main.cpp @@ -44,10 +44,13 @@ 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_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) +const float min_target_area = 0.1f; // Ignore checkerboards smaller than this + // area (percentage of image area) // Debug bool verbose = true; @@ -76,12 +79,13 @@ struct Args { std::string app_name; std::string svo_path = ""; bool is_radtan_lens = true; - bool is_zed_x_one_virtual_stereo = false; + bool is_zed_x_one_virtual_stereo = true; bool is_zed_sdk_format = false; - int left_camera_id = -1; - int right_camera_id = -1; + int left_camera_id = 0; + int right_camera_id = 1; int left_camera_sn = -1; int right_camera_sn = -1; + bool use_stored_images = false; void parse(int argc, char* argv[]) { app_name = argv[0]; @@ -109,6 +113,8 @@ struct Args { v_edges = std::stoi(argv[++i]); } else if (arg == "--square_size" && i + 1 < argc) { square_size = std::stof(argv[++i]); + } else if (arg == "--use_stored_values") { + use_stored_images = true; } else if (arg == "--help" || arg == "-h") { std::cout << "Usage: " << argv[0] << " [options]" << std::endl; std::cout << " --h_edges Number of horizontal inner " @@ -179,11 +185,8 @@ int main(int argc, char* argv[]) { 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; + min_samples, max_samples, min_target_area, + idealParams, verbose); // Flags bool is_dual_mono_camera = false; @@ -202,368 +205,395 @@ int main(int argc, char* argv[]) { 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; + "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." + // Initialize the stereo calibration data structure + StereoCalib calib; + calib.initDefault(args.is_radtan_lens); + std::cout << " * Lens distorsion model: " + << (args.is_radtan_lens ? "Radial-Tangential" : "Fisheye") + << std::endl; + + // Status variables + int image_count = -1; + bool can_use_calib_prior = false; + sl::CameraInformation zed_info; + + if (!args.use_stored_images) { + // Coverage scores + float size_score = 0.0f, skew_score = 0.0f, pos_score_x = 0.0f, + pos_score_y = 0.0f; + + // ZED Camera initialization + 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; - 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; + auto cams = sl::CameraOne::getDeviceList(); - 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; + } + } - 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); } - 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." + int left_model = sn_left / 10000000; + int right_model = sn_right / 10000000; + + if (left_model != right_model) { + std::cerr << "Error: Left and Right cameras must be of the same model." << 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); + 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; + } } - int left_model = sn_left / 10000000; - int right_model = sn_right / 10000000; + auto status = zed_camera.open(init_params); - if (left_model != right_model) { - std::cerr << "Error: Left and Right cameras must be of the same model." + // 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 EXIT_FAILURE; + return 1; } - 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); - - // 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; - } - - // 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; + // change can_use_calib_prior if you dont want to use the calibration file + can_use_calib_prior = + status != sl::ERROR_CODE::INVALID_CALIBRATION_FILE; - std::cout << " * Using prior calibration: " - << (can_use_calib_prior ? "Yes" : "No") << std::endl; + std::cout << " * Using prior calibration: " + << (can_use_calib_prior ? "Yes" : "No") << std::endl; - StereoCalib calib; - calib.initDefault(args.is_radtan_lens); - std::cout << " * Lens distorsion model: " - << (args.is_radtan_lens ? "Radial-Tangential" : "Fisheye") - << std::endl; + zed_info = zed_camera.getCameraInformation(); - auto zed_info = zed_camera.getCameraInformation(); + if (can_use_calib_prior) + calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); - if (can_use_calib_prior) - calib.setFrom(zed_info.camera_configuration.calibration_parameters_raw); + sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; - sl::Resolution camera_resolution = zed_info.camera_configuration.resolution; + 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()); - 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()); + 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()); - 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()); + cv::Mat coverage_indicator = + cv::Mat::zeros(display_size.height, display_size.width, CV_8UC1); - 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 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; - 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)); + 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(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; + // 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(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; + // Create the temp image folder + if (!fs::create_directories(image_folder)) { + std::cerr << "Error creating storage folder!"; + return 1; } - 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); - } - - if (image_stack_horizontal) { - cv::hconcat(rgb_d_fill, rgb2_d, display); - } else { - cv::vconcat(rgb_d_fill, rgb2_d, display); + char key = ' '; + // 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; } - 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); + 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); } - 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); + if (image_stack_horizontal) { + cv::hconcat(rgb_d_fill, rgb2_d, display); + } else { + cv::vconcat(rgb_d_fill, rgb2_d, display); } - if (low_target_variability_on_last_pics) { + 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, - " * 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); - } + "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); + } - 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); - } + 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); + } - cv::imshow(window_name, rendering_image); - key = cv::waitKey(10); + 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); + } - if (acquisition_completed) { - std::cout << " *** Starting the calibration process ***" << std::endl; - break; - } + 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); + } - if ((key == 's' || key == 'S') || key == ' ') { - std::cout << "************************************************" - << std::endl; + cv::imshow(window_name, rendering_image); + key = cv::waitKey(10); + + if (acquisition_completed) { + std::cout << " *** Starting the calibration process ***" << std::endl; + break; + } - 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 + 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; + } + + // 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; - acquisition_completed = true; + low_target_variability_on_last_pics = true; } - - // 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; } } } } + + // Stop camera acquisition + zed_camera.close(); } // Start the calibration process @@ -572,12 +602,11 @@ int main(int argc, char* argv[]) { 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; + std::cout << std::endl + << " +++++ Calibration successful +++++" << std::endl; else std::cout << std::endl << " ----- Calibration failed -----" << std::endl; - zed_camera.close(); - return EXIT_SUCCESS; } diff --git a/stereo_calibration/src/opencv_calibration.cpp b/stereo_calibration/src/opencv_calibration.cpp index 6eb7b44..180ff4f 100644 --- a/stereo_calibration/src/opencv_calibration.cpp +++ b/stereo_calibration/src/opencv_calibration.cpp @@ -1,5 +1,7 @@ #include "opencv_calibration.hpp" +#include + 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, @@ -12,6 +14,21 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, std::cout << std::endl << "Loading the stored images from folder: " << folder << std::endl; + // Count images in the folder + if (img_count==-1) { + std::cout << " * Counting images in the folder..." << std::endl; + int actual_img_count = 0; + for (int i = 0; i < img_count; i++) { + std::string left_path = folder + "image_left_" + std::to_string(i) + ".png"; + std::string right_path = folder + "image_right_" + std::to_string(i) + ".png"; + if (std::filesystem::exists(left_path) && std::filesystem::exists(right_path)) { + actual_img_count++; + } + } + img_count = actual_img_count; + std::cout << " - Found " << img_count << " images." << std::endl; + } + for (int i = 0; i < img_count; i++) { std::cout << "." << std::flush; cv::Mat grey_l = @@ -82,7 +99,8 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, object_points.push_back(pattern_points); } else { std::cout << std::endl - << "- No valid targets detected on frames #" << i << " -" << std::endl; + << "- No valid targets detected on frames #" << i << " -" + << std::endl; } } @@ -96,17 +114,25 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, return EXIT_FAILURE; } - std::cout << std::endl << " * Valid samples: " << pts_l.size() << "/" << img_count + std::cout << std::endl + << " * Valid samples: " << pts_l.size() << "/" << img_count << std::endl; auto flags = use_intrinsic_prior ? cv::CALIB_USE_INTRINSIC_GUESS : 0; + if (use_intrinsic_prior) { + std::cout + << "[DEBUG][calibrate] Using intrinsic parameters as calibration prior." + << std::endl; + } std::cout << "Left camera calibration... " << std::flush; - auto rms_l = calib_data.left.mono_calibrate(object_points, pts_l, imageSize, flags, verbose); + 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); + 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; @@ -121,21 +147,24 @@ int calibrate(int img_count, const std::string& folder, StereoCalib& calib_data, 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; + << (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; + << (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; + << (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; + 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; }