Add camera calibration toolchain and armor pose solver

- capture_calib: capture calibration images (MindVision / USB) with a live quality gate (Laplacian sharpness, 3x3 coverage, board auto-detect) - calibrate: batch intrinsics from calib_images/, writes configs/camera.yaml - camera_calib: CameraCalib loader (intrinsics + distortion, cached undistort remap) and solvePose() via IPPE with a reprojection gate - configs/camera.yaml: MV-SUA131GC intrinsics, RMS 0.483 px over 31 images, ruler-verified distance error < 2% - xmake: three targets sharing an rm_deps rule, C++17 - gitignore: keep calibration photos/videos and generated compile_commands.json out of the repo
2026-09-15 23:33:01 +08:00
parent 14d7f9e571
commit ca6891eef2
12 changed files with 1183 additions and 110 deletions
@@ -0,0 +1,100 @@
+#ifndef CAMERA_CALIB_HPP
+#define CAMERA_CALIB_HPP
+
+/**
+ * @file  camera_calib.hpp
+ * @brief 相机标定数据（内参/畸变）的加载与使用 —— 27RM
+ *
+ * 配置来源：configs/camera.yaml（由 ./calibrate 生成）
+ *
+ * 典型用法：
+ *   CameraCalib calib;
+ *   if (!calib.load("configs/camera.yaml")) return -1;
+ *
+ *   // 1) 去畸变（每帧）
+ *   cv::Mat undist;
+ *   calib.undistort(frame, undist);
+ *
+ *   // 2) 装甲板四角点 → 位姿（角点顺序：左下、左上、右上、右下）
+ *   std::array<cv::Point2f, 4> corners{lb, lt, rt, rb};
+ *   if (auto pose = calib.solvePose(corners, ArmorSize::small())) {
+ *       double yaw, pitch;
+ *       CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch);   // 云台需要的角度
+ *       printf("距离 %.2f m, yaw %.1f°, pitch %.1f°, 重投影 %.2f px\n",
+ *              pose->distance_mm / 1000.0, yaw * 180 / CV_PI,
+ *              pitch * 180 / CV_PI, pose->reproj_error_px);
+ *   }
+ */
+
+#include <opencv2/opencv.hpp>
+
+#include <array>
+#include <optional>
+#include <string>
+
+/** 装甲板物理尺寸（毫米）：width = 两灯条中心间距，height = 灯条长度 */
+struct ArmorSize {
+    double width_mm;
+    double height_mm;
+
+    /** 大装甲板：英雄（一号）230mm 间距 */
+    static ArmorSize big() { return ArmorSize{230.0, 56.0}; }
+    /** 小装甲板：2/3/4/5 号、哨兵 135mm 间距 */
+    static ArmorSize small() { return ArmorSize{135.0, 56.0}; }
+};
+
+/** 位姿解算结果（相机坐标系，单位 mm） */
+struct ArmorPose {
+    cv::Vec3d rvec{0, 0, 0};        // 旋转向量
+    cv::Vec3d tvec{0, 0, 0};        // 装甲板中心位置
+    double distance_mm = 0.0;       // 到装甲板中心的距离
+    double reproj_error_px = 0.0;   // 重投影误差（质量指标）
+};
+
+class CameraCalib {
+public:
+    CameraCalib() = default;
+    explicit CameraCalib(const std::string &yaml_path) { load(yaml_path); }
+
+    /** 加载标定文件；成功返回 true（失败会打印原因） */
+    bool load(const std::string &yaml_path);
+
+    bool valid() const { return valid_; }
+    const cv::Mat &K() const { return camera_matrix_; }
+    const cv::Mat &D() const { return dist_coeffs_; }
+    cv::Size imageSize() const { return image_size_; }
+
+    /** 每帧去畸变（内部缓存映射表，尺寸不变时不用重建） */
+    void undistort(const cv::Mat &src, cv::Mat &dst);
+
+    /** 重投影误差门限（像素）：超过则视为无效解，默认 8.0 */
+    void setMaxReprojError(double px) { max_reproj_err_px_ = px; }
+
+    /**
+     * 四个角点 → 装甲板位姿
+     * @param corners 图像坐标，顺序必须是：左下、左上、右上、右下
+     *                （与 jlu_vision 的 ArmorPoints.hpp 3D 点顺序一致）
+     * @param size    装甲板物理尺寸
+     * @return 成功返回位姿；无解或重投影误差过大返回 std::nullopt
+     */
+    std::optional<ArmorPose> solvePose(const std::array<cv::Point2f, 4> &corners,
+                                       const ArmorSize &size) const;
+
+    /** 位置 → 云台角度（弧度）：yaw 向右为正、pitch 向上为正 */
+    static void poseToYawPitch(const cv::Vec3d &tvec, double &yaw, double &pitch) {
+        yaw = std::atan2(tvec[0], tvec[2]);
+        pitch = std::atan2(-tvec[1], std::sqrt(tvec[0] * tvec[0] +
+                                               tvec[2] * tvec[2]));
+    }
+
+private:
+    cv::Mat camera_matrix_;
+    cv::Mat dist_coeffs_;
+    cv::Size image_size_{0, 0};
+    bool valid_ = false;
+    double max_reproj_err_px_ = 8.0;
+
+    cv::Mat map1_, map2_;   // 去畸变映射表缓存
+};
+
+#endif // CAMERA_CALIB_HPP