From 16dfed6627a21261f628bc4a003939655c97f37b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 3 Mar 2026 13:25:14 +0800 Subject: [PATCH] =?UTF-8?q?add=20=E6=A3=8B=E7=9B=98=E6=A0=BC=E9=AA=8C?= =?UTF-8?q?=E8=AF=81=E9=80=9A=E8=BF=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- calibration/board_pattern.hpp | 2 +- calibration/capture.cpp | 51 ++++-- calibration/test/camera_calibration.py | 184 ++++++++++++++++++++ calibration/test/chessboard_detector.py | 221 ++++++++++++++++++++++++ calibration/test/visualize_video.py | 127 ++++++++++++++ configs/calibration.yaml | 4 + 6 files changed, 575 insertions(+), 14 deletions(-) create mode 100644 calibration/test/camera_calibration.py create mode 100644 calibration/test/chessboard_detector.py create mode 100644 calibration/test/visualize_video.py diff --git a/calibration/board_pattern.hpp b/calibration/board_pattern.hpp index dfcd681..cb6284b 100644 --- a/calibration/board_pattern.hpp +++ b/calibration/board_pattern.hpp @@ -78,7 +78,7 @@ inline bool find_pattern_points( else cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); - auto flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE; + auto flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK; auto success = cv::findChessboardCorners(gray, board_pattern.pattern_size, points, flags); if (!success) return false; diff --git a/calibration/capture.cpp b/calibration/capture.cpp index 9009b42..5d8dd92 100644 --- a/calibration/capture.cpp +++ b/calibration/capture.cpp @@ -1,16 +1,16 @@ #include +#include #include #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" +#include "src/device/camera.hpp" +#include "src/device/gimbal/gimbal.hpp" const std::string keys = "{help h usage ? | | 输出命令行参数说明}" @@ -27,10 +27,13 @@ void write_q(const std::string q_path, const Eigen::Quaterniond & q) } void capture_loop( - const std::string & config_path, const std::string & output_folder, - const calibration::BoardPattern & board_pattern) + const std::string & config_path, const std::string & output_folder) { - device::CBoard cboard(config_path); + // 从配置文件加载标定板参数（支持 circles_grid 和 chessboard） + auto yaml = YAML::LoadFile(config_path); + auto board_pattern = calibration::load_board_pattern(yaml); + + device::Gimbal gimbal(config_path); device::Camera camera(config_path); cv::Mat img; std::chrono::steady_clock::time_point timestamp; @@ -38,7 +41,7 @@ void capture_loop( int count = 0; while (true) { camera.read(img, timestamp); - Eigen::Quaterniond q = cboard.imu_at(timestamp); + Eigen::Quaterniond q = gimbal.q(timestamp); // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂 auto img_with_ypr = img.clone(); @@ -48,7 +51,26 @@ 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 = calibration::find_pattern_points(img, board_pattern, centers_2d); + bool success; + if (board_pattern.pattern_type == calibration::PatternType::chessboard) { + // 棋盘格检测很慢，先在缩小图上快速检测，再映射回原图做亚像素精化 + cv::Mat small; + double scale = 0.5; + cv::resize(img, small, {}, scale, scale); + std::vector small_pts; + success = calibration::find_pattern_points(small, board_pattern, small_pts); + if (success) { + for (auto & p : small_pts) { p.x /= scale; p.y /= scale; } + cv::Mat gray; + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + cv::cornerSubPix( + gray, small_pts, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3)); + centers_2d = std::move(small_pts); + } + } else { + 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); // 显示时缩小图片尺寸 @@ -69,7 +91,7 @@ void capture_loop( component::logger()->info("[{}] Saved in {}", count, output_folder); } - // 离开该作用域时，camera和cboard会自动关闭 + // 离开该作用域时，camera和gimbal会自动关闭 } int main(int argc, char * argv[]) @@ -82,17 +104,20 @@ 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); + // 从配置文件读取标定板类型和尺寸 + auto yaml = YAML::LoadFile(config_path); + auto board_pattern = calibration::load_board_pattern(yaml); component::logger()->info( - "标定板类型: {}, 尺寸: {}x{}", calibration::pattern_name(board_pattern.pattern_type), + "标定板类型: {}, 尺寸: {}列{}行", + calibration::pattern_name(board_pattern.pattern_type), board_pattern.pattern_size.width, board_pattern.pattern_size.height); + // 主循环，保存图片和对应四元数 - capture_loop(config_path, output_folder, board_pattern); + capture_loop(config_path, output_folder); component::logger()->warn("注意四元数输出顺序为wxyz"); diff --git a/calibration/test/camera_calibration.py b/calibration/test/camera_calibration.py new file mode 100644 index 0000000..5550210 --- /dev/null +++ b/calibration/test/camera_calibration.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +相机校准应用程序 +使用检测到的棋盘格参数进行图像矫正和去畸变 +""" + +import cv2 +import numpy as np +import json +import os + + +class CameraCalibration: + def __init__(self, calibration_file='chessboard_detection_output/calibration_result.json'): + """ + 加载校准参数 + + Args: + calibration_file: 校准结果JSON文件路径 + """ + self.calibration_file = calibration_file + self.camera_matrix = None + self.dist_coeffs = None + self.load_calibration() + + def load_calibration(self): + """从JSON文件加载校准参数""" + if not os.path.exists(self.calibration_file): + raise FileNotFoundError(f"校准文件不存在: {self.calibration_file}") + + with open(self.calibration_file, 'r', encoding='utf-8') as f: + data = json.load(f) + + self.camera_matrix = np.array(data['camera_matrix']) + self.dist_coeffs = np.array(data['distortion_coefficients']) + + print("✓ 校准参数加载成功") + print(f" 重投影误差: {data['reprojection_error']:.4f} 像素") + print(f" 使用图像数: {data['num_images']}") + + def undistort_image(self, image): + """ + 对图像进行去畸变处理 + + Args: + image: 输入图像 + + Returns: + undistorted: 去畸变后的图像 + """ + h, w = image.shape[:2] + new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix( + self.camera_matrix, self.dist_coeffs, (w, h), 1, (w, h) + ) + + # 去畸变 + undistorted = cv2.undistort(image, self.camera_matrix, self.dist_coeffs, None, new_camera_matrix) + + # 裁剪图像 + x, y, w, h = roi + undistorted = undistorted[y:y+h, x:x+w] + + return undistorted + + def undistort_video(self, input_video, output_video='undistorted_video.avi'): + """ + 对视频进行去畸变处理 + + Args: + input_video: 输入视频路径 + output_video: 输出视频路径 + """ + cap = cv2.VideoCapture(input_video) + if not cap.isOpened(): + print(f"无法打开视频: {input_video}") + return + + # 获取视频参数 + fps = cap.get(cv2.CAP_PROP_FPS) + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + + # 读取第一帧获取尺寸 + ret, frame = cap.read() + if not ret: + print("无法读取视频帧") + return + + h, w = frame.shape[:2] + new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix( + self.camera_matrix, self.dist_coeffs, (w, h), 1, (w, h) + ) + x, y, w_roi, h_roi = roi + + # 创建视频写入器 + fourcc = cv2.VideoWriter_fourcc(*'XVID') + out = cv2.VideoWriter(output_video, fourcc, fps, (w_roi, h_roi)) + + print(f"开始处理视频 (共 {total_frames} 帧)...") + + # 重置到开头 + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + frame_count = 0 + + while True: + ret, frame = cap.read() + if not ret: + break + + frame_count += 1 + + # 去畸变 + undistorted = cv2.undistort(frame, self.camera_matrix, self.dist_coeffs, None, new_camera_matrix) + undistorted = undistorted[y:y+h_roi, x:x+w_roi] + + out.write(undistorted) + + if frame_count % 50 == 0: + print(f" 处理进度: {frame_count}/{total_frames} ({100*frame_count/total_frames:.1f}%)") + + cap.release() + out.release() + + print(f"\n✓ 视频处理完成，已保存到: {output_video}") + + def compare_images(self, image_path, output_path='comparison.jpg'): + """ + 生成原始图像和去畸变图像的对比图 + + Args: + image_path: 输入图像路径 + output_path: 输出对比图路径 + """ + image = cv2.imread(image_path) + if image is None: + print(f"无法读取图像: {image_path}") + return + + undistorted = self.undistort_image(image) + + # 调整尺寸以便并排显示 + h1, w1 = image.shape[:2] + h2, w2 = undistorted.shape[:2] + h = min(h1, h2) + image_resized = cv2.resize(image, (int(w1 * h / h1), h)) + undistorted_resized = cv2.resize(undistorted, (int(w2 * h / h2), h)) + + # 并排拼接 + comparison = np.hstack([image_resized, undistorted_resized]) + + # 添加文字标注 + cv2.putText(comparison, 'Original', (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) + cv2.putText(comparison, 'Undistorted', (w1 + 50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3) + + cv2.imwrite(output_path, comparison) + print(f"✓ 对比图已保存到: {output_path}") + + +def main(): + """主函数 - 演示如何使用校准参数""" + print("=== 相机校准应用程序 ===\n") + + # 加载校准参数 + calib = CameraCalibration() + + # 示例1: 对检测结果图像进行去畸变 + output_dir = 'chessboard_detection_output' + detected_images = [f for f in os.listdir(output_dir) if f.startswith('detected_') and f.endswith('.jpg')] + + if detected_images: + print(f"\n找到 {len(detected_images)} 张检测图像") + sample_image = os.path.join(output_dir, detected_images[0]) + print(f"生成对比图: {sample_image}") + calib.compare_images(sample_image, os.path.join(output_dir, 'comparison.jpg')) + + # 示例2: 对原始视频进行去畸变 + print("\n是否要对原始视频进行去畸变处理？") + print("注意: 这将处理整个视频，可能需要一些时间") + print("如需处理，请取消注释下面的代码行：") + print("# calib.undistort_video('Video_20260303114232727.avi', 'undistorted_video.avi')") + + +if __name__ == '__main__': + main() diff --git a/calibration/test/chessboard_detector.py b/calibration/test/chessboard_detector.py new file mode 100644 index 0000000..a25fe49 --- /dev/null +++ b/calibration/test/chessboard_detector.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +棋盘格检测程序 +用于检测视频中的11x8内角点棋盘格，并用于相机校准 +""" + +import cv2 +import numpy as np +import os +import json +from datetime import datetime + + +class ChessboardDetector: + def __init__(self, pattern_size=(11, 8), square_size=1.0): + """ + 初始化棋盘格检测器 + + Args: + pattern_size: 棋盘格内角点数量 (列, 行) + square_size: 棋盘格方格实际尺寸(单位:mm或其他) + """ + self.pattern_size = pattern_size + self.square_size = square_size + + # 准备棋盘格的3D坐标点 + self.objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32) + self.objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) + self.objp *= square_size + + # 存储所有图像的角点 + self.obj_points = [] # 3D点 + self.img_points = [] # 2D点 + + # 角点检测参数 + self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + def detect_chessboard(self, image): + """ + 检测单张图像中的棋盘格 + + Args: + image: 输入图像 + + Returns: + ret: 是否检测成功 + corners: 角点坐标 + gray: 灰度图 + """ + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + + # 查找棋盘格角点 + ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None) + + if ret: + # 亚像素精度优化 + corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) + + return ret, corners, gray + + def process_video(self, video_path, output_dir='output', sample_interval=30): + """ + 处理视频文件，检测棋盘格 + + Args: + video_path: 视频文件路径 + output_dir: 输出目录 + sample_interval: 采样间隔(帧数) + """ + if not os.path.exists(output_dir): + os.makedirs(output_dir) + + cap = cv2.VideoCapture(video_path) + if not cap.isOpened(): + print(f"无法打开视频: {video_path}") + return False + + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + fps = cap.get(cv2.CAP_PROP_FPS) + + print(f"视频信息: 总帧数={total_frames}, FPS={fps}") + print(f"开始检测棋盘格 (内角点: {self.pattern_size[0]}x{self.pattern_size[1]})...") + + frame_count = 0 + detected_count = 0 + + while True: + ret, frame = cap.read() + if not ret: + break + + frame_count += 1 + + # 按间隔采样 + if frame_count % sample_interval != 0: + continue + + # 检测棋盘格 + success, corners, gray = self.detect_chessboard(frame) + + if success: + detected_count += 1 + self.obj_points.append(self.objp) + self.img_points.append(corners) + + # 绘制角点 + vis_img = frame.copy() + cv2.drawChessboardCorners(vis_img, self.pattern_size, corners, success) + + # 保存结果图像 + output_path = os.path.join(output_dir, f'detected_{detected_count:03d}_frame{frame_count}.jpg') + cv2.imwrite(output_path, vis_img) + + print(f"✓ 帧 {frame_count}: 检测成功 (已保存 {detected_count} 张)") + else: + print(f"✗ 帧 {frame_count}: 未检测到棋盘格") + + cap.release() + + print(f"\n检测完成: 共处理 {frame_count} 帧, 成功检测 {detected_count} 张图像") + return detected_count > 0 + + def calibrate_camera(self, image_size): + """ + 执行相机校准 + + Args: + image_size: 图像尺寸 (width, height) + + Returns: + ret: 标定误差 + camera_matrix: 相机内参矩阵 + dist_coeffs: 畸变系数 + rvecs: 旋转向量 + tvecs: 平移向量 + """ + if len(self.obj_points) < 3: + print("错误: 需要至少3张成功检测的图像进行校准") + return None + + print(f"\n开始相机校准 (使用 {len(self.obj_points)} 张图像)...") + + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + self.obj_points, self.img_points, image_size, None, None + ) + + print(f"标定完成! 重投影误差: {ret:.4f} 像素") + + return ret, camera_matrix, dist_coeffs, rvecs, tvecs + + def save_calibration_results(self, camera_matrix, dist_coeffs, ret, output_path='calibration_result.json'): + """ + 保存校准结果到JSON文件 + """ + result = { + 'timestamp': datetime.now().strftime('%Y-%m-%d %H:%M:%S'), + 'pattern_size': self.pattern_size, + 'square_size': self.square_size, + 'num_images': len(self.obj_points), + 'reprojection_error': float(ret), + 'camera_matrix': camera_matrix.tolist(), + 'distortion_coefficients': dist_coeffs.tolist() + } + + with open(output_path, 'w', encoding='utf-8') as f: + json.dump(result, f, indent=4, ensure_ascii=False) + + print(f"\n校准结果已保存到: {output_path}") + + # 打印结果 + print("\n=== 相机校准结果 ===") + print(f"重投影误差: {ret:.4f} 像素") + print(f"\n相机内参矩阵:") + print(camera_matrix) + print(f"\n畸变系数:") + print(dist_coeffs.ravel()) + + +def main(): + # 配置参数 + VIDEO_PATH = 'Video_20260303114232727.avi' + OUTPUT_DIR = 'chessboard_detection_output' + PATTERN_SIZE = (11, 8) # 11x8 内角点 + SQUARE_SIZE = 25.0 # 假设每个方格25mm，根据实际情况调整 + SAMPLE_INTERVAL = 30 # 每30帧采样一次 + + # 创建检测器 + detector = ChessboardDetector(pattern_size=PATTERN_SIZE, square_size=SQUARE_SIZE) + + # 处理视频 + success = detector.process_video(VIDEO_PATH, OUTPUT_DIR, SAMPLE_INTERVAL) + + if not success: + print("未能检测到任何棋盘格，程序退出") + return + + # 获取图像尺寸 + cap = cv2.VideoCapture(VIDEO_PATH) + ret, frame = cap.read() + if ret: + image_size = (frame.shape[1], frame.shape[0]) + cap.release() + + # 执行相机校准 + calib_result = detector.calibrate_camera(image_size) + + if calib_result: + ret, camera_matrix, dist_coeffs, rvecs, tvecs = calib_result + + # 保存校准结果 + detector.save_calibration_results( + camera_matrix, dist_coeffs, ret, + os.path.join(OUTPUT_DIR, 'calibration_result.json') + ) + + print("\n✓ 校准完成！可以使用生成的校准参数进行图像矫正") + + +if __name__ == '__main__': + main() diff --git a/calibration/test/visualize_video.py b/calibration/test/visualize_video.py new file mode 100644 index 0000000..76b1e00 --- /dev/null +++ b/calibration/test/visualize_video.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +可视化视频中的棋盘格检测 +实时显示每一帧的检测结果 +""" + +import cv2 +import numpy as np + + +class ChessboardVisualizer: + def __init__(self, pattern_size=(11, 8)): + """ + 初始化可视化器 + + Args: + pattern_size: 棋盘格内角点数量 (列, 行) + """ + self.pattern_size = pattern_size + self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + def visualize_video(self, video_path, output_path='visualized_video.avi', show_window=False): + """ + 可视化整个视频的棋盘格检测 + + Args: + video_path: 输入视频路径 + output_path: 输出视频路径 + show_window: 是否显示实时窗口 + """ + cap = cv2.VideoCapture(video_path) + if not cap.isOpened(): + print(f"无法打开视频: {video_path}") + return + + # 获取视频参数 + fps = cap.get(cv2.CAP_PROP_FPS) + width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + + print(f"视频信息: {width}x{height}, {fps:.2f} FPS, {total_frames} 帧") + + # 创建视频写入器 + fourcc = cv2.VideoWriter_fourcc(*'XVID') + out = cv2.VideoWriter(output_path, fourcc, fps, (width, height)) + + frame_count = 0 + detected_count = 0 + + print(f"\n开始处理视频...") + print("绿色角点 = 检测成功, 红色文字 = 未检测到") + + while True: + ret, frame = cap.read() + if not ret: + break + + frame_count += 1 + + # 转换为灰度图 + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # 检测棋盘格 + ret_detect, corners = cv2.findChessboardCorners(gray, self.pattern_size, None) + + # 创建可视化图像 + vis_frame = frame.copy() + + if ret_detect: + detected_count += 1 + # 亚像素精度优化 + corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria) + + # 绘制角点 + cv2.drawChessboardCorners(vis_frame, self.pattern_size, corners, ret_detect) + + # 添加成功标记 + cv2.putText(vis_frame, f'DETECTED #{detected_count}', (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + else: + # 添加未检测标记 + cv2.putText(vis_frame, 'NOT DETECTED', (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + + # 添加帧信息 + cv2.putText(vis_frame, f'Frame: {frame_count}/{total_frames}', (20, height - 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) + + # 写入输出视频 + out.write(vis_frame) + + # 显示窗口（可选） + if show_window: + cv2.imshow('Chessboard Detection', vis_frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + print("\n用户中断") + break + + # 进度显示 + if frame_count % 50 == 0: + progress = 100 * frame_count / total_frames + print(f" 进度: {frame_count}/{total_frames} ({progress:.1f}%) - 已检测: {detected_count}") + + cap.release() + out.release() + if show_window: + cv2.destroyAllWindows() + + print(f"\n✓ 处理完成!") + print(f" 总帧数: {frame_count}") + print(f" 检测成功: {detected_count} 帧 ({100*detected_count/frame_count:.1f}%)") + print(f" 输出文件: {output_path}") + + +def main(): + VIDEO_PATH = 'Video_20260303114232727.avi' + OUTPUT_PATH = 'visualized_chessboard_detection.avi' + PATTERN_SIZE = (11, 8) + + visualizer = ChessboardVisualizer(pattern_size=PATTERN_SIZE) + visualizer.visualize_video(VIDEO_PATH, OUTPUT_PATH, show_window=False) + + +if __name__ == '__main__': + main() diff --git a/configs/calibration.yaml b/configs/calibration.yaml index 7cb9f73..7a40478 100644 --- a/configs/calibration.yaml +++ b/configs/calibration.yaml @@ -18,3 +18,7 @@ can_interface: "can0" #####-----gimbal参数-----##### com_port: "/dev/ttyUSB0" baudrate: 115200 + +# 重投影误差: 0.1791px +camera_matrix: [1827.8294221039337, 0, 716.86057740384501, 0, 1828.9736207357851, 613.69509305531699, 0, 0, 1] +distort_coeffs: [-0.083642708058668358, 0.18891600176175308, -0.00030362184648520616, -0.00066798903909152669, 0]