add 棋盘格验证通过

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;
 

calibration/capture.cpp

@@ -1,16 +1,16 @@
 #include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
 
 #include <filesystem>
 #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"
+#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 std::string & config_path, const std::string & output_folder)
-  const calibration::BoardPattern & board_pattern)
 {
-  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<cv::Point2f> 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<cv::Point2f> 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<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);
 
+  // 从配置文件读取标定板类型和尺寸
+  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");
 

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()

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()

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()

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]