119 lines
4.0 KiB
C++
119 lines
4.0 KiB
C++
#include <fmt/core.h>
|
||
#include <yaml-cpp/yaml.h>
|
||
|
||
#include <fstream>
|
||
#include <opencv2/opencv.hpp>
|
||
|
||
#include "calibration/board_pattern.hpp"
|
||
#include "src/component/img_tools.hpp"
|
||
|
||
const std::string keys =
|
||
"{help h usage ? | | 输出命令行参数说明}"
|
||
"{config-path c | configs/calibration.yaml | yaml配置文件路径 }"
|
||
"{@input-folder | assets/img_with_q | 输入文件夹路径 }";
|
||
|
||
void load(
|
||
const std::string & input_folder, const std::string & config_path, cv::Size & img_size,
|
||
std::vector<std::vector<cv::Point3f>> & obj_points,
|
||
std::vector<std::vector<cv::Point2f>> & img_points)
|
||
{
|
||
// 读取yaml参数
|
||
auto yaml = YAML::LoadFile(config_path);
|
||
auto board_pattern = calibration::load_board_pattern(yaml);
|
||
|
||
for (int i = 1; true; i++) {
|
||
// 读取图片
|
||
auto img_path = fmt::format("{}/{}.jpg", input_folder, i);
|
||
auto img = cv::imread(img_path);
|
||
if (img.empty()) break;
|
||
|
||
// 设置图片尺寸
|
||
img_size = img.size();
|
||
|
||
// 识别标定板
|
||
std::vector<cv::Point2f> centers_2d;
|
||
auto success = calibration::find_pattern_points(img, board_pattern, centers_2d);
|
||
|
||
// 显示识别结果
|
||
auto drawing = img.clone();
|
||
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);
|
||
|
||
// 输出识别结果
|
||
fmt::print("[{}] {}\n", success ? "success" : "failure", img_path);
|
||
if (!success) continue;
|
||
|
||
// 记录所需的数据
|
||
img_points.emplace_back(centers_2d);
|
||
obj_points.emplace_back(
|
||
calibration::planar_points(board_pattern.pattern_size, board_pattern.center_distance_mm));
|
||
}
|
||
}
|
||
|
||
void print_yaml(const cv::Mat & camera_matrix, const cv::Mat & distort_coeffs, double error)
|
||
{
|
||
YAML::Emitter result;
|
||
std::vector<double> camera_matrix_data(
|
||
camera_matrix.begin<double>(), camera_matrix.end<double>());
|
||
std::vector<double> distort_coeffs_data(
|
||
distort_coeffs.begin<double>(), distort_coeffs.end<double>());
|
||
|
||
result << YAML::BeginMap;
|
||
result << YAML::Comment(fmt::format("重投影误差: {:.4f}px", error));
|
||
result << YAML::Key << "camera_matrix";
|
||
result << YAML::Value << YAML::Flow << camera_matrix_data;
|
||
result << YAML::Key << "distort_coeffs";
|
||
result << YAML::Value << YAML::Flow << distort_coeffs_data;
|
||
result << YAML::Newline;
|
||
result << YAML::EndMap;
|
||
|
||
fmt::print("\n{}\n", result.c_str());
|
||
}
|
||
|
||
int main(int argc, char * argv[])
|
||
{
|
||
// 读取命令行参数
|
||
cv::CommandLineParser cli(argc, argv, keys);
|
||
if (cli.has("help")) {
|
||
cli.printMessage();
|
||
return 0;
|
||
}
|
||
auto input_folder = cli.get<std::string>(0);
|
||
auto config_path = cli.get<std::string>("config-path");
|
||
|
||
// 从输入文件夹中加载标定所需的数据
|
||
cv::Size img_size;
|
||
std::vector<std::vector<cv::Point3f>> obj_points;
|
||
std::vector<std::vector<cv::Point2f>> img_points;
|
||
load(input_folder, config_path, img_size, obj_points, img_points);
|
||
|
||
// 相机标定
|
||
cv::Mat camera_matrix, distort_coeffs;
|
||
std::vector<cv::Mat> rvecs, tvecs;
|
||
auto criteria = cv::TermCriteria(
|
||
cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 100,
|
||
DBL_EPSILON); // 默认迭代次数(30)有时会导致结果发散，故设为100
|
||
cv::calibrateCamera(
|
||
obj_points, img_points, img_size, camera_matrix, distort_coeffs, rvecs, tvecs, cv::CALIB_FIX_K3,
|
||
criteria); // 由于视场角较小，不需要考虑k3
|
||
|
||
// 重投影误差
|
||
double error_sum = 0;
|
||
size_t total_points = 0;
|
||
for (size_t i = 0; i < obj_points.size(); i++) {
|
||
std::vector<cv::Point2f> reprojected_points;
|
||
cv::projectPoints(
|
||
obj_points[i], rvecs[i], tvecs[i], camera_matrix, distort_coeffs, reprojected_points);
|
||
|
||
total_points += reprojected_points.size();
|
||
for (size_t j = 0; j < reprojected_points.size(); j++)
|
||
error_sum += cv::norm(img_points[i][j] - reprojected_points[j]);
|
||
}
|
||
auto error = error_sum / total_points;
|
||
|
||
// 输出yaml
|
||
print_yaml(camera_matrix, distort_coeffs, error);
|
||
}
|