robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(calibration): support configurable board size and chessboard

Browse Source
This commit is contained in:
Robofish 2026-03-01 04:54:54 +08:00
parent 9b0701fe6b
commit 5f67ef69e4
6 changed files with 140 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
calibration/board_pattern.hpp Normal file
View File

@ -0,0 +1,108 @@
#ifndef CALIBRATION__BOARD_PATTERN_HPP
#define CALIBRATION__BOARD_PATTERN_HPP
#include <algorithm>
#include <cctype>
#include <stdexcept>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>
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<char>(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<std::string>());
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<int>(), yaml["pattern_rows"].as<int>());
board_pattern.center_distance_mm = yaml["center_distance_mm"].as<double>();
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<cv::Point2f> & 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<cv::Point3f> planar_points(
const cv::Size & pattern_size, const double center_distance_mm)
{
std::vector<cv::Point3f> points;
points.reserve(static_cast<size_t>(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<float>(j * center_distance_mm), static_cast<float>(i * center_distance_mm), 0.0f});
}
}
return points;
}
} // namespace calibration
#endif // CALIBRATION__BOARD_PATTERN_HPP

24
calibration/calibrate_camera.cpp
View File

@ -4,6 +4,7 @@
#include <fstream>
#include <opencv2/opencv.hpp>
#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<cv::Point3f> centers_3d(const cv::Size & pattern_size, const float center_distance)
{
std::vector<cv::Point3f> 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<std::vector<cv::Point3f>> & obj_points,
@ -29,10 +19,7 @@ void load(
{
// 读取yaml参数
auto yaml = YAML::LoadFile(config_path);
auto pattern_cols = yaml["pattern_cols"].as<int>();
auto pattern_rows = yaml["pattern_rows"].as<int>();
auto center_distance_mm = yaml["center_distance_mm"].as<double>();
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<cv::Point2f> 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));
}
}

24
calibration/calibrate_handeye.cpp
View File

@ -6,6 +6,7 @@
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#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<cv::Point3f> centers_3d(const cv::Size & pattern_size, const float center_distance)
{
std::vector<cv::Point3f> 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<int>();
auto pattern_rows = yaml["pattern_rows"].as<int>();
auto center_distance_mm = yaml["center_distance_mm"].as<double>();
auto board_pattern = calibration::load_board_pattern(yaml);
R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
cv::Size pattern_size(pattern_cols, pattern_rows);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> 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<cv::Point2f> 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);

13
calibration/calibrate_robotworld_handeye.cpp
View File

@ -6,6 +6,7 @@
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#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<int>();
auto pattern_rows = yaml["pattern_rows"].as<int>();
auto center_distance_mm = yaml["center_distance_mm"].as<double>();
auto board_pattern = calibration::load_board_pattern(yaml);
R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
cv::Size pattern_size(pattern_cols, pattern_rows);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> 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<cv::Point2f> 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);

17
calibration/capture.cpp
View File

@ -4,11 +4,13 @@
#include <fstream>
#include <opencv2/opencv.hpp>
#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<cv::Point2f> 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<std::string>(0);
auto output_folder = cli.get<std::string>("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");

1
configs/calibration.yaml
View File

@ -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]