diff --git a/README.md b/README.md index 36a3580..03bbf36 100644 --- a/README.md +++ b/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**（卡尺实测值，不是打印标称值） diff --git a/configs/camera_runtime.yaml b/configs/camera_runtime.yaml new file mode 100644 index 0000000..8100111 --- /dev/null +++ b/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=高速 diff --git a/inc/camera_calib.hpp b/inc/camera_calib.hpp index 57571bc..15aa80b 100644 --- a/inc/camera_calib.hpp +++ b/inc/camera_calib.hpp @@ -72,10 +72,12 @@ public: /** * 四个角点 → 装甲板位姿 - * @param corners 图像坐标，顺序必须是：左下、左上、右上、右下 - * （与 jlu_vision 的 ArmorPoints.hpp 3D 点顺序一致） + * 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 → + * 依次筛掉「z<=0（相机后方）」「重投影超差」→ 取重投影最小者。 + * 注：角点在解算前先做去畸变（IPPE+畸变+板内旋转组合有数值坑）。 + * @param corners 图像坐标（原图，含畸变），顺序：左下、左上、右上、右下 * @param size 装甲板物理尺寸 - * @return 成功返回位姿；无解或重投影误差过大返回 std::nullopt + * @return 成功返回位姿；全部候选被筛掉时返回 std::nullopt */ std::optional solvePose(const std::array &corners, const ArmorSize &size) const; diff --git a/inc/camera_set.hpp b/inc/camera_set.hpp index 7fffda0..00fed07 100644 --- a/inc/camera_set.hpp +++ b/inc/camera_set.hpp @@ -14,12 +14,14 @@ public: bool initializeCamera(); // 初始化相机 bool startCamera(); // 启动相机 bool stopCamera(); // 停止相机 - // bool setExposureTime(double exposureTime); // 设置曝光时间 - // bool setGain(double gain); // 设置增益 - // bool setWhiteBalance(double redGain, double blueGain); // 设置白平衡 - // bool setResolution(int width, int height); // 设置分辨率 - // bool setFrameRate(double frameRate); // 设置帧率 - bool getFrame(cv::Mat &frame); // 获取一帧图像 + // ---- 参数设置：在 initializeCamera 之后、startCamera 之前调用 ---- + bool setAeState(bool aeOn); // 曝光模式：true=自动, false=手动（比赛必须手动） + bool setExposureTime(double exposureUs); // 曝光时间（微秒 us） + bool setAnalogGainRaw(int gainRaw); // 模拟增益（SDK 原始整数，范围见 capability.sExposeDesc） + bool setAnalogGainTimes(double gainTimes); // 模拟增益（倍率，内部按 fAnalogGainStep 换算成原始值） + bool setWbMode(bool autoWb); // 白平衡：true=自动, false=手动 + bool setFrameSpeed(int speed); // 帧率档位：0=低速 1=正常 2=高速 + bool getFrame(cv::Mat &frame); // 获取一帧图像 private: CameraHandle hCamera = -1; // 相机句柄 diff --git a/src/camera_calib.cpp b/src/camera_calib.cpp index 34bc992..50e5ac9 100644 --- a/src/camera_calib.cpp +++ b/src/camera_calib.cpp @@ -65,26 +65,66 @@ std::optional CameraCalib::solvePose( const std::vector img_points{corners[0], corners[1], corners[2], corners[3]}; + // ⚠️ 先把角点去畸变到理想像素坐标，再用 IPPE（D 传空）。 + // 实测：IPPE + 畸变系数 + 板内旋转 三者组合会解出垃圾解 + // （OpenCV 4.x/5.x 都有这坑），去畸变后数值稳定。 + std::vector ideal; + cv::undistortPoints(img_points, ideal, camera_matrix_, dist_coeffs_, + cv::noArray(), camera_matrix_); + + // 平面靶标有镜像歧义：IPPE 会给出两个候选解， + // solvePnPGeneric 一次性全部取回，逐个筛。 + std::vector 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; + } + + int best = -1; + double best_err = 1e18; + std::vector proj; + 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(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 k = 0; k < proj.size(); ++k) { + err += cv::norm(proj[k] - img_points[k]); + } + err /= static_cast(proj.size()); + if (err > max_reproj_err_px_) { + continue; + } + + if (err < best_err) { + best_err = err; + best = i; + } + } + + if (best < 0) { + return std::nullopt; + } + ArmorPose pose; - if (!cv::solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, - pose.rvec, pose.tvec, false, cv::SOLVEPNP_IPPE)) { - return std::nullopt; - } - - // 自检：把 3D 点投影回图像，误差过大说明解不可信（例如角点顺序错、板子不是这个尺寸） - std::vector reproj; - cv::projectPoints(obj_points, pose.rvec, pose.tvec, camera_matrix_, - dist_coeffs_, reproj); - double err = 0.0; - for (std::size_t i = 0; i < reproj.size(); ++i) { - err += cv::norm(reproj[i] - img_points[i]); - } - err /= static_cast(reproj.size()); - pose.reproj_error_px = err; - if (err > max_reproj_err_px_) { - return std::nullopt; - } - + const cv::Mat &rv = rvecs[best]; + const cv::Mat &tv = tvecs[best]; + pose.rvec = cv::Vec3d(rv.at(0), rv.at(1), rv.at(2)); + pose.tvec = cv::Vec3d(tv.at(0), tv.at(1), tv.at(2)); + pose.reproj_error_px = best_err; pose.distance_mm = cv::norm(pose.tvec); return pose; } diff --git a/src/camera_set.cpp b/src/camera_set.cpp index 0ed5854..0b724d8 100644 --- a/src/camera_set.cpp +++ b/src/camera_set.cpp @@ -1,6 +1,7 @@ #include "camera_set.hpp" #include #include "CameraApi.h" +#include #include 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) { // 前者条件检查 diff --git a/src/pose_test.cpp b/src/pose_test.cpp new file mode 100644 index 0000000..fdd38da --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include + +namespace { + +// --------------------------------------------------------------- +// 命令行选项 +// --------------------------------------------------------------- +struct Options { + std::string image_path; + std::string calib_path = "configs/camera.yaml"; + std::string runtime_path = "configs/camera_runtime.yaml"; + std::vector 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 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(param); + if (st->pts.size() >= 4) return; + st->pts.push_back(cv::Point2f(static_cast(x), static_cast(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(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 toOriginalCorners(const CameraCalib &calib, + const std::array &viewPts, + bool viewIsUndistorted) +{ + if (!viewIsUndistorted) return viewPts; + + const cv::Mat &K = calib.K(); + const cv::Mat &D = calib.D(); + const double fx = K.at(0, 0), fy = K.at(1, 1); + const double cx = K.at(0, 2), cy = K.at(1, 2); + const double k1 = D.at(0), k2 = D.at(1); + const double p1 = D.at(2), p2 = D.at(3); + const double k3 = D.total() >= 5 ? D.at(4) : 0.0; + + std::array 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(xd * fx + cx), + static_cast(yd * fy + cy)); + } + return out; +} + +void drawPoseAxes(cv::Mat &view, const CameraCalib &calib, const ArmorPose &pose) +{ + std::vector 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 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 &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(0, 0), calib.K().at(1, 1), + calib.D().at(0), calib.D().at(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 obj{ + {0.f, static_cast(W / 2), static_cast(-L / 2)}, // 左下 + {0.f, static_cast(W / 2), static_cast(L / 2)}, // 左上 + {0.f, static_cast(-W / 2), static_cast(L / 2)}, // 右上 + {0.f, static_cast(-W / 2), static_cast(-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_(3, 3) << 0, 0, 1, 0, 1, 0, -1, 0, 0); // Ry(90°) + R = R * (cv::Mat_(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); // Rx(90°) + cv::Mat Rz = (cv::Mat_(3, 3) << std::cos(r), -std::sin(r), 0, + std::sin(r), std::cos(r), 0, 0, 0, 1); + cv::Mat Ry = (cv::Mat_(3, 3) << std::cos(y), 0, std::sin(y), 0, + 1, 0, -std::sin(y), 0, std::cos(y)); + cv::Mat Rx = (cv::Mat_(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(0), rv.at(1), rv.at(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 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 img; + cv::projectPoints(obj, c.rvec, c.tvec, calib.K(), calib.D(), img); + std::array 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 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 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; +} diff --git a/xmake.lua b/xmake.lua index 3c0828c..581dc05 100644 --- a/xmake.lua +++ b/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")