diff --git a/calibration/board_pattern.hpp b/calibration/board_pattern.hpp new file mode 100644 index 0000000..dfcd681 --- /dev/null +++ b/calibration/board_pattern.hpp @@ -0,0 +1,108 @@ +#ifndef CALIBRATION__BOARD_PATTERN_HPP +#define CALIBRATION__BOARD_PATTERN_HPP + +#include +#include +#include +#include +#include + +#include +#include + +namespace calibration +{ +enum class PatternType +{ + circles_grid, + chessboard +}; + +struct BoardPattern +{ + cv::Size pattern_size; + double center_distance_mm; + PatternType pattern_type; +}; + +inline std::string normalize(std::string text) +{ + std::transform( + text.begin(), text.end(), text.begin(), + [](unsigned char c) { return static_cast(std::tolower(c)); }); + return text; +} + +inline PatternType parse_pattern_type(const YAML::Node & yaml) +{ + if (!yaml["pattern_type"]) return PatternType::circles_grid; + + auto raw = normalize(yaml["pattern_type"].as()); + if (raw == "circles_grid" || raw == "circle_grid" || raw == "circles") { + return PatternType::circles_grid; + } + if (raw == "chessboard" || raw == "checkerboard") { + return PatternType::chessboard; + } + + throw std::runtime_error( + "Unsupported pattern_type: " + raw + + ". Supported values: circles_grid, chessboard."); +} + +inline std::string pattern_name(PatternType pattern_type) +{ + return pattern_type == PatternType::chessboard ? "chessboard" : "circles_grid"; +} + +inline BoardPattern load_board_pattern(const YAML::Node & yaml) +{ + BoardPattern board_pattern; + board_pattern.pattern_size = cv::Size(yaml["pattern_cols"].as(), yaml["pattern_rows"].as()); + board_pattern.center_distance_mm = yaml["center_distance_mm"].as(); + board_pattern.pattern_type = parse_pattern_type(yaml); + return board_pattern; +} + +inline bool find_pattern_points( + const cv::Mat & img, const BoardPattern & board_pattern, std::vector & points) +{ + if (board_pattern.pattern_type == PatternType::circles_grid) { + return cv::findCirclesGrid( + img, board_pattern.pattern_size, points, cv::CALIB_CB_SYMMETRIC_GRID); + } + + cv::Mat gray; + if (img.channels() == 1) + gray = img; + else + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + + auto flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE; + auto success = cv::findChessboardCorners(gray, board_pattern.pattern_size, points, flags); + if (!success) return false; + + cv::cornerSubPix( + gray, points, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3)); + return true; +} + +inline std::vector planar_points( + const cv::Size & pattern_size, const double center_distance_mm) +{ + std::vector points; + points.reserve(static_cast(pattern_size.width * pattern_size.height)); + + for (int i = 0; i < pattern_size.height; i++) { + for (int j = 0; j < pattern_size.width; j++) { + points.push_back({static_cast(j * center_distance_mm), static_cast(i * center_distance_mm), 0.0f}); + } + } + + return points; +} + +} // namespace calibration + +#endif // CALIBRATION__BOARD_PATTERN_HPP diff --git a/calibration/calibrate_camera.cpp b/calibration/calibrate_camera.cpp index 9f4c055..54f0d49 100644 --- a/calibration/calibrate_camera.cpp +++ b/calibration/calibrate_camera.cpp @@ -4,6 +4,7 @@ #include #include +#include "calibration/board_pattern.hpp" #include "src/component/img_tools.hpp" const std::string keys = @@ -11,17 +12,6 @@ const std::string keys = "{config-path c | configs/calibration.yaml | yaml配置文件路径 }" "{@input-folder | assets/img_with_q | 输入文件夹路径 }"; -std::vector centers_3d(const cv::Size & pattern_size, const float center_distance) -{ - std::vector centers_3d; - - for (int i = 0; i < pattern_size.height; i++) - for (int j = 0; j < pattern_size.width; j++) - centers_3d.push_back({j * center_distance, i * center_distance, 0}); - - return centers_3d; -} - void load( const std::string & input_folder, const std::string & config_path, cv::Size & img_size, std::vector> & obj_points, @@ -29,10 +19,7 @@ void load( { // 读取yaml参数 auto yaml = YAML::LoadFile(config_path); - auto pattern_cols = yaml["pattern_cols"].as(); - auto pattern_rows = yaml["pattern_rows"].as(); - auto center_distance_mm = yaml["center_distance_mm"].as(); - cv::Size pattern_size(pattern_cols, pattern_rows); + auto board_pattern = calibration::load_board_pattern(yaml); for (int i = 1; true; i++) { // 读取图片 @@ -45,11 +32,11 @@ void load( // 识别标定板 std::vector centers_2d; - auto success = cv::findCirclesGrid(img, pattern_size, centers_2d, cv::CALIB_CB_SYMMETRIC_GRID); + auto success = calibration::find_pattern_points(img, board_pattern, centers_2d); // 显示识别结果 auto drawing = img.clone(); - cv::drawChessboardCorners(drawing, pattern_size, centers_2d, success); + cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success); cv::resize(drawing, drawing, {}, 0.5, 0.5); // 缩小图片尺寸便于显示完全 cv::imshow("Press any to continue", drawing); cv::waitKey(0); @@ -60,7 +47,8 @@ void load( // 记录所需的数据 img_points.emplace_back(centers_2d); - obj_points.emplace_back(centers_3d(pattern_size, center_distance_mm)); + obj_points.emplace_back( + calibration::planar_points(board_pattern.pattern_size, board_pattern.center_distance_mm)); } } diff --git a/calibration/calibrate_handeye.cpp b/calibration/calibrate_handeye.cpp index ec88b9b..f622d02 100644 --- a/calibration/calibrate_handeye.cpp +++ b/calibration/calibrate_handeye.cpp @@ -6,6 +6,7 @@ #include #include +#include "calibration/board_pattern.hpp" #include "src/component/img_tools.hpp" #include "src/component/math_tools.hpp" @@ -14,17 +15,6 @@ const std::string keys = "{config-path c | configs/calibration.yaml | yaml配置文件路径 }" "{@input-folder | assets/img_with_q | 输入文件夹路径 }"; -std::vector centers_3d(const cv::Size & pattern_size, const float center_distance) -{ - std::vector centers_3d; - - for (int i = 0; i < pattern_size.height; i++) - for (int j = 0; j < pattern_size.width; j++) - centers_3d.push_back({j * center_distance, i * center_distance, 0}); - - return centers_3d; -} - Eigen::Quaterniond read_q(const std::string & q_path) { std::ifstream q_file(q_path); @@ -41,14 +31,11 @@ void load( { // 读取yaml参数 auto yaml = YAML::LoadFile(config_path); - auto pattern_cols = yaml["pattern_cols"].as(); - auto pattern_rows = yaml["pattern_rows"].as(); - auto center_distance_mm = yaml["center_distance_mm"].as(); + auto board_pattern = calibration::load_board_pattern(yaml); R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as>(); auto camera_matrix_data = yaml["camera_matrix"].as>(); auto distort_coeffs_data = yaml["distort_coeffs"].as>(); - cv::Size pattern_size(pattern_cols, pattern_rows); Eigen::Matrix R_gimbal2imubody(R_gimbal2imubody_data.data()); cv::Matx33d camera_matrix(camera_matrix_data.data()); cv::Mat distort_coeffs(distort_coeffs_data); @@ -75,10 +62,10 @@ void load( // 识别标定板 std::vector centers_2d; - auto success = cv::findCirclesGrid(img, pattern_size, centers_2d); // 默认是对称圆点图案 + auto success = calibration::find_pattern_points(img, board_pattern, centers_2d); // 显示识别结果 - cv::drawChessboardCorners(drawing, pattern_size, centers_2d, success); + cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success); cv::resize(drawing, drawing, {}, 0.5, 0.5); // 显示时缩小图片尺寸 cv::imshow("Press any to continue", drawing); cv::waitKey(0); @@ -92,7 +79,8 @@ void load( cv::Mat R_gimbal2world_cv; cv::eigen2cv(R_gimbal2world, R_gimbal2world_cv); cv::Mat rvec, tvec; - auto centers_3d_ = centers_3d(pattern_size, center_distance_mm); + auto centers_3d_ = + calibration::planar_points(board_pattern.pattern_size, board_pattern.center_distance_mm); cv::solvePnP( centers_3d_, centers_2d, camera_matrix, distort_coeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE); diff --git a/calibration/calibrate_robotworld_handeye.cpp b/calibration/calibrate_robotworld_handeye.cpp index 7c60e4c..c75b61e 100644 --- a/calibration/calibrate_robotworld_handeye.cpp +++ b/calibration/calibrate_robotworld_handeye.cpp @@ -6,6 +6,7 @@ #include #include +#include "calibration/board_pattern.hpp" #include "src/component/img_tools.hpp" #include "src/component/math_tools.hpp" @@ -46,14 +47,11 @@ void load( { // 读取yaml参数 auto yaml = YAML::LoadFile(config_path); - auto pattern_cols = yaml["pattern_cols"].as(); - auto pattern_rows = yaml["pattern_rows"].as(); - auto center_distance_mm = yaml["center_distance_mm"].as(); + auto board_pattern = calibration::load_board_pattern(yaml); R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as>(); auto camera_matrix_data = yaml["camera_matrix"].as>(); auto distort_coeffs_data = yaml["distort_coeffs"].as>(); - cv::Size pattern_size(pattern_cols, pattern_rows); Eigen::Matrix R_gimbal2imubody(R_gimbal2imubody_data.data()); cv::Matx33d camera_matrix(camera_matrix_data.data()); cv::Mat distort_coeffs(distort_coeffs_data); @@ -80,10 +78,10 @@ void load( // 识别标定板 std::vector centers_2d; - auto success = cv::findCirclesGrid(img, pattern_size, centers_2d); // 默认是对称圆点图案 + auto success = calibration::find_pattern_points(img, board_pattern, centers_2d); // 显示识别结果 - cv::drawChessboardCorners(drawing, pattern_size, centers_2d, success); + cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success); cv::resize(drawing, drawing, {}, 0.5, 0.5); // 显示时缩小图片尺寸 cv::imshow("Press any to continue", drawing); cv::waitKey(0); @@ -98,7 +96,8 @@ void load( cv::Mat R_world2gimbal_cv; cv::eigen2cv(R_world2gimbal, R_world2gimbal_cv); cv::Mat rvec, tvec; - auto centers_3d_ = centers_3d(pattern_size, center_distance_mm); + auto centers_3d_ = + centers_3d(board_pattern.pattern_size, board_pattern.center_distance_mm); cv::solvePnP( centers_3d_, centers_2d, camera_matrix, distort_coeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE); diff --git a/calibration/capture.cpp b/calibration/capture.cpp index 8f02b05..9009b42 100644 --- a/calibration/capture.cpp +++ b/calibration/capture.cpp @@ -4,11 +4,13 @@ #include #include +#include "calibration/board_pattern.hpp" #include "src/device/camera.hpp" #include "src/device/cboard.hpp" #include "src/component/img_tools.hpp" #include "src/component/logger.hpp" #include "src/component/math_tools.hpp" +#include "src/component/yaml.hpp" const std::string keys = "{help h usage ? | | 输出命令行参数说明}" @@ -25,7 +27,8 @@ void write_q(const std::string q_path, const Eigen::Quaterniond & q) } void capture_loop( - const std::string & config_path, const std::string & can, const std::string & output_folder) + const std::string & config_path, const std::string & output_folder, + const calibration::BoardPattern & board_pattern) { device::CBoard cboard(config_path); device::Camera camera(config_path); @@ -45,8 +48,8 @@ void capture_loop( component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255}); std::vector centers_2d; - auto success = cv::findCirclesGrid(img, cv::Size(10, 7), centers_2d); // 默认是对称圆点图案 - cv::drawChessboardCorners(img_with_ypr, cv::Size(10, 7), centers_2d, success); // 显示识别结果 + auto success = calibration::find_pattern_points(img, board_pattern, centers_2d); + cv::drawChessboardCorners(img_with_ypr, board_pattern.pattern_size, centers_2d, success); cv::resize(img_with_ypr, img_with_ypr, {}, 0.5, 0.5); // 显示时缩小图片尺寸 // 按“s”保存图片和对应四元数，按“q”退出程序 @@ -79,13 +82,17 @@ int main(int argc, char * argv[]) } auto config_path = cli.get(0); auto output_folder = cli.get("output-folder"); + auto yaml = component::load(config_path); + auto board_pattern = calibration::load_board_pattern(yaml); // 新建输出文件夹 std::filesystem::create_directory(output_folder); - component::logger()->info("默认标定板尺寸为10列7行"); + component::logger()->info( + "标定板类型: {}, 尺寸: {}x{}", calibration::pattern_name(board_pattern.pattern_type), + board_pattern.pattern_size.width, board_pattern.pattern_size.height); // 主循环，保存图片和对应四元数 - capture_loop(config_path, "can0", output_folder); + capture_loop(config_path, output_folder, board_pattern); component::logger()->warn("注意四元数输出顺序为wxyz"); diff --git a/configs/calibration.yaml b/configs/calibration.yaml index b080281..fa6f79d 100644 --- a/configs/calibration.yaml +++ b/configs/calibration.yaml @@ -1,6 +1,7 @@ pattern_cols: 10 pattern_rows: 7 center_distance_mm: 40 +pattern_type: circles_grid # circles_grid | chessboard R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1] @@ -13,4 +14,4 @@ vid_pid: "2bdf:0001" quaternion_canid: 0x01 bullet_speed_canid: 0x110 send_canid: 0xff -can_interface: "can0" \ No newline at end of file +can_interface: "can0"