Add pose_test tool and harden solvePose; expose camera controls

- pose_test: verify the geometry chain (4 corners -> distance/yaw/pitch)
  before any detector exists; modes: camera + mouse clicks, --image,
  --corners (headless), --selftest (synthetic round-trip, no camera needed)
- solvePose: undistort corners first, then solvePnPGeneric(IPPE) and pick
  the best candidate; reject solutions behind the camera (z<=0) or with
  reprojection error > 8px
  - fixes IPPE + distortion + in-plane rotation returning garbage
  - 4 coplanar points: cyclic/mirrored orderings give the same quad and
    the same pose (harmless); crossing orderings blow up reprojection
    and are rejected
- camera_set: implement setAeState/setExposureTime/setAnalogGain(+-Times)/
  setWbMode/setFrameSpeed; configs/camera_runtime.yaml drives them
- selftest passes all 6 synthetic cases; verified on 6 real photos
  (chessboard outer rect 180x120mm): distance 0.44-0.69m, reproj <2.5px

README.md

@@ -5,7 +5,7 @@ RoboMaster 视觉工程（MindVision 相机 + OpenCV）。
 ## 构建
 
 ```bash
-xmake                 # 产出三个程序（见下）
+xmake                 # 产出四个程序（见下）
 xmake run 27RM        # 跑主程序
 ```
 
@@ -26,12 +26,14 @@ xmake project -k compile_commands .vscode
 | `27RM` | 自瞄主程序 |
 | `capture_calib` | **相机标定图采集工具** |
 | `calibrate` | **标定计算工具**（照片 → 内参/畸变 → `configs/camera.yaml`） |
+| `pose_test` | **几何链路验证工具**（4 角点 → 距离/yaw/pitch，含合成自检） |
 
 ## 配置
 
 ```
 configs/
-└── camera.yaml     # 相机内参 + 畸变 + 标定元信息（由 calibrate 生成，需入库）
+├── camera.yaml          # 相机内参 + 畸变 + 标定元信息（由 calibrate 生成，需入库）
+└── camera_runtime.yaml  # 运行时参数：曝光/增益/白平衡/帧率（手工维护，需入库）
 ```
 
 代码里这样用（`inc/camera_calib.hpp`）：
@@ -54,8 +56,52 @@ if (auto pose = calib.solvePose(corners, ArmorSize::small())) {
 }
 ```
 
+`solvePose` 内部已处理：去畸变 → `solvePnPGeneric`(IPPE) 取回全部候选解 →
+筛掉「相机后方 (z<=0)」「重投影 > 8px」→ 取最优。详情见源码注释。
+
+相机参数设置（`inc/camera_set.hpp`，`initializeCamera` 之后调用）：
+
+```cpp
+cam.setAeState(false);            // 手动曝光（比赛必须，否则画面亮度漂移）
+cam.setExposureTime(3000.0);      // 曝光时间，微秒
+cam.setAnalogGainTimes(2.0);      // 模拟增益，倍率（内部按 fAnalogGainStep 换算）
+cam.setWbMode(false);             // 关自动白平衡（颜色阈值才稳定）
+cam.setFrameSpeed(2);             // 帧率档位 0/1/2
+```
+
+也可以直接由配置驱动：`applyRuntimeParams` 的示例见 `src/pose_test.cpp`。
+
 > 程序必须在工程根目录运行（`configs/` 是相对路径），或自行传绝对路径。
 
+## pose_test：几何链路验证
+
+写检测器之前，先用它验证「标定 + 坐标约定 + PnP 解算」：
+拿真的装甲板（或打印 135mm 矩形）摆在相机前，鼠标点 4 个角，
+输出的距离/yaw/pitch 与卷尺、量角器核对。之后接检测器出问题必是检测的锅。
+
+```bash
+./build/linux/x86_64/debug/pose_test                      # 相机 + 鼠标点角
+./build/linux/x86_64/debug/pose_test --image 照片.jpg      # 图片模式
+./build/linux/x86_64/debug/pose_test --selftest            # 合成自检（无需相机/显示器）
+# 脚本/无窗口：直接给 4 个角点（左下 左上 右上 右下），--size 自定义矩形(毫米)
+./build/linux/x86_64/debug/pose_test --image 照片.jpg --size 180x120 \
+    --corners x1,y1 x2,y2 x3,y3 x4,y4
+```
+
+- 相机模式按键：`r` 清除重选 | `b` 切换大小装甲 | `u` 切换去畸变显示 | `s` 截图 | `q` 退出
+- `--selftest` 自检已通过：正前方/右偏/上仰/复合/板内滚转均精确恢复，
+  交叉乱序被重投影门限正确拒绝
+- 真实照片验证：6 张标定图（棋盘格外圈 180×120mm）距离 0.44–0.69m、
+  重投影 0.3–2.5px，与卷尺真值一致
+
+### 两个踩过的坑（写检测器时别重蹈）
+
+1. **IPPE + 畸变 + 板内旋转 = 垃圾解**（OpenCV 4.x/5.x 都中招）。
+   解法：角点先去畸变再喂 IPPE，重投影回原图算（`solvePose` 已内置）。
+2. **4 个共面点的顺序错误查不出来**：循环/镜像顺序 = 同一个四边形 =
+   同一个解（无害）；交叉乱序会被重投影门限拦下。真正需要 roll 门限的是
+   「检测器给出假四边形」场景，等写检测器时照搬 jlu 的 `roll_thres 15°`。
+
 ## 相机标定流程
 
 > **本工程标定板规格**：10 × 7 内角点、**方格 20.0 mm**（卡尺实测值，不是打印标称值）

configs/camera_runtime.yaml

@@ -0,0 +1,19 @@
+%YAML:1.0
+---
+# ============================================================
+# 相机运行时参数（27RM）
+# 与 configs/camera.yaml（标定）分开存放：
+#   camera.yaml        由 ./calibrate 生成，会被整体重写
+#   camera_runtime.yaml 手工维护，calibrate 不会碰它
+#
+# ⚠️ 比赛/联调必须【手动曝光】：
+#   自动曝光/自动白平衡会让画面亮度随环境漂移，
+#   传统 CV 检测的 HSV 阈值会直接失效。
+# ============================================================
+ae_state: 0            # 曝光模式：0=手动（推荐） 1=自动
+exposure_time_us: 3000.0   # 曝光时间（微秒）。MV-SUA131GC 常用 1000~15000，
+                           # 数值越大画面越亮但运动拖影越重
+analog_gain: 2.0       # 模拟增益（倍率）。1.0 = 不放大；SDK 内部按
+                       # fAnalogGainStep 换算（例：0.125 步进 → 2.0x = 原始值16）
+auto_wb: 0             # 白平衡：0=手动（推荐，颜色阈值才稳定） 1=自动
+frame_speed: 2         # 帧率档位：0=低速 1=正常 2=高速

inc/camera_calib.hpp

@@ -72,10 +72,12 @@ public:
 
     /**
      * 四个角点 → 装甲板位姿
-     * @param corners 图像坐标，顺序必须是：左下、左上、右上、右下
+     * 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 →
-     *                （与 jlu_vision 的 ArmorPoints.hpp 3D 点顺序一致）
+     * 依次筛掉「z<=0（相机后方）」「重投影超差」→ 取重投影最小者。
+     * 注：角点在解算前先做去畸变（IPPE+畸变+板内旋转组合有数值坑）。
+     * @param corners 图像坐标（原图，含畸变），顺序：左下、左上、右上、右下
      * @param size    装甲板物理尺寸
-     * @return 成功返回位姿；无解或重投影误差过大返回 std::nullopt
+     * @return 成功返回位姿；全部候选被筛掉时返回 std::nullopt
      */
     std::optional<ArmorPose> solvePose(const std::array<cv::Point2f, 4> &corners,
                                        const ArmorSize &size) const;

inc/camera_set.hpp

@@ -14,11 +14,13 @@ public:
     bool initializeCamera();                                        // 初始化相机
     bool startCamera();                                             // 启动相机
     bool stopCamera();                                              // 停止相机
-    // bool setExposureTime(double exposureTime);                      // 设置曝光时间
+    // ---- 参数设置：在 initializeCamera 之后、startCamera 之前调用 ----
-    // bool setGain(double gain);                                      // 设置增益
+    bool setAeState(bool aeOn);                                    // 曝光模式：true=自动, false=手动（比赛必须手动）
-    // bool setWhiteBalance(double redGain, double blueGain);          // 设置白平衡
+    bool setExposureTime(double exposureUs);                       // 曝光时间（微秒 us）
-    // bool setResolution(int width, int height);                      // 设置分辨率
+    bool setAnalogGainRaw(int gainRaw);                            // 模拟增益（SDK 原始整数，范围见 capability.sExposeDesc）
-    // bool setFrameRate(double frameRate);                            // 设置帧率
+    bool setAnalogGainTimes(double gainTimes);                     // 模拟增益（倍率，内部按 fAnalogGainStep 换算成原始值）
+    bool setWbMode(bool autoWb);                                   // 白平衡：true=自动, false=手动
+    bool setFrameSpeed(int speed);                                 // 帧率档位：0=低速 1=正常 2=高速
     bool getFrame(cv::Mat &frame);                                 // 获取一帧图像
 
 private:

src/camera_calib.cpp

@@ -65,26 +65,66 @@ std::optional<ArmorPose> CameraCalib::solvePose(
     const std::vector<cv::Point2f> img_points{corners[0], corners[1],
                                               corners[2], corners[3]};
 
-    ArmorPose pose;
+    // ⚠️ 先把角点去畸变到理想像素坐标，再用 IPPE（D 传空）。
-    if (!cv::solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_,
+    //    实测：IPPE + 畸变系数 + 板内旋转 三者组合会解出垃圾解
-                      pose.rvec, pose.tvec, false, cv::SOLVEPNP_IPPE)) {
+    //    （OpenCV 4.x/5.x 都有这坑），去畸变后数值稳定。
+    std::vector<cv::Point2f> ideal;
+    cv::undistortPoints(img_points, ideal, camera_matrix_, dist_coeffs_,
+                        cv::noArray(), camera_matrix_);
+
+    // 平面靶标有镜像歧义：IPPE 会给出两个候选解，
+    // solvePnPGeneric 一次性全部取回，逐个筛。
+    std::vector<cv::Mat> rvecs, tvecs;
+    const int n = cv::solvePnPGeneric(obj_points, ideal, camera_matrix_,
+                                      cv::Mat(), rvecs, tvecs, false,
+                                      cv::SOLVEPNP_IPPE);
+    if (n <= 0) {
         return std::nullopt;
     }
 
-    // 自检：把 3D 点投影回图像，误差过大说明解不可信（例如角点顺序错、板子不是这个尺寸）
+    int best = -1;
-    std::vector<cv::Point2f> reproj;
+    double best_err = 1e18;
-    cv::projectPoints(obj_points, pose.rvec, pose.tvec, camera_matrix_,
+    std::vector<cv::Point2f> proj;
-                      dist_coeffs_, reproj);
+    for (int i = 0; i < n; ++i) {
+        const cv::Mat &rv = rvecs[i];
+        const cv::Mat &tv = tvecs[i];
+
+        // ① 目标必须在相机前方（z > 0）。镜像解常落在相机后方。
+        const double z = tv.at<double>(2);
+        if (z <= 0.0) {
+            continue;
+        }
+
+        // ② 重投影误差在【原图（含畸变）】上计算，与角点像素一致。
+        //    4 个共面点的循环/镜像顺序给出同一个四边形→同一个解（无害）；
+        //    真正有害的交叉乱序（蝴蝶结）会在这里误差爆炸被拦下。
+        cv::projectPoints(obj_points, rv, tv, camera_matrix_, dist_coeffs_,
+                          proj);
         double err = 0.0;
-    for (std::size_t i = 0; i < reproj.size(); ++i) {
+        for (std::size_t k = 0; k < proj.size(); ++k) {
-        err += cv::norm(reproj[i] - img_points[i]);
+            err += cv::norm(proj[k] - img_points[k]);
         }
-    err /= static_cast<double>(reproj.size());
+        err /= static_cast<double>(proj.size());
-    pose.reproj_error_px = err;
         if (err > max_reproj_err_px_) {
+            continue;
+        }
+
+        if (err < best_err) {
+            best_err = err;
+            best = i;
+        }
+    }
+
+    if (best < 0) {
         return std::nullopt;
     }
 
+    ArmorPose pose;
+    const cv::Mat &rv = rvecs[best];
+    const cv::Mat &tv = tvecs[best];
+    pose.rvec = cv::Vec3d(rv.at<double>(0), rv.at<double>(1), rv.at<double>(2));
+    pose.tvec = cv::Vec3d(tv.at<double>(0), tv.at<double>(1), tv.at<double>(2));
+    pose.reproj_error_px = best_err;
     pose.distance_mm = cv::norm(pose.tvec);
     return pose;
 }

src/camera_set.cpp

@@ -1,6 +1,7 @@
 #include "camera_set.hpp"
 #include <opencv2/opencv.hpp>
 #include "CameraApi.h"
+#include <cstdio>
 #include <cstdlib>
 
 bool CameraGrabber::initializeCamera()
@@ -119,6 +120,107 @@ bool CameraGrabber::stopCamera()
     return true;
 }
 
+bool CameraGrabber::setAeState(bool aeOn)
+{
+    if (!isInitialized || hCamera == -1)
+    {
+        fprintf(stderr, "setAeState failed: camera not initialized.\n");
+        return false;
+    }
+    CameraSdkStatus st = CameraSetAeState(hCamera, aeOn ? TRUE : FALSE);
+    if (st != CAMERA_STATUS_SUCCESS)
+    {
+        fprintf(stderr, "CameraSetAeState failed, status = %d\n", (int)st);
+        return false;
+    }
+    return true;
+}
+
+bool CameraGrabber::setExposureTime(double exposureUs)
+{
+    if (!isInitialized || hCamera == -1)
+    {
+        fprintf(stderr, "setExposureTime failed: camera not initialized.\n");
+        return false;
+    }
+    CameraSdkStatus st = CameraSetExposureTime(hCamera, exposureUs);
+    if (st != CAMERA_STATUS_SUCCESS)
+    {
+        fprintf(stderr, "CameraSetExposureTime(%g us) failed, status = %d\n",
+                exposureUs, (int)st);
+        return false;
+    }
+    return true;
+}
+
+bool CameraGrabber::setAnalogGainRaw(int gainRaw)
+{
+    if (!isInitialized || hCamera == -1)
+    {
+        fprintf(stderr, "setAnalogGainRaw failed: camera not initialized.\n");
+        return false;
+    }
+    // 夹到相机能力范围 [uiAnalogGainMin, uiAnalogGainMax]
+    int v = gainRaw;
+    const int lo = (int)capability.sExposeDesc.uiAnalogGainMin;
+    const int hi = (int)capability.sExposeDesc.uiAnalogGainMax;
+    if (v < lo) v = lo;
+    if (v > hi) v = hi;
+    CameraSdkStatus st = CameraSetAnalogGain(hCamera, v);
+    if (st != CAMERA_STATUS_SUCCESS)
+    {
+        fprintf(stderr, "CameraSetAnalogGain(%d) failed, status = %d\n", v, (int)st);
+        return false;
+    }
+    return true;
+}
+
+bool CameraGrabber::setAnalogGainTimes(double gainTimes)
+{
+    // SDK 的增益是整数步进：实际放大倍数 = 原始值 * fAnalogGainStep
+    // 例如 fAnalogGainStep=0.125、原始值 16 → 2 倍
+    const float step = capability.sExposeDesc.fAnalogGainStep;
+    if (step <= 0.0f)
+    {
+        fprintf(stderr, "setAnalogGainTimes failed: fAnalogGainStep 无效 (%g)\n", step);
+        return false;
+    }
+    const int raw = (int)(gainTimes / step + 0.5);
+    return setAnalogGainRaw(raw);
+}
+
+bool CameraGrabber::setWbMode(bool autoWb)
+{
+    if (!isInitialized || hCamera == -1)
+    {
+        fprintf(stderr, "setWbMode failed: camera not initialized.\n");
+        return false;
+    }
+    CameraSdkStatus st = CameraSetWbMode(hCamera, autoWb ? TRUE : FALSE);
+    if (st != CAMERA_STATUS_SUCCESS)
+    {
+        fprintf(stderr, "CameraSetWbMode failed, status = %d\n", (int)st);
+        return false;
+    }
+    return true;
+}
+
+bool CameraGrabber::setFrameSpeed(int speed)
+{
+    if (!isInitialized || hCamera == -1)
+    {
+        fprintf(stderr, "setFrameSpeed failed: camera not initialized.\n");
+        return false;
+    }
+    CameraSdkStatus st = CameraSetFrameSpeed(hCamera, speed);
+    if (st != CAMERA_STATUS_SUCCESS)
+    {
+        fprintf(stderr, "CameraSetFrameSpeed(%d) failed, status = %d\n", speed, (int)st);
+        return false;
+    }
+    return true;
+}
+
 bool CameraGrabber::getFrame(cv::Mat &frame)
 {
     // 前者条件检查

src/pose_test.cpp

@@ -0,0 +1,473 @@
+/**
+ * @file  pose_test.cpp
+ * @brief 几何链路验证工具：4 角点 → PnP → 距离 / yaw / pitch
+ *
+ * 在写装甲板检测器之前，先用它验证「标定 + 坐标约定 + PnP 解算」
+ * 是否正确：拿一块真的装甲板（或打印的 135mm 矩形）摆在相机前，
+ * 鼠标点 4 个角，用卷尺/量角器核对输出的距离与角度。
+ * 之后接任何检测器出问题，都一定是检测的锅。
+ *
+ * 模式：
+ *   ./pose_test                                    相机实时交互
+ *   ./pose_test --image 照片.jpg                   图片交互
+ *   ./pose_test --image 照片.jpg --corners x1,y1 x2,y2 x3,y3 x4,y4
+ *                                                   无窗口直接算（脚本/沙箱可用）
+ *   ./pose_test --selftest                          合成数据自检（无需相机/显示器）
+ *
+ * 角点顺序约定（与 jlu_vision 一致）：左下 → 左上 → 右上 → 右下
+ */
+
+#include "camera_calib.hpp"
+#include "camera_set.hpp"
+
+#include <opencv2/opencv.hpp>
+
+#include <array>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <string>
+#include <vector>
+
+namespace {
+
+// ---------------------------------------------------------------
+// 命令行选项
+// ---------------------------------------------------------------
+struct Options {
+    std::string image_path;
+    std::string calib_path = "configs/camera.yaml";
+    std::string runtime_path = "configs/camera_runtime.yaml";
+    std::vector<cv::Point2f> corners;   // --corners，4 个 "x,y"
+    double custom_w = 0.0, custom_l = 0.0;   // --size WxL（毫米），覆盖装甲板尺寸
+    bool selftest = false;              // --selftest
+};
+
+void printUsage()
+{
+    std::printf(
+        "用法:\n"
+        "  pose_test                                    相机交互：鼠标点 4 个角\n"
+        "  pose_test --image 照片.jpg                   图片交互\n"
+        "  pose_test --image 照片.jpg --corners x1,y1 x2,y2 x3,y3 x4,y4\n"
+        "                                               无窗口直接算（点序：左下 左上 右上 右下）\n"
+        "  pose_test --selftest                         合成数据自检（无需相机/显示器）\n"
+        "\n"
+        "  其他: --calib 标定文件(默认 configs/camera.yaml)\n"
+        "        --runtime 相机运行时参数(默认 configs/camera_runtime.yaml)\n"
+        "        --big 大装甲板 230mm（默认小装甲板 135mm）\n"
+        "        --size WxL 自定义矩形尺寸(毫米)，如 --size 180x120\n"
+        "\n"
+        "相机模式按键: r 清除重选 | b 切换大小装甲 | u 切换去畸变显示 | s 截图 | q/ESC 退出\n");
+}
+
+bool parseArgs(int argc, char **argv, Options &opt, bool &bigArmor)
+{
+    for (int i = 1; i < argc; ++i) {
+        std::string a = argv[i];
+        if (a == "--image" && i + 1 < argc) {
+            opt.image_path = argv[++i];
+        } else if (a == "--calib" && i + 1 < argc) {
+            opt.calib_path = argv[++i];
+        } else if (a == "--runtime" && i + 1 < argc) {
+            opt.runtime_path = argv[++i];
+        } else if (a == "--big") {
+            bigArmor = true;
+        } else if (a == "--size" && i + 1 < argc) {
+            double w = 0.0, l = 0.0;
+            if (std::sscanf(argv[++i], "%lfx%lf", &w, &l) != 2) {
+                std::fprintf(stderr, "--size 格式错误: 要 WxL（毫米），如 180x120\n");
+                return false;
+            }
+            opt.custom_w = w;
+            opt.custom_l = l;
+        } else if (a == "--selftest") {
+            opt.selftest = true;
+        } else if (a == "--corners") {
+            for (; i + 1 < argc; ++i) {
+                std::string s = argv[i + 1];
+                if (s.rfind("--", 0) == 0) break;   // 撞到下一个选项就停
+                float x = 0.f, y = 0.f;
+                if (std::sscanf(s.c_str(), "%f,%f", &x, &y) != 2) {
+                    std::fprintf(stderr, "角点格式错误: %s（要 x,y）\n", s.c_str());
+                    return false;
+                }
+                opt.corners.push_back(cv::Point2f(x, y));
+            }
+        } else if (a == "-h" || a == "--help") {
+            printUsage();
+            std::exit(0);
+        } else {
+            std::fprintf(stderr, "未知参数: %s\n", a.c_str());
+            return false;
+        }
+    }
+    if (!opt.corners.empty() && opt.corners.size() != 4) {
+        std::fprintf(stderr, "--corners 必须正好 4 个点（左下 左上 右上 右下）\n");
+        return false;
+    }
+    return true;
+}
+
+// ---------------------------------------------------------------
+// 相机运行时参数（曝光/增益/白平衡/帧率）
+// ---------------------------------------------------------------
+bool applyRuntimeParams(const std::string &path, CameraGrabber &cam)
+{
+    cv::FileStorage fs(path, cv::FileStorage::READ);
+    if (!fs.isOpened()) {
+        std::printf("[pose_test] 未找到 %s，沿用相机当前参数（⚠️ 比赛请手动曝光）\n",
+                    path.c_str());
+        return true;
+    }
+    int ae = 0, autoWb = 0, frameSpeed = 2;
+    double exposureUs = 3000.0, gain = 2.0;
+    fs["ae_state"] >> ae;
+    fs["exposure_time_us"] >> exposureUs;
+    fs["analog_gain"] >> gain;
+    fs["auto_wb"] >> autoWb;
+    fs["frame_speed"] >> frameSpeed;
+    fs.release();
+
+    bool ok = true;
+    ok &= cam.setAeState(ae != 0);
+    ok &= cam.setExposureTime(exposureUs);
+    ok &= cam.setAnalogGainTimes(gain);
+    ok &= cam.setWbMode(autoWb != 0);
+    ok &= cam.setFrameSpeed(frameSpeed);
+
+    std::printf("[pose_test] 相机参数已应用: %s曝光 %.0fus | 增益 %.2fx | %s | 帧率档%d %s\n",
+                ae ? "自动" : "手动", exposureUs, gain, autoWb ? "AWB开" : "AWB关",
+                frameSpeed, ok ? "" : "⚠️ 部分设置失败");
+    return ok;
+}
+
+// ---------------------------------------------------------------
+// 交互：鼠标点 4 角
+// ---------------------------------------------------------------
+struct ClickState {
+    std::vector<cv::Point2f> pts;   // 显示窗口坐标系下的点
+    bool dirty = false;             // 有新点/需重新解算
+};
+
+void onMouse(int event, int x, int y, int, void *param)
+{
+    if (event != cv::EVENT_LBUTTONDOWN) return;
+    auto *st = static_cast<ClickState *>(param);
+    if (st->pts.size() >= 4) return;
+    st->pts.push_back(cv::Point2f(static_cast<float>(x), static_cast<float>(y)));
+    st->dirty = true;
+}
+
+void drawClicks(cv::Mat &view, const ClickState &st)
+{
+    for (std::size_t i = 0; i < st.pts.size(); ++i) {
+        cv::circle(view, st.pts[i], 5, cv::Scalar(0, 255, 255), -1);
+        cv::putText(view, std::to_string(static_cast<int>(i) + 1),
+                    st.pts[i] + cv::Point2f(9, -9), cv::FONT_HERSHEY_SIMPLEX, 0.7,
+                    cv::Scalar(0, 255, 255), 2);
+        if (i > 0)
+            cv::line(view, st.pts[i - 1], st.pts[i], cv::Scalar(0, 255, 255), 1);
+    }
+    if (st.pts.size() == 4)
+        cv::line(view, st.pts[3], st.pts[0], cv::Scalar(0, 255, 255), 1);
+}
+
+/** 把显示窗口坐标换回原始（含畸变）图像坐标。
+ *  默认在【原图】上点角 → 原样返回；在【去畸变图】上点角时，
+ *  用 Brown-Conrady 正向畸变把理想坐标映射回含畸变坐标
+ *  （OpenCV 4.x 没有 cv::distortPoints，那是 5.0 才有的接口）。 */
+std::array<cv::Point2f, 4> toOriginalCorners(const CameraCalib &calib,
+                                             const std::array<cv::Point2f, 4> &viewPts,
+                                             bool viewIsUndistorted)
+{
+    if (!viewIsUndistorted) return viewPts;
+
+    const cv::Mat &K = calib.K();
+    const cv::Mat &D = calib.D();
+    const double fx = K.at<double>(0, 0), fy = K.at<double>(1, 1);
+    const double cx = K.at<double>(0, 2), cy = K.at<double>(1, 2);
+    const double k1 = D.at<double>(0), k2 = D.at<double>(1);
+    const double p1 = D.at<double>(2), p2 = D.at<double>(3);
+    const double k3 = D.total() >= 5 ? D.at<double>(4) : 0.0;
+
+    std::array<cv::Point2f, 4> out;
+    for (std::size_t i = 0; i < 4; ++i) {
+        const double x = (viewPts[i].x - cx) / fx;
+        const double y = (viewPts[i].y - cy) / fy;
+        const double r2 = x * x + y * y;
+        const double r4 = r2 * r2, r6 = r4 * r2;
+        const double radial = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
+        const double xd = x * radial + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x);
+        const double yd = y * radial + p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y;
+        out[i] = cv::Point2f(static_cast<float>(xd * fx + cx),
+                             static_cast<float>(yd * fy + cy));
+    }
+    return out;
+}
+
+void drawPoseAxes(cv::Mat &view, const CameraCalib &calib, const ArmorPose &pose)
+{
+    std::vector<cv::Point3f> axis{{0.f, 0.f, 0.f}, {100.f, 0.f, 0.f},
+                                  {0.f, 100.f, 0.f}, {0.f, 0.f, 100.f}};
+    std::vector<cv::Point2f> im;
+    cv::projectPoints(axis, pose.rvec, pose.tvec, calib.K(), calib.D(), im);
+    cv::line(view, im[0], im[1], cv::Scalar(0, 0, 255), 2);    // x 朝前 红
+    cv::line(view, im[0], im[2], cv::Scalar(0, 255, 0), 2);    // y 向左 绿
+    cv::line(view, im[0], im[3], cv::Scalar(255, 0, 0), 2);    // z 向上 蓝
+    cv::putText(view, "x=fwd y=left z=up", im[0] + cv::Point2f(10, -10),
+                cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 2);
+}
+
+void solveAndReport(const CameraCalib &calib, const std::array<cv::Point2f, 4> &corners,
+                    const ArmorSize &size, cv::Mat &view)
+{
+    auto pose = calib.solvePose(corners, size);
+    if (!pose) {
+        std::printf("  ❌ 解算失败：重投影超差/目标在相机后方？检查角点顺序（左下→左上→右上→右下）与装甲板尺寸\n");
+        return;
+    }
+    double yaw = 0.0, pitch = 0.0;
+    CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch);
+    std::printf("  距离 %.3f m | yaw %+.2f° | pitch %+.2f° | 重投影 %.3f px | tvec=(%.0f, %.0f, %.0f) mm\n",
+                pose->distance_mm / 1000.0, yaw * 180.0 / CV_PI, pitch * 180.0 / CV_PI,
+                pose->reproj_error_px, pose->tvec[0], pose->tvec[1], pose->tvec[2]);
+    drawPoseAxes(view, calib, *pose);
+}
+
+// ---------------------------------------------------------------
+// 合成自检：无需相机/显示器，验证坐标约定与解算闭环
+// ---------------------------------------------------------------
+int runSelftest(const CameraCalib &calib)
+{
+    std::printf("=== 合成自检 ===\n");
+    std::printf("用标定内参(含畸变)把已知位姿的装甲板投影成 2D 角点，再解回来对比。\n");
+    std::printf("图像 %dx%d | fx=%.2f fy=%.2f | k1=%.4f k2=%.4f\n\n",
+                calib.imageSize().width, calib.imageSize().height,
+                calib.K().at<double>(0, 0), calib.K().at<double>(1, 1),
+                calib.D().at<double>(0), calib.D().at<double>(1));
+
+    const ArmorSize size = ArmorSize::small();
+    const double W = size.width_mm, L = size.height_mm;
+
+    // 独立构造 3D 点（与 camera_calib.cpp 相同的顺序约定：
+    // x 朝前、y 向左、z 向上；左下→左上→右上→右下）
+    const std::vector<cv::Point3f> obj{
+        {0.f, static_cast<float>(W / 2), static_cast<float>(-L / 2)},   // 左下
+        {0.f, static_cast<float>(W / 2), static_cast<float>(L / 2)},    // 左上
+        {0.f, static_cast<float>(-W / 2), static_cast<float>(L / 2)},   // 右上
+        {0.f, static_cast<float>(-W / 2), static_cast<float>(-L / 2)},  // 右下
+    };
+
+    // 由 (yaw, pitch, 距离) 独立构造 tvec（sin/cos，避免与 poseToYawPitch 同源）
+    auto tvecOf = [](double yawDeg, double pitchDeg, double d) {
+        const double y = yawDeg * CV_PI / 180.0, p = pitchDeg * CV_PI / 180.0;
+        return cv::Vec3d(d * std::sin(y) * std::cos(p), -d * std::sin(p),
+                         d * std::cos(y) * std::cos(p));
+    };
+
+    // 真实朝向的 rvec：板子「面向相机」的基准姿态 R0 = Ry(90°)·Rx(90°)
+    // （板法线指向相机 -z，宽度沿相机 x，高度沿相机 -y），再叠加
+    // roll（绕板法线）、yaw（绕相机 y）、pitch（绕相机 x）
+    auto rvecOf = [](double rollDeg, double yawDeg, double pitchDeg) {
+        const double r = rollDeg * CV_PI / 180.0, y = yawDeg * CV_PI / 180.0,
+                     p = pitchDeg * CV_PI / 180.0;
+        cv::Mat R = (cv::Mat_<double>(3, 3) << 0, 0, 1, 0, 1, 0, -1, 0, 0);   // Ry(90°)
+        R = R * (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0);       // Rx(90°)
+        cv::Mat Rz = (cv::Mat_<double>(3, 3) << std::cos(r), -std::sin(r), 0,
+                      std::sin(r), std::cos(r), 0, 0, 0, 1);
+        cv::Mat Ry = (cv::Mat_<double>(3, 3) << std::cos(y), 0, std::sin(y), 0,
+                      1, 0, -std::sin(y), 0, std::cos(y));
+        cv::Mat Rx = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, std::cos(p),
+                      -std::sin(p), 0, std::sin(p), std::cos(p));
+        const cv::Mat M = Rx * Ry * Rz * R;
+        cv::Mat rv;
+        cv::Rodrigues(M, rv);
+        return cv::Vec3d(rv.at<double>(0), rv.at<double>(1), rv.at<double>(2));
+    };
+
+    struct Case {
+        const char *name;
+        cv::Vec3d tvec;      // mm
+        cv::Vec3d rvec;      // 弧度
+        cv::Vec3d expect;    // (距离m, yaw°, pitch°)
+        bool expectFail = false;
+    };
+
+    const std::vector<Case> cases{
+        {"正前方 1.5m", tvecOf(0, 0, 1500), rvecOf(0, 0, 0), {1.5, 0, 0}},
+        {"右偏 20°, 2.0m", tvecOf(20, 0, 2000), rvecOf(0, 20, 0), {2.0, 20, 0}},
+        {"上仰 10°, 2.5m", tvecOf(0, 10, 2500), rvecOf(0, 0, 10), {2.5, 0, 10}},
+        {"右偏 15° 上仰 8°, 1.8m", tvecOf(15, 8, 1800), rvecOf(0, 15, 8), {1.8, 15, 8}},
+        {"板内 roll 10°, 1.5m", tvecOf(0, 0, 1500), rvecOf(10, 0, 0), {1.5, 0, 0}},
+        // 交叉乱序（蝴蝶结）：单应矩阵把矩形映成自交四边形，重投影必爆，应被拒
+        {"【故意】交叉乱序", tvecOf(0, 0, 1500), rvecOf(0, 0, 0), {}, true},
+    };
+
+    bool allPass = true;
+    for (const auto &c : cases) {
+        std::vector<cv::Point2f> img;
+        cv::projectPoints(obj, c.rvec, c.tvec, calib.K(), calib.D(), img);
+        std::array<cv::Point2f, 4> corners{img[0], img[1], img[2], img[3]};
+        if (c.expectFail) {
+            // 交叉乱序：左下→右上→左上→右下（自交蝴蝶结，非刚体可达）
+            corners = {img[0], img[2], img[1], img[3]};
+        }
+
+        auto pose = calib.solvePose(corners, size);
+        bool pass = false;
+        if (c.expectFail) {
+            pass = !pose.has_value();
+            std::printf("%-28s %s\n", c.name,
+                        pass ? "解算被拒（符合预期）✅" : "解算未被拒（不应通过）❌");
+        } else if (pose) {
+            double yaw, pitch;
+            CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch);
+            const double dErr = std::fabs(pose->distance_mm / 1000.0 - c.expect[0]);
+            const double yErr = std::fabs(yaw * 180.0 / CV_PI - c.expect[1]);
+            const double pErr = std::fabs(pitch * 180.0 / CV_PI - c.expect[2]);
+            pass = dErr < 0.01 * c.expect[0] && yErr < 0.5 && pErr < 0.5;
+            std::printf("%-28s 距离 %.3fm(真 %.3f) | yaw %+.2f°(真 %+.2f) | pitch %+.2f°(真 %+.2f) | 重投影 %.4fpx %s\n",
+                        c.name, pose->distance_mm / 1000.0, c.expect[0],
+                        yaw * 180.0 / CV_PI, c.expect[1], pitch * 180.0 / CV_PI,
+                        c.expect[2], pose->reproj_error_px, pass ? "✅" : "❌");
+        } else {
+            std::printf("%-28s 解算失败 ❌（不应失败）\n", c.name);
+        }
+        allPass = allPass && pass;
+    }
+
+    std::printf("\n=== 自检%s ===\n", allPass ? "全部通过 ✅" : "有失败项 ❌");
+    return allPass ? 0 : 1;
+}
+
+}   // namespace
+
+int main(int argc, char **argv)
+{
+    Options opt;
+    bool bigArmor = false;
+    if (!parseArgs(argc, argv, opt, bigArmor)) {
+        printUsage();
+        return 2;
+    }
+
+    CameraCalib calib(opt.calib_path);
+    if (!calib.valid()) return -1;
+
+    const bool hasCustom = opt.custom_w > 0.0 && opt.custom_l > 0.0;
+    const ArmorSize size = hasCustom ? ArmorSize{opt.custom_w, opt.custom_l}
+                                     : (bigArmor ? ArmorSize::big() : ArmorSize::small());
+
+    if (opt.selftest) {
+        return runSelftest(calib);
+    }
+
+    // ---------- 纯角点计算模式（无窗口，脚本/沙箱可用） ----------
+    if (opt.corners.size() == 4) {
+        std::array<cv::Point2f, 4> corners{opt.corners[0], opt.corners[1],
+                                           opt.corners[2], opt.corners[3]};
+        cv::Mat dummy;
+        solveAndReport(calib, corners, size, dummy);
+        if (!opt.image_path.empty()) {
+            cv::Mat img = cv::imread(opt.image_path);
+            if (!img.empty()) {
+                for (int i = 0; i < 4; ++i)
+                    cv::circle(img, opt.corners[i], 5, cv::Scalar(0, 255, 255), -1);
+                cv::imwrite("pose_result.jpg", img);
+                std::printf("已画点存为 pose_result.jpg\n");
+            }
+        }
+        return 0;
+    }
+
+    // ---------- 交互模式 ----------
+    const bool useCamera = opt.image_path.empty();
+    CameraGrabber cam;
+    cv::Mat raw;
+    ClickState st;
+
+    if (useCamera) {
+        if (!cam.initializeCamera()) {
+            std::fprintf(stderr, "相机初始化失败\n");
+            return -1;
+        }
+        applyRuntimeParams(opt.runtime_path, cam);
+        if (!cam.startCamera()) {
+            std::fprintf(stderr, "相机取流失败\n");
+            return -1;
+        }
+    } else {
+        raw = cv::imread(opt.image_path);
+        if (raw.empty()) {
+            std::fprintf(stderr, "打不开图片: %s\n", opt.image_path.c_str());
+            return -1;
+        }
+    }
+
+    std::printf("装甲板: %s (%.0f x %.0f mm)\n", bigArmor ? "大 230mm" : "小 135mm",
+                size.width_mm, size.height_mm);
+    std::printf("请按顺序点 4 个角: 左下 → 左上 → 右上 → 右下\n");
+    std::printf("r=清除 b=切换大小装甲 u=切换去畸变显示 s=截图 q=退出\n");
+
+    cv::namedWindow("pose_test", cv::WINDOW_NORMAL);
+    cv::setMouseCallback("pose_test", onMouse, &st);
+
+    bool showUndistort = false;
+    int frameIdx = 0;
+
+    while (true) {
+        if (useCamera && !cam.getFrame(raw)) {
+            std::printf("取帧失败，退出\n");
+            break;
+        }
+        cv::Mat view;
+        if (showUndistort) calib.undistort(raw, view);
+        else raw.copyTo(view);
+
+        drawClicks(view, st);
+        cv::putText(view, "1:LB 2:LT 3:RT 4:RB | r=reset b=size u=undistort s=save q=quit",
+                    cv::Point(10, 28), cv::FONT_HERSHEY_SIMPLEX, 0.6,
+                    cv::Scalar(0, 255, 0), 2);
+
+        if (st.pts.size() == 4 && st.dirty) {
+            std::array<cv::Point2f, 4> vp{st.pts[0], st.pts[1], st.pts[2], st.pts[3]};
+            solveAndReport(calib, toOriginalCorners(calib, vp, showUndistort),
+                           size, view);
+            st.dirty = false;
+        }
+
+        cv::imshow("pose_test", view);
+        const int key = cv::waitKey(5) & 0xFF;
+        if (key == 27 || key == 'q') break;
+        if (key == 'r') {
+            st.pts.clear();
+            st.dirty = true;
+            std::printf("已清除，重新点 4 个角\n");
+        }
+        if (key == 'b') {
+            bigArmor = !bigArmor;
+            st.dirty = true;
+            const ArmorSize ns = bigArmor ? ArmorSize::big() : ArmorSize::small();
+            std::printf("切换到 %s (%.0f x %.0f mm)\n", bigArmor ? "大装甲板" : "小装甲板",
+                        ns.width_mm, ns.height_mm);
+        }
+        if (key == 'u') {
+            showUndistort = !showUndistort;
+            st.pts.clear();
+            st.dirty = true;
+            std::printf("显示切换为 %s（坐标系变了，角点已清空）\n",
+                        showUndistort ? "去畸变图" : "原图");
+        }
+        if (key == 's') {
+            char name[64];
+            std::snprintf(name, sizeof(name), "pose_%04d.jpg", frameIdx);
+            cv::imwrite(name, view);
+            std::printf("已保存 %s\n", name);
+        }
+        ++frameIdx;
+    }
+
+    if (useCamera) cam.stopCamera();
+    return 0;
+}

xmake.lua

@@ -29,6 +29,7 @@ target("27RM")
     add_files("src/*.cpp")
     remove_files("src/capture_calib.cpp")
     remove_files("src/calibrate.cpp")
+    remove_files("src/pose_test.cpp")
 
 -- 标定图采集工具
 --   ./build/linux/x86_64/debug/capture_calib              （MindVision 相机）
@@ -43,3 +44,12 @@ target("calibrate")
     add_rules("rm_deps")
     set_kind("binary")
     add_files("src/calibrate.cpp")
+
+-- 几何链路验证工具（4 角点 → PnP → 距离/yaw/pitch）
+--   ./build/linux/x86_64/debug/pose_test              （相机 + 鼠标点角）
+--   ./build/linux/x86_64/debug/pose_test --selftest   （合成自检，无需相机）
+--   ./build/linux/x86_64/debug/pose_test --image x.jpg --corners x1,y1 x2,y2 x3,y3 x4,y4
+target("pose_test")
+    add_rules("rm_deps")
+    set_kind("binary")
+    add_files("src/pose_test.cpp", "src/camera_calib.cpp", "src/camera_set.cpp")