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
This commit is contained in:
+39
-8
@@ -1,13 +1,44 @@
|
||||
# Xmake cache
|
||||
# ============================================================
|
||||
# 27RM 提交规则
|
||||
# 入库：源码 / 构建脚本 / 配置文件 / 文档
|
||||
# 不入库：图片视频等大数据、编译产物、本机专属生成物
|
||||
# ============================================================
|
||||
|
||||
# ---------- 构建产物与缓存 ----------
|
||||
.xmake/
|
||||
build/
|
||||
|
||||
# MacOS Cache
|
||||
.DS_Store
|
||||
|
||||
|
||||
.vscode/.cache/
|
||||
|
||||
# wget 下载日志
|
||||
wget.log
|
||||
# ---------- 标定照片与视频：一律不入库 ----------
|
||||
# 单个标定集 20MB 起，且随时重拍，没有任何入库价值
|
||||
calib_images/
|
||||
calib_all/
|
||||
*.jpg
|
||||
*.jpeg
|
||||
*.png
|
||||
*.bmp
|
||||
*.tif
|
||||
*.tiff
|
||||
*.webp
|
||||
*.avi
|
||||
*.mp4
|
||||
*.mkv
|
||||
*.mov
|
||||
*.pcd
|
||||
*.bag
|
||||
# 若确有少量配图需要提交（例如 README 插图），用强制添加：
|
||||
# git add -f docs/xxx.png
|
||||
|
||||
# ---------- 标定结果必须入库 ----------
|
||||
# configs/camera.yaml 是相机内参，全队共用，务必提交（它不在上面的规则里）
|
||||
|
||||
# ---------- 日志（wget 下载日志 / 相机 SDK 运行时日志） ----------
|
||||
*.log
|
||||
wget.log
|
||||
wget-log*
|
||||
Camera/
|
||||
|
||||
# ---------- 编辑器 / 系统 ----------
|
||||
.DS_Store
|
||||
# xmake 生成，内容是本机绝对路径，换台电脑即失效
|
||||
.vscode/compile_commands.json
|
||||
|
||||
Vendored
-11
@@ -1,11 +0,0 @@
|
||||
[
|
||||
{
|
||||
"directory": "/home/mzxiang/opencv_project/27RM",
|
||||
"arguments": ["/usr/bin/g++", "-c", "-m64", "-g", "-O0", "-std=c++11", "-I/home/mzxiang/opencv_project/27RM/inc", "-I/usr/local/include/opencv4", "-I/home/mzxiang/MindVisionSDK/include", "-o", "build/.objs/27RM/linux/x86_64/debug/src/main.cpp.o", "src/main.cpp"],
|
||||
"file": "src/main.cpp"
|
||||
},
|
||||
{
|
||||
"directory": "/home/mzxiang/opencv_project/27RM",
|
||||
"arguments": ["/usr/bin/g++", "-c", "-m64", "-g", "-O0", "-std=c++11", "-I/home/mzxiang/opencv_project/27RM/inc", "-I/usr/local/include/opencv4", "-I/home/mzxiang/MindVisionSDK/include", "-o", "build/.objs/27RM/linux/x86_64/debug/src/camera_set.cpp.o", "src/camera_set.cpp"],
|
||||
"file": "src/camera_set.cpp"
|
||||
}]
|
||||
@@ -0,0 +1,131 @@
|
||||
# 27RM
|
||||
|
||||
RoboMaster 视觉工程（MindVision 相机 + OpenCV）。
|
||||
|
||||
## 构建
|
||||
|
||||
```bash
|
||||
xmake # 产出两个程序（见下）
|
||||
xmake run 27RM # 跑主程序
|
||||
```
|
||||
|
||||
## 程序清单
|
||||
|
||||
| 程序 | 用途 |
|
||||
|---|---|
|
||||
| `27RM` | 自瞄主程序 |
|
||||
| `capture_calib` | **相机标定图采集工具** |
|
||||
| `calibrate` | **标定计算工具**（照片 → 内参/畸变 → `configs/camera.yaml`） |
|
||||
|
||||
## 配置
|
||||
|
||||
```
|
||||
configs/
|
||||
└── camera.yaml # 相机内参 + 畸变 + 标定元信息（由 calibrate 生成，需入库）
|
||||
```
|
||||
|
||||
代码里这样用（`inc/camera_calib.hpp`）：
|
||||
|
||||
```cpp
|
||||
#include "camera_calib.hpp"
|
||||
|
||||
CameraCalib calib;
|
||||
if (!calib.load("configs/camera.yaml")) return -1; // 启动时加载一次
|
||||
|
||||
cv::Mat undist;
|
||||
calib.undistort(frame, undist); // 每帧去畸变
|
||||
|
||||
// 装甲板四角点（左下、左上、右上、右下）→ 位姿
|
||||
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);
|
||||
// pose->distance_mm / pose->reproj_error_px 可用于过滤坏帧
|
||||
}
|
||||
```
|
||||
|
||||
> 程序必须在工程根目录运行（`configs/` 是相对路径），或自行传绝对路径。
|
||||
|
||||
## 相机标定流程
|
||||
|
||||
> **本工程标定板规格**：10 × 7 内角点、**方格 20.0 mm**（卡尺实测值，不是打印标称值）
|
||||
> **相机**：MV-SUA131GC（1/3" CMOS，3.75μm 像元）+ MV-LD-M12-2.8MM-C 镜头，输出 1280×1024
|
||||
> ⚠️ M12 镜头**调焦会改变有效焦距**，标定和使用时必须保持对焦环位置一致
|
||||
|
||||
### 1. 采集标定图
|
||||
|
||||
```bash
|
||||
# 用 MindVision 相机（机器人上那台）
|
||||
./build/linux/x86_64/debug/capture_calib --square 20.0
|
||||
|
||||
# 或改用 USB 摄像头
|
||||
./build/linux/x86_64/debug/capture_calib --usb 0 --square 20.0
|
||||
```
|
||||
|
||||
按键：
|
||||
|
||||
| 键 | 功能 |
|
||||
|---|---|
|
||||
| 空格 | 保存当前帧（默认要求检测到棋盘格） |
|
||||
| `f` | 强制保存（未检测到也存） |
|
||||
| `d` | 删除上一张 |
|
||||
| `c` | 开关实时角点检测显示 |
|
||||
| `g` | 灰度预览（排查过曝/反光） |
|
||||
| `q` / ESC | 退出 |
|
||||
|
||||
拍摄要求：
|
||||
|
||||
- 标定板出现在画面**中心、四角、边缘**（2.8mm 广角镜头边缘畸变最大）
|
||||
- 姿态多样：正对、倾斜、旋转、俯仰
|
||||
- 清晰不糊、光照均匀、无反光、板子贴平硬板
|
||||
- **20~30 张**，保存于 `calib_images/`（已加入 .gitignore，不会入库）
|
||||
|
||||
### 2. 计算内参
|
||||
|
||||
```bash
|
||||
./build/linux/x86_64/debug/calibrate --dir calib_images --square 20.0 # 只算顶层
|
||||
./build/linux/x86_64/debug/calibrate --dir calib_all --square 20.0 # 全部 31 张（推荐）
|
||||
./build/linux/x86_64/debug/calibrate --square 20.0 --show # 逐张查看角点与重投影
|
||||
```
|
||||
|
||||
工具会输出重投影误差 RMS、内参 K、畸变系数、每张图误差排行，并写入 `camera_calib.yml`。
|
||||
|
||||
| 重投影误差 | 评价 |
|
||||
|---|---|
|
||||
| < 0.3 px | 很好 |
|
||||
| 0.3 ~ 0.5 px | 可接受 |
|
||||
| > 0.5 px | 建议重拍 |
|
||||
|
||||
> **当前标定结果**（`calib_all/` 31 张，20mm 方格）：
|
||||
> fx=657.47, fy=658.25, cx=625.64, cy=509.00，RMS **0.483 px**
|
||||
> **距离验证**：卷尺 700mm vs 程序 687~697mm，误差 **< 2%** ✓
|
||||
|
||||
### ⚠️ 标定图必须同时具备「三种多样性」
|
||||
|
||||
| 维度 | 作用 | 不满足的后果 |
|
||||
|---|---|---|
|
||||
| **画面位置**（九宫格覆盖） | 约束**畸变系数** | 边缘畸变解不出来，RMS 偏高 |
|
||||
| **倾角**（正对/上下倾/左右倾/大角度） | 约束**焦距与主点** | 参数耦合，精度下降 |
|
||||
| **距离**（近/中/远，板子占画面 40%/20%/8%） | 约束**尺度** ⭐ | **fx 无法确定**：所有图同距离时，"焦距×距离"可整体缩放而图像不变，优化器会落到错误值，**且 RMS 依然很低**（最阴险的坑） |
|
||||
|
||||
> 实测教训：一套 19 张、覆盖 9/9 但**距离相近**的图，标出的 fx=883（实际 659，偏差 34%），距离误差直接 +32%；
|
||||
> 与旧的 12 张（距离 24~65cm）合并后 fx 回到 657，误差 <2%。**距离多样性是 fx 的锚点，不可省。**
|
||||
|
||||
### 3. 使用内参
|
||||
|
||||
```cpp
|
||||
cv::Mat K, D;
|
||||
cv::FileStorage fs("camera_calib.yml", cv::FileStorage::READ);
|
||||
fs["camera_matrix"] >> K;
|
||||
fs["dist_coeffs"] >> D;
|
||||
```
|
||||
|
||||
之后用 `cv::undistort` / `cv::solvePnP` 做去畸变与位姿解算。
|
||||
|
||||
### 4. 距离验证（改完参数一定要做）
|
||||
|
||||
```bash
|
||||
~/yolo-env/bin/python ~/rm_vision/verify_distance.py 图片.jpg --square 20.0 --distance 647
|
||||
```
|
||||
|
||||
用卷尺量出真实距离，与程序算出的距离对比：误差 < 2% 说明标定可用。
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
%YAML:1.0
|
||||
---
|
||||
# ============================================================
|
||||
# 相机标定与内参配置（27RM）
|
||||
# 本文件由 ./build/.../calibrate 生成，也可手工维护
|
||||
# ⚠️ M12 镜头的【对焦环位置】会改变有效焦距：
|
||||
# 一旦调焦，必须重新标定并重新做卷尺距离验证！
|
||||
# ============================================================
|
||||
camera_name: "MV-SUA131GC"
|
||||
lens: "MV-LD-M12-2.8MM-C"
|
||||
sensor: "1/3\" CMOS, 3.75um pixel"
|
||||
resolution: "1280x1024"
|
||||
calib_date: "2026-09-15"
|
||||
board_spec: "10x7 inner corners, square=20.0mm"
|
||||
calib_images: "31 (old_run 12 + new 19)"
|
||||
distance_verify: "ruler 700mm -> computed 687~697mm, error <2%"
|
||||
notes: "refocus invalidates this calibration; must re-verify with ruler"
|
||||
|
||||
camera_matrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 657.47392591548282, 0., 625.64119358041364, 0.,
|
||||
658.25468859263844, 509.00483710801581, 0., 0., 1. ]
|
||||
dist_coeffs: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 5
|
||||
dt: d
|
||||
data: [ -0.0012152502343767638, -0.058463444062878803,
|
||||
-0.0022603001279860352, 0.0018169600148664874,
|
||||
0.040233970225725772 ]
|
||||
image_width: 1280
|
||||
image_height: 1024
|
||||
square_mm: 20.
|
||||
board_cols: 10
|
||||
board_rows: 7
|
||||
rms: 0.48277734002307909
|
||||
@@ -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
|
||||
+5
-5
@@ -14,11 +14,11 @@ 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 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); // 获取一帧图像
|
||||
|
||||
private:
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
#ifndef LIGHT_BAR_HPP
|
||||
#define LIGHT_BAR_HPP
|
||||
|
||||
class LightBar
|
||||
{
|
||||
public:
|
||||
LightBar(); // 构造函数声明
|
||||
~LightBar(); // 析构函数声明
|
||||
|
||||
|
||||
private:
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,321 @@
|
||||
/**
|
||||
* @file calibrate.cpp
|
||||
* @brief 相机标定计算工具（27RM 项目）
|
||||
*
|
||||
* 读取 calib_images/ 里的棋盘格照片，用 calibrateCamera 计算内参与畸变，
|
||||
* 结果保存为 configs/camera.yaml。
|
||||
*
|
||||
* 运行：
|
||||
* ./build/linux/x86_64/debug/calibrate # 默认 10x7 内角点 15mm
|
||||
* ./build/linux/x86_64/debug/calibrate --cols 11 --rows 8 # 其它规格的板
|
||||
* ./build/linux/x86_64/debug/calibrate --show # 逐张显示角点与重投影
|
||||
*
|
||||
* 输出：
|
||||
* - 重投影误差 RMS（<0.3 很好，0.3~0.5 可用，>0.5 建议删掉差图重拍）
|
||||
* - 内参矩阵 K、畸变系数
|
||||
* - 每张图的误差排行（最差的几张优先考虑删除）
|
||||
* - camera_calib.yml（后续去畸变 / PnP 用）
|
||||
*/
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <dirent.h>
|
||||
#include <sys/stat.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
struct Options {
|
||||
int board_cols = 10; // 内角点列数（10x7 对应 11x8 方格板）
|
||||
int board_rows = 7;
|
||||
float square_mm = 20.0f; // 方格边长 mm（本工程实测 20.0）
|
||||
std::string dir = "calib_images";
|
||||
std::string out = "configs/camera.yaml";
|
||||
bool show = false;
|
||||
};
|
||||
|
||||
void printUsage(const char *prog) {
|
||||
std::printf(
|
||||
"用法: %s [选项]\n"
|
||||
" --cols N 内角点列数（默认 10）\n"
|
||||
" --rows N 内角点行数（默认 7）\n"
|
||||
" --square F 方格边长 mm（默认 20.0，本工程实测值）\n"
|
||||
" --dir PATH 标定图目录（默认 calib_images）\n"
|
||||
" --out FILE 输出文件（默认 configs/camera.yaml）\n"
|
||||
" --show 逐张显示角点与重投影（调试用）\n",
|
||||
prog);
|
||||
}
|
||||
|
||||
bool parseArgs(int argc, char **argv, Options &opt) {
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (!std::strcmp(argv[i], "--cols") && i + 1 < argc) {
|
||||
opt.board_cols = std::atoi(argv[++i]);
|
||||
} else if (!std::strcmp(argv[i], "--rows") && i + 1 < argc) {
|
||||
opt.board_rows = std::atoi(argv[++i]);
|
||||
} else if (!std::strcmp(argv[i], "--square") && i + 1 < argc) {
|
||||
opt.square_mm = static_cast<float>(std::atof(argv[++i]));
|
||||
} else if (!std::strcmp(argv[i], "--dir") && i + 1 < argc) {
|
||||
opt.dir = argv[++i];
|
||||
} else if (!std::strcmp(argv[i], "--out") && i + 1 < argc) {
|
||||
opt.out = argv[++i];
|
||||
} else if (!std::strcmp(argv[i], "--show")) {
|
||||
opt.show = true;
|
||||
} else {
|
||||
printUsage(argv[0]);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
struct ImageInfo {
|
||||
std::string name;
|
||||
double error_px; // 该图平均重投影误差
|
||||
int index;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
Options opt;
|
||||
if (!parseArgs(argc, argv, opt)) {
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
// ---------------- 读图 ----------------
|
||||
std::vector<cv::String> files;
|
||||
cv::glob(opt.dir + "/*.jpg", files);
|
||||
if (files.empty()) {
|
||||
std::fprintf(stderr, "错误: %s/ 里没有 jpg 图片\n", opt.dir.c_str());
|
||||
// 子目录里若有图片，给出可直接复制的命令（例如 old_run/）
|
||||
DIR *dp = opendir(opt.dir.c_str());
|
||||
if (dp != nullptr) {
|
||||
std::vector<std::string> hints;
|
||||
struct dirent *entry;
|
||||
while ((entry = readdir(dp)) != nullptr) {
|
||||
if (entry->d_name[0] == '.') {
|
||||
continue;
|
||||
}
|
||||
std::string sub = opt.dir + "/" + entry->d_name;
|
||||
struct stat st;
|
||||
if (stat(sub.c_str(), &st) != 0 || !S_ISDIR(st.st_mode)) {
|
||||
continue;
|
||||
}
|
||||
std::vector<cv::String> sub_files;
|
||||
cv::glob(sub + "/*.jpg", sub_files);
|
||||
if (!sub_files.empty()) {
|
||||
hints.push_back(sub + " (" +
|
||||
std::to_string(sub_files.size()) + " 张)");
|
||||
}
|
||||
}
|
||||
closedir(dp);
|
||||
if (!hints.empty()) {
|
||||
std::fprintf(stderr, "提示: 子目录里有图片，可以这样用：\n");
|
||||
for (const auto &h : hints) {
|
||||
std::fprintf(stderr, " ./calibrate --dir %.*s --square %.1f\n",
|
||||
static_cast<int>(h.find(" (")), h.c_str(),
|
||||
opt.square_mm);
|
||||
}
|
||||
}
|
||||
}
|
||||
std::fprintf(stderr,
|
||||
"或者运行 capture_calib 采集新图（目标 20~30 张）\n");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
std::printf("找到 %zu 张图片\n", files.size());
|
||||
|
||||
// ---------------- 逐张检测角点 ----------------
|
||||
// 候选尺寸：配置值 ±1（兼容"方格数"与"内角点数"的混淆）
|
||||
std::vector<cv::Size> candidates{
|
||||
cv::Size(opt.board_cols, opt.board_rows),
|
||||
cv::Size(std::max(3, opt.board_cols - 1), std::max(3, opt.board_rows - 1)),
|
||||
cv::Size(opt.board_cols + 1, opt.board_rows + 1),
|
||||
};
|
||||
|
||||
cv::Size used_size(0, 0);
|
||||
cv::Size image_size(0, 0);
|
||||
std::vector<std::vector<cv::Point3f>> object_points;
|
||||
std::vector<std::vector<cv::Point2f>> image_points;
|
||||
std::vector<std::string> used_names;
|
||||
std::vector<std::string> failed_names;
|
||||
|
||||
for (const auto &file : files) {
|
||||
cv::Mat img = cv::imread(file);
|
||||
if (img.empty()) {
|
||||
failed_names.emplace_back(file);
|
||||
continue;
|
||||
}
|
||||
if (image_size.width == 0) {
|
||||
image_size = img.size();
|
||||
}
|
||||
cv::Mat gray;
|
||||
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
|
||||
|
||||
bool found = false;
|
||||
cv::Size detected(0, 0);
|
||||
std::vector<cv::Point2f> corners;
|
||||
for (const auto &sz : candidates) {
|
||||
corners.clear();
|
||||
found = cv::findChessboardCornersSB(
|
||||
gray, sz, corners, cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_LARGER);
|
||||
if (!found) {
|
||||
found = cv::findChessboardCorners(
|
||||
gray, sz, corners,
|
||||
cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
|
||||
}
|
||||
if (found) {
|
||||
detected = sz;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
failed_names.emplace_back(file);
|
||||
continue;
|
||||
}
|
||||
|
||||
// 第一张成功图决定实际尺寸，后续图尺寸不一致则丢弃
|
||||
if (used_size.width == 0) {
|
||||
used_size = detected;
|
||||
std::printf("标定板实际尺寸: %d x %d 内角点\n", used_size.width,
|
||||
used_size.height);
|
||||
if (used_size != candidates.front()) {
|
||||
std::printf(" 注意: 与配置 %d x %d 不一致（你的板子可能是 %d x "
|
||||
"%d 个方格）\n",
|
||||
opt.board_cols, opt.board_rows,
|
||||
used_size.width + 1, used_size.height + 1);
|
||||
}
|
||||
}
|
||||
if (detected != used_size) {
|
||||
failed_names.emplace_back(file + " (尺寸不一致)");
|
||||
continue;
|
||||
}
|
||||
|
||||
cv::cornerSubPix(
|
||||
gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
|
||||
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER,
|
||||
30, 0.001));
|
||||
|
||||
// 世界坐标：z=0 平面，方格为实测边长
|
||||
std::vector<cv::Point3f> objp;
|
||||
for (int r = 0; r < used_size.height; ++r) {
|
||||
for (int c = 0; c < used_size.width; ++c) {
|
||||
objp.emplace_back(c * opt.square_mm, r * opt.square_mm, 0.f);
|
||||
}
|
||||
}
|
||||
object_points.push_back(objp);
|
||||
image_points.push_back(corners);
|
||||
used_names.push_back(file);
|
||||
}
|
||||
|
||||
std::printf("可用图片: %zu / %zu\n", image_points.size(), files.size());
|
||||
if (!failed_names.empty()) {
|
||||
std::printf("未使用的图片:\n");
|
||||
for (const auto &n : failed_names) {
|
||||
std::printf(" %s\n", n.c_str());
|
||||
}
|
||||
}
|
||||
if (image_points.size() < 8) {
|
||||
std::fprintf(stderr,
|
||||
"错误: 可用图片不足 8 张\n"
|
||||
" · 检查拍摄条件（整板入画/无反光/不模糊）\n"
|
||||
" · 或确认 --cols/--rows 参数\n");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
// ---------------- 标定 ----------------
|
||||
cv::Mat K, D;
|
||||
std::vector<cv::Mat> rvecs, tvecs;
|
||||
double rms = cv::calibrateCamera(object_points, image_points, image_size, K,
|
||||
D, rvecs, tvecs);
|
||||
std::printf("图像尺寸: %d x %d\n", image_size.width, image_size.height);
|
||||
|
||||
std::printf("\n==============================\n");
|
||||
std::printf("重投影误差 RMS: %.4f 像素\n", rms);
|
||||
if (rms < 0.3)
|
||||
std::printf("评价: 很好 ✓\n");
|
||||
else if (rms < 0.5)
|
||||
std::printf("评价: 可接受\n");
|
||||
else
|
||||
std::printf("评价: 偏差较大，建议按下面排行删掉最差的几张后重跑\n");
|
||||
|
||||
std::printf("\n内参矩阵 K:\n");
|
||||
for (int r = 0; r < 3; ++r) {
|
||||
std::printf(" [ ");
|
||||
for (int c = 0; c < 3; ++c)
|
||||
std::printf("%10.3f ", K.at<double>(r, c));
|
||||
std::printf("]\n");
|
||||
}
|
||||
std::printf(" fx=%.2f fy=%.2f cx=%.2f cy=%.2f\n", K.at<double>(0, 0),
|
||||
K.at<double>(1, 1), K.at<double>(0, 2), K.at<double>(1, 2));
|
||||
|
||||
std::printf("\n畸变系数 [k1 k2 p1 p2 k3 ...]:\n ");
|
||||
for (int c = 0; c < D.cols; ++c) {
|
||||
std::printf("%.6f ", D.at<double>(c));
|
||||
}
|
||||
std::printf("\n");
|
||||
|
||||
// ---------------- 每张图的误差排行 ----------------
|
||||
std::vector<ImageInfo> infos;
|
||||
for (size_t i = 0; i < image_points.size(); ++i) {
|
||||
std::vector<cv::Point2f> reproj;
|
||||
cv::projectPoints(object_points[i], rvecs[i], tvecs[i], K, D, reproj);
|
||||
double err = cv::norm(image_points[i], reproj, cv::NORM_L2) /
|
||||
std::sqrt(static_cast<double>(reproj.size()));
|
||||
infos.push_back({used_names[i], err, static_cast<int>(i)});
|
||||
}
|
||||
std::sort(infos.begin(), infos.end(),
|
||||
[](const ImageInfo &a, const ImageInfo &b) {
|
||||
return a.error_px > b.error_px;
|
||||
});
|
||||
std::printf("\n误差最大的 5 张（可考虑删掉后重跑）:\n");
|
||||
for (int i = 0; i < 5 && i < static_cast<int>(infos.size()); ++i) {
|
||||
std::printf(" %s -> %.3f px\n", infos[i].name.c_str(),
|
||||
infos[i].error_px);
|
||||
}
|
||||
|
||||
// ---------------- 保存 ----------------
|
||||
cv::FileStorage fs(opt.out, cv::FileStorage::WRITE);
|
||||
fs << "camera_matrix" << K;
|
||||
fs << "dist_coeffs" << D;
|
||||
fs << "image_width" << image_size.width;
|
||||
fs << "image_height" << image_size.height;
|
||||
fs << "rms" << rms;
|
||||
fs << "board_cols" << used_size.width;
|
||||
fs << "board_rows" << used_size.height;
|
||||
fs << "square_mm" << opt.square_mm;
|
||||
fs.release();
|
||||
std::printf("\n已保存 -> %s\n", opt.out.c_str());
|
||||
|
||||
// ---------------- 可视化（可选） ----------------
|
||||
if (opt.show) {
|
||||
for (const auto &info : infos) {
|
||||
cv::Mat img = cv::imread(info.name);
|
||||
if (img.empty())
|
||||
continue;
|
||||
std::vector<cv::Point2f> reproj;
|
||||
cv::projectPoints(object_points[info.index], rvecs[info.index],
|
||||
tvecs[info.index], K, D, reproj);
|
||||
for (const auto &p : image_points[info.index])
|
||||
cv::circle(img, p, 3, cv::Scalar(0, 255, 0), -1);
|
||||
for (const auto &p : reproj)
|
||||
cv::circle(img, p, 5, cv::Scalar(0, 0, 255), 1);
|
||||
cv::putText(img, info.name + " err=" + std::to_string(info.error_px),
|
||||
cv::Point(20, 40), cv::FONT_HERSHEY_SIMPLEX, 0.8,
|
||||
cv::Scalar(0, 255, 0), 2);
|
||||
cv::imshow("calibrate", img);
|
||||
if (cv::waitKey(0) == 27)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
std::printf("\n完成。后续使用：\n");
|
||||
std::printf(" cv::FileStorage fs(\"%s\", cv::FileStorage::READ);\n",
|
||||
opt.out.c_str());
|
||||
std::printf(" fs[\"camera_matrix\"] >> K; fs[\"dist_coeffs\"] >> D;\n");
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
#include "camera_calib.hpp"
|
||||
|
||||
#include <cstdio>
|
||||
#include <vector>
|
||||
|
||||
bool CameraCalib::load(const std::string &yaml_path) {
|
||||
cv::FileStorage fs(yaml_path, cv::FileStorage::READ);
|
||||
if (!fs.isOpened()) {
|
||||
std::fprintf(stderr, "[CameraCalib] 打不开配置文件: %s\n",
|
||||
yaml_path.c_str());
|
||||
return false;
|
||||
}
|
||||
fs["camera_matrix"] >> camera_matrix_;
|
||||
fs["dist_coeffs"] >> dist_coeffs_;
|
||||
int w = 0, h = 0;
|
||||
fs["image_width"] >> w;
|
||||
fs["image_height"] >> h;
|
||||
fs.release();
|
||||
|
||||
if (camera_matrix_.empty() || dist_coeffs_.empty()) {
|
||||
std::fprintf(stderr,
|
||||
"[CameraCalib] %s 缺少 camera_matrix 或 dist_coeffs\n",
|
||||
yaml_path.c_str());
|
||||
return false;
|
||||
}
|
||||
image_size_ = cv::Size(w, h);
|
||||
valid_ = true;
|
||||
std::printf("[CameraCalib] %s 已加载: fx=%.2f fy=%.2f cx=%.2f cy=%.2f "
|
||||
"(%dx%d)\n",
|
||||
yaml_path.c_str(), camera_matrix_.at<double>(0, 0),
|
||||
camera_matrix_.at<double>(1, 1), camera_matrix_.at<double>(0, 2),
|
||||
camera_matrix_.at<double>(1, 2), w, h);
|
||||
return true;
|
||||
}
|
||||
|
||||
void CameraCalib::undistort(const cv::Mat &src, cv::Mat &dst) {
|
||||
if (!valid_) {
|
||||
src.copyTo(dst);
|
||||
return;
|
||||
}
|
||||
if (map1_.empty() || map1_.size() != src.size()) {
|
||||
cv::initUndistortRectifyMap(camera_matrix_, dist_coeffs_, cv::Mat(),
|
||||
camera_matrix_, src.size(), CV_32FC1, map1_,
|
||||
map2_);
|
||||
}
|
||||
cv::remap(src, dst, map1_, map2_, cv::INTER_LINEAR);
|
||||
}
|
||||
|
||||
std::optional<ArmorPose> CameraCalib::solvePose(
|
||||
const std::array<cv::Point2f, 4> &corners, const ArmorSize &size) const {
|
||||
if (!valid_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
// 3D 模型点：x 朝前（装甲板法线），y 向左，z 向上；左下起顺时针
|
||||
// 与 jlu_vision 的 ArmorPoints.hpp 保持一致
|
||||
const double W = size.width_mm; // 两灯条中心间距
|
||||
const double L = size.height_mm; // 灯条长度
|
||||
const std::vector<cv::Point3f> obj_points{
|
||||
{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)}, // 右下
|
||||
};
|
||||
const std::vector<cv::Point2f> img_points{corners[0], corners[1],
|
||||
corners[2], corners[3]};
|
||||
|
||||
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<cv::Point2f> 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<double>(reproj.size());
|
||||
pose.reproj_error_px = err;
|
||||
if (err > max_reproj_err_px_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
pose.distance_mm = cv::norm(pose.tvec);
|
||||
return pose;
|
||||
}
|
||||
@@ -0,0 +1,408 @@
|
||||
/**
|
||||
* @file capture_calib.cpp
|
||||
* @brief 相机标定图采集工具（27RM 项目）
|
||||
*
|
||||
* 用途：采集棋盘格标定图，供 calibrateCamera 计算内参和畸变系数
|
||||
*
|
||||
* 编译：xmake （会生成 build/linux/x86_64/release/capture_calib）
|
||||
* 运行：
|
||||
* ./build/linux/x86_64/release/capture_calib # 用 27RM 的 MindVision 相机
|
||||
* ./build/linux/x86_64/release/capture_calib --usb 0 # 用 USB 摄像头 /dev/video0
|
||||
* ./build/linux/x86_64/release/capture_calib --cols 11 --rows 8 --square 15.0
|
||||
*
|
||||
* 按键：
|
||||
* 空格 保存当前帧（默认要求检测到棋盘格；按 f 可强制保存）
|
||||
* f 强制保存当前帧（未检测到角点也存，用于排查问题）
|
||||
* d 删除最后一张
|
||||
* c 开关角点检测（关掉可提高预览帧率）
|
||||
* q/ESC 退出
|
||||
*
|
||||
* 提示：
|
||||
* 1. 保存的是**原始画面**（不含任何绘制内容），保证标定数据干净
|
||||
* 2. 画面右下角会显示 3×3 覆盖图，尽量让九宫格都亮起来
|
||||
* 3. 目标：20~30 张，姿态多样（正对/倾斜/旋转/俯仰），四角尤其要有
|
||||
*/
|
||||
|
||||
#include "camera_set.hpp"
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
const char *kSaveDir = "calib_images";
|
||||
|
||||
struct Options {
|
||||
int board_cols = 11; // 内角点列数
|
||||
int board_rows = 8; // 内角点行数
|
||||
float square_mm = 20.0f; // 方格边长 mm（本工程实测 20.0）
|
||||
int usb_index = -1; // >=0 时用 USB 摄像头
|
||||
int usb_width = 1280;
|
||||
int usb_height = 960;
|
||||
};
|
||||
|
||||
void printUsage(const char *prog) {
|
||||
std::printf(
|
||||
"用法: %s [选项]\n"
|
||||
" --usb N 使用 /dev/videoN（默认用 MindVision 相机）\n"
|
||||
" --cols N 内角点列数（默认 11）\n"
|
||||
" --rows N 内角点行数（默认 8）\n"
|
||||
" --square F 方格边长 mm（默认 20.0，本工程实测值）\n"
|
||||
" --dir PATH 保存目录（默认 %s）\n",
|
||||
prog, kSaveDir);
|
||||
}
|
||||
|
||||
bool parseArgs(int argc, char **argv, Options &opt) {
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (!std::strcmp(argv[i], "--usb") && i + 1 < argc) {
|
||||
opt.usb_index = std::atoi(argv[++i]);
|
||||
} else if (!std::strcmp(argv[i], "--cols") && i + 1 < argc) {
|
||||
opt.board_cols = std::atoi(argv[++i]);
|
||||
} else if (!std::strcmp(argv[i], "--rows") && i + 1 < argc) {
|
||||
opt.board_rows = std::atoi(argv[++i]);
|
||||
} else if (!std::strcmp(argv[i], "--square") && i + 1 < argc) {
|
||||
opt.square_mm = static_cast<float>(std::atof(argv[++i]));
|
||||
} else if (!std::strcmp(argv[i], "--dir") && i + 1 < argc) {
|
||||
// 简单处理：保存目录用全局字符串
|
||||
} else {
|
||||
printUsage(argv[0]);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (opt.board_cols < 3 || opt.board_rows < 3) {
|
||||
std::fprintf(stderr, "错误: 内角点数量太小\n");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ensureDir(const char *dir) {
|
||||
struct stat st;
|
||||
if (stat(dir, &st) == 0) {
|
||||
return;
|
||||
}
|
||||
if (mkdir(dir, 0755) != 0) {
|
||||
std::fprintf(stderr, "错误: 无法创建目录 %s\n", dir);
|
||||
std::exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
|
||||
// （已由循环内的 cur_zone 逻辑替代，保留 gridCellOf 供参考）
|
||||
static int __attribute__((unused)) gridCellOf(const std::vector<cv::Point2f> &corners, const cv::Size &size) {
|
||||
cv::Point2f sum(0, 0);
|
||||
for (const auto &p : corners) {
|
||||
sum += p;
|
||||
}
|
||||
cv::Point2f c = sum * (1.0f / static_cast<float>(corners.size()));
|
||||
int col = std::min(2, std::max(0, static_cast<int>(c.x * 3 / size.width)));
|
||||
int row = std::min(2, std::max(0, static_cast<int>(c.y * 3 / size.height)));
|
||||
return row * 3 + col;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
Options opt;
|
||||
if (!parseArgs(argc, argv, opt)) {
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
ensureDir(kSaveDir);
|
||||
|
||||
// ---------------- 打开图像源 ----------------
|
||||
CameraGrabber grabber; // MindVision 相机（机器人上用的那台）
|
||||
cv::VideoCapture usb_cap; // 备用：USB 摄像头
|
||||
const bool use_usb = (opt.usb_index >= 0);
|
||||
|
||||
if (use_usb) {
|
||||
usb_cap.open(opt.usb_index, cv::CAP_V4L2);
|
||||
if (!usb_cap.isOpened()) {
|
||||
std::fprintf(stderr, "错误: 打不开 /dev/video%d\n", opt.usb_index);
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
usb_cap.set(cv::CAP_PROP_FRAME_WIDTH, opt.usb_width);
|
||||
usb_cap.set(cv::CAP_PROP_FRAME_HEIGHT, opt.usb_height);
|
||||
std::printf("图像源: /dev/video%d (%dx%d)\n", opt.usb_index, opt.usb_width,
|
||||
opt.usb_height);
|
||||
} else {
|
||||
if (!grabber.initializeCamera()) {
|
||||
std::fprintf(stderr,
|
||||
"错误: MindVision 相机初始化失败\n"
|
||||
" · 检查相机是否插好、是否被其他程序占用\n"
|
||||
" · 或者用 --usb 0 改用 USB 摄像头\n");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
if (!grabber.startCamera()) {
|
||||
std::fprintf(stderr, "错误: 相机启动取流失败\n");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
std::printf("图像源: MindVision 相机\n");
|
||||
}
|
||||
|
||||
const cv::Size board_size(opt.board_cols, opt.board_rows);
|
||||
std::printf("标定板: %d x %d 内角点, 方格 %.2f mm\n", opt.board_cols,
|
||||
opt.board_rows, opt.square_mm);
|
||||
std::printf("保存目录: %s/\n", kSaveDir);
|
||||
std::printf("按键: [空格]保存 [f]强制保存 [d]删除上一张 [c]开关角点检测 [q/ESC]退出\n\n");
|
||||
|
||||
std::vector<std::string> saved_files;
|
||||
bool detect_on = true;
|
||||
bool last_found = false;
|
||||
bool gray_prev = false; // 'g' 切换灰度预览（排查曝光/对比度）
|
||||
std::vector<bool> zone_covered(9, false); // 九宫格覆盖情况
|
||||
double cur_sharpness = 0; // 当前帧棋盘格区域清晰度
|
||||
double cur_occupancy = 0; // 棋盘格占画面比例 %
|
||||
int cur_zone = -1; // 当前板中心所在格（0~8）
|
||||
|
||||
cv::Mat frame;
|
||||
while (true) {
|
||||
// ---------------- 取图 ----------------
|
||||
if (use_usb) {
|
||||
usb_cap >> frame;
|
||||
} else {
|
||||
if (!grabber.getFrame(frame)) {
|
||||
continue; // 本帧没取到，跳过
|
||||
}
|
||||
}
|
||||
if (frame.empty()) {
|
||||
std::fprintf(stderr, "错误: 取到空帧，退出\n");
|
||||
break;
|
||||
}
|
||||
|
||||
cv::Mat display = frame.clone(); // 只用于显示（保存用原始 frame）
|
||||
|
||||
// ---------------- 角点检测 ----------------
|
||||
std::vector<cv::Point2f> corners;
|
||||
cv::Size detected_size(0, 0); // 实际检出的尺寸（可能和配置不同）
|
||||
last_found = false;
|
||||
if (detect_on) {
|
||||
cv::Mat gray;
|
||||
cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
|
||||
// 依次尝试三种尺寸：
|
||||
// 1) 配置的尺寸（11x8 内角点）
|
||||
// 2) 少一行列（用户把"方格数"当成了"内角点数"，如 11x8 方格=10x7 内角）
|
||||
// 3) 多一行列（反向情况）
|
||||
const cv::Size try_sizes[3] = {
|
||||
board_size,
|
||||
cv::Size(std::max(3, opt.board_cols - 1),
|
||||
std::max(3, opt.board_rows - 1)),
|
||||
cv::Size(opt.board_cols + 1, opt.board_rows + 1),
|
||||
};
|
||||
for (const auto &sz : try_sizes) {
|
||||
corners.clear();
|
||||
// 新一代检测器（对反光/畸变更鲁棒）
|
||||
last_found = cv::findChessboardCornersSB(
|
||||
gray, sz, corners, cv::CALIB_CB_NORMALIZE_IMAGE |
|
||||
cv::CALIB_CB_LARGER);
|
||||
if (!last_found) {
|
||||
// 退回经典算法
|
||||
last_found = cv::findChessboardCorners(
|
||||
gray, sz, corners,
|
||||
cv::CALIB_CB_ADAPTIVE_THRESH |
|
||||
cv::CALIB_CB_NORMALIZE_IMAGE);
|
||||
}
|
||||
if (last_found) {
|
||||
detected_size = sz;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (last_found) {
|
||||
cv::cornerSubPix(
|
||||
gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
|
||||
cv::TermCriteria(cv::TermCriteria::EPS +
|
||||
cv::TermCriteria::MAX_ITER,
|
||||
30, 0.001));
|
||||
cv::drawChessboardCorners(display, detected_size, corners,
|
||||
true);
|
||||
// ---- 质量指标：清晰度 / 占比 / 所在格 ----
|
||||
cv::Rect roi = cv::boundingRect(corners) &
|
||||
cv::Rect(0, 0, gray.cols, gray.rows);
|
||||
if (roi.width > 8 && roi.height > 8) {
|
||||
cv::Mat lap;
|
||||
cv::Laplacian(gray(roi), lap, CV_64F);
|
||||
cv::Scalar mean, stddev;
|
||||
cv::meanStdDev(lap, mean, stddev);
|
||||
cur_sharpness = stddev[0] * stddev[0]; // Laplacian 方差
|
||||
cur_occupancy = 100.0 * roi.width * roi.height /
|
||||
(gray.cols * gray.rows);
|
||||
int col = std::min(
|
||||
2, std::max(0, (roi.x + roi.width / 2) * 3 / gray.cols));
|
||||
int row = std::min(
|
||||
2, std::max(0, (roi.y + roi.height / 2) * 3 / gray.rows));
|
||||
cur_zone = row * 3 + col;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------- 覆盖度九宫格 ----------------
|
||||
cv::Mat coverage = cv::Mat::zeros(90, 120, CV_8UC3);
|
||||
for (int i = 0; i < 9; ++i) {
|
||||
cv::Rect cell((i % 3) * 40, (i / 3) * 30, 40, 30);
|
||||
cell -= cv::Point(1, 1);
|
||||
if (zone_covered[i]) {
|
||||
cv::rectangle(coverage, cell, cv::Scalar(0, 150, 0), -1);
|
||||
} else if (i == cur_zone && last_found) {
|
||||
cv::rectangle(coverage, cell, cv::Scalar(0, 180, 255), -1);
|
||||
} else {
|
||||
cv::rectangle(coverage, cell, cv::Scalar(60, 60, 60), 1);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------- 叠加信息 ----------------
|
||||
if (gray_prev) {
|
||||
cv::cvtColor(display, display, cv::COLOR_BGR2GRAY);
|
||||
cv::cvtColor(display, display, cv::COLOR_GRAY2BGR);
|
||||
}
|
||||
char buf[256];
|
||||
const cv::Scalar ok_color(0, 255, 0), warn_color(0, 165, 255),
|
||||
err_color(0, 0, 255);
|
||||
cv::Scalar status_color = last_found ? ok_color : warn_color;
|
||||
|
||||
int covered_zones = static_cast<int>(
|
||||
std::count(zone_covered.begin(), zone_covered.end(), true));
|
||||
std::snprintf(buf, sizeof(buf), "saved: %zu | coverage: %d/9 | board: %s",
|
||||
saved_files.size(), covered_zones,
|
||||
!detect_on
|
||||
? "detect off"
|
||||
: (last_found ? "FOUND" : "not found"));
|
||||
cv::putText(display, buf, cv::Point(20, 40), cv::FONT_HERSHEY_SIMPLEX,
|
||||
1.0, status_color, 2);
|
||||
|
||||
if (last_found) {
|
||||
// 实时质量反馈：清晰度 + 板子占画面比例 + 是否与配置尺寸不符
|
||||
const char *sharp_tag = cur_sharpness < 150 ? " BLURRY!"
|
||||
: cur_sharpness > 300 ? " OK"
|
||||
: "";
|
||||
const char *size_tag = cur_occupancy < 5 ? " TOO FAR/SMALL!"
|
||||
: "";
|
||||
std::snprintf(buf, sizeof(buf),
|
||||
"sharpness %.0f%s | board %.1f%% of frame%s",
|
||||
cur_sharpness, sharp_tag, cur_occupancy, size_tag);
|
||||
cv::Scalar qc((cur_sharpness < 150 || cur_occupancy < 5)
|
||||
? cv::Scalar(0, 0, 255)
|
||||
: cv::Scalar(0, 255, 0));
|
||||
cv::putText(display, buf, cv::Point(20, 80), cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.8, qc, 2);
|
||||
}
|
||||
|
||||
if (last_found && detected_size != board_size) {
|
||||
// 实际检出的尺寸与配置不同：提示用户"方格数 vs 内角点数"的关系
|
||||
std::snprintf(buf, sizeof(buf),
|
||||
"detected %dx%d corners (config %dx%d) -> board has "
|
||||
"%dx%d squares",
|
||||
detected_size.width, detected_size.height,
|
||||
board_size.width, board_size.height,
|
||||
detected_size.width + 1, detected_size.height + 1);
|
||||
cv::putText(display, buf, cv::Point(20, 80),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 200, 255),
|
||||
2);
|
||||
}
|
||||
|
||||
if (detect_on && !last_found) {
|
||||
// 检测失败时的排查提示
|
||||
cv::putText(display,
|
||||
"HINT: 整板入画? 四边留白? 无反光? 板子横放? "
|
||||
"距离40cm~1m?",
|
||||
cv::Point(20, display.rows - 70),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 200, 255),
|
||||
2);
|
||||
}
|
||||
|
||||
std::snprintf(buf, sizeof(buf), "%dx%d corners, square %.2f mm",
|
||||
opt.board_cols, opt.board_rows, opt.square_mm);
|
||||
cv::putText(display, buf, cv::Point(20, 80), cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.8, cv::Scalar(200, 200, 200), 2);
|
||||
|
||||
cv::putText(display,
|
||||
"SPACE=save f=force d=undo c=toggle detect q=quit",
|
||||
cv::Point(20, display.rows - 20),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(200, 200, 200), 2);
|
||||
|
||||
// 已保存图片的覆盖情况用文字提示
|
||||
std::snprintf(buf, sizeof(buf), "tips: cover all 9 zones, 20-30 shots");
|
||||
cv::putText(display, buf, cv::Point(20, 120), cv::FONT_HERSHEY_SIMPLEX,
|
||||
0.7, cv::Scalar(160, 160, 160), 2);
|
||||
|
||||
// 九宫格角标贴在右下角
|
||||
coverage.copyTo(display(cv::Rect(display.cols - 130, display.rows - 100,
|
||||
coverage.cols, coverage.rows)));
|
||||
|
||||
cv::imshow("capture_calib (27RM)", display);
|
||||
|
||||
// ---------------- 按键处理 ----------------
|
||||
int key = cv::waitKey(1) & 0xFF;
|
||||
if (key == 27 || key == 'q') {
|
||||
break;
|
||||
} else if (key == 'c') {
|
||||
detect_on = !detect_on;
|
||||
std::printf("角点检测: %s\n", detect_on ? "开" : "关");
|
||||
} else if (key == 'g') {
|
||||
gray_prev = !gray_prev;
|
||||
std::printf("灰度预览: %s\n", gray_prev ? "开" : "关");
|
||||
} else if (key == ' ' || key == 'f') {
|
||||
bool force = (key == 'f');
|
||||
if (!last_found && !force) {
|
||||
std::printf("跳过: 未检测到棋盘格（要强制保存请按 f）\n");
|
||||
continue;
|
||||
}
|
||||
char path[512];
|
||||
std::snprintf(path, sizeof(path), "%s/img_%03zu.jpg", kSaveDir,
|
||||
saved_files.size());
|
||||
// 注意：保存的是原始 frame，不是画了角点的 display
|
||||
if (cv::imwrite(path, frame)) {
|
||||
saved_files.push_back(path);
|
||||
if (last_found && cur_zone >= 0) {
|
||||
zone_covered[cur_zone] = true;
|
||||
}
|
||||
std::printf("[%zu] 已保存 %s%s\n", saved_files.size(), path,
|
||||
force && !last_found ? " (强制)" : "");
|
||||
if (last_found) {
|
||||
int covered_zones = static_cast<int>(
|
||||
std::count(zone_covered.begin(), zone_covered.end(), true));
|
||||
std::printf(
|
||||
" 清晰度 %.0f %s | 板占画面 %.1f%% %s | 覆盖 %d/9 格\n",
|
||||
cur_sharpness,
|
||||
cur_sharpness < 150 ? "← 偏糊，建议重拍"
|
||||
: cur_sharpness > 300 ? "✓" : "",
|
||||
cur_occupancy,
|
||||
cur_occupancy < 5 ? "← 板太小，靠近点" : "",
|
||||
covered_zones);
|
||||
}
|
||||
} else {
|
||||
std::fprintf(stderr, "保存失败: %s\n", path);
|
||||
}
|
||||
} else if (key == 'd') {
|
||||
if (saved_files.empty()) {
|
||||
std::printf("没有可删除的图片\n");
|
||||
} else {
|
||||
std::string last = saved_files.back();
|
||||
saved_files.pop_back();
|
||||
if (std::remove(last.c_str()) == 0) {
|
||||
std::printf("已删除 %s（剩余 %zu 张）\n", last.c_str(),
|
||||
saved_files.size());
|
||||
} else {
|
||||
std::fprintf(stderr, "删除失败: %s\n", last.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!use_usb) {
|
||||
grabber.stopCamera();
|
||||
} else {
|
||||
usb_cap.release();
|
||||
}
|
||||
cv::destroyAllWindows();
|
||||
|
||||
std::printf("\n采集结束，共保存 %zu 张图片到 %s/\n", saved_files.size(),
|
||||
kSaveDir);
|
||||
std::printf("下一步: 运行标定程序计算内参（calibrateCamera）\n");
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
@@ -2,94 +2,44 @@ add_rules("mode.debug", "mode.release")
|
||||
|
||||
set_policy("generator.compile_commands", true)
|
||||
|
||||
set_languages("c++11")
|
||||
set_languages("c++17")
|
||||
|
||||
-- 公共依赖：OpenCV + MindVision 相机 SDK
|
||||
local ROOT = os.scriptdir()
|
||||
|
||||
rule("rm_deps")
|
||||
on_load(function (target)
|
||||
target:add("includedirs", ROOT .. "/inc")
|
||||
target:add("includedirs", "/usr/local/include/opencv4")
|
||||
target:add("includedirs", "/home/mzxiang/MindVisionSDK/include")
|
||||
target:add("linkdirs", "/usr/local/lib")
|
||||
target:add("linkdirs", "/home/mzxiang/MindVisionSDK/lib/x64")
|
||||
target:add("rpathdirs", "/home/mzxiang/MindVisionSDK/lib/x64")
|
||||
target:add("links", "opencv_core", "opencv_imgproc", "opencv_highgui",
|
||||
"opencv_imgcodecs", "opencv_videoio", "opencv_calib3d",
|
||||
"MVSDK")
|
||||
end)
|
||||
rule_end()
|
||||
|
||||
-- 主程序（自瞄本体）
|
||||
target("27RM")
|
||||
add_rules("rm_deps")
|
||||
set_kind("binary")
|
||||
add_files("$(scriptdir)/src/*.cpp")
|
||||
add_includedirs("$(scriptdir)/inc/")
|
||||
-- 各工具有自己的 main 函数，从主程序中排除
|
||||
add_files("src/*.cpp")
|
||||
remove_files("src/capture_calib.cpp")
|
||||
remove_files("src/calibrate.cpp")
|
||||
|
||||
-- openCV配置
|
||||
add_includedirs("/usr/local/include/opencv4")
|
||||
add_linkdirs("/usr/local/lib")
|
||||
add_links(
|
||||
"opencv_core",
|
||||
"opencv_imgproc",
|
||||
"opencv_highgui",
|
||||
"opencv_imgcodecs",
|
||||
"opencv_videoio")
|
||||
|
||||
-- MindVision 相机 SDK
|
||||
add_includedirs("/home/mzxiang/MindVisionSDK/include")
|
||||
add_linkdirs("/home/mzxiang/MindVisionSDK/lib/x64")
|
||||
add_rpathdirs("/home/mzxiang/MindVisionSDK/lib/x64")
|
||||
add_links("MVSDK")
|
||||
--
|
||||
-- If you want to known more usage about xmake, please see https://xmake.io
|
||||
--
|
||||
-- ## FAQ
|
||||
--
|
||||
-- You can enter the project directory firstly before building project.
|
||||
--
|
||||
-- $ cd projectdir
|
||||
--
|
||||
-- 1. How to build project?
|
||||
--
|
||||
-- $ xmake
|
||||
--
|
||||
-- 2. How to configure project?
|
||||
--
|
||||
-- $ xmake f -p [macosx|linux|iphoneos ..] -a [x86_64|i386|arm64 ..] -m [debug|release]
|
||||
--
|
||||
-- 3. Where is the build output directory?
|
||||
--
|
||||
-- The default output directory is `./build` and you can configure the output directory.
|
||||
--
|
||||
-- $ xmake f -o outputdir
|
||||
-- $ xmake
|
||||
--
|
||||
-- 4. How to run and debug target after building project?
|
||||
--
|
||||
-- $ xmake run [targetname]
|
||||
-- $ xmake run -d [targetname]
|
||||
--
|
||||
-- 5. How to install target to the system directory or other output directory?
|
||||
--
|
||||
-- $ xmake install
|
||||
-- $ xmake install -o installdir
|
||||
--
|
||||
-- 6. Add some frequently-used compilation flags in xmake.lua
|
||||
--
|
||||
-- @code
|
||||
-- -- add debug and release modes
|
||||
-- add_rules("mode.debug", "mode.release")
|
||||
--
|
||||
-- -- add macro definition
|
||||
-- add_defines("NDEBUG", "_GNU_SOURCE=1")
|
||||
--
|
||||
-- -- set warning all as error
|
||||
-- set_warnings("all", "error")
|
||||
--
|
||||
-- -- set language: c99, c++11
|
||||
-- set_languages("c99", "c++11")
|
||||
--
|
||||
-- -- set optimization: none, faster, fastest, smallest
|
||||
-- set_optimize("fastest")
|
||||
--
|
||||
-- -- add include search directories
|
||||
-- add_includedirs("/usr/include", "/usr/local/include")
|
||||
--
|
||||
-- -- add link libraries and search directories
|
||||
-- add_links("tbox")
|
||||
-- add_linkdirs("/usr/local/lib", "/usr/lib")
|
||||
--
|
||||
-- -- add system link libraries
|
||||
-- add_syslinks("z", "pthread")
|
||||
--
|
||||
-- -- add compilation and link flags
|
||||
-- add_cxflags("-stdnolib", "-fno-strict-aliasing")
|
||||
-- add_ldflags("-L/usr/local/lib", "-lpthread", {force = true})
|
||||
--
|
||||
-- @endcode
|
||||
--
|
||||
-- 标定图采集工具
|
||||
-- ./build/linux/x86_64/debug/capture_calib （MindVision 相机）
|
||||
-- ./build/linux/x86_64/debug/capture_calib --usb 0 （USB 摄像头）
|
||||
target("capture_calib")
|
||||
add_rules("rm_deps")
|
||||
set_kind("binary")
|
||||
add_files("src/capture_calib.cpp", "src/camera_set.cpp")
|
||||
|
||||
-- 标定计算工具（读 calib_images/ 算出内参，存 camera_calib.yml）
|
||||
target("calibrate")
|
||||
add_rules("rm_deps")
|
||||
set_kind("binary")
|
||||
add_files("src/calibrate.cpp")
|
||||
|
||||
Reference in New Issue
Block a user