mzxwww
f3b06d6abe
- traditional CV pipeline referencing jlu_vision_26: HSV mask -> contour LightBar -> PCA corner refine -> pairing gates - big/small armor classification (bar distance / bar length ratio) - detect_test with synthetic selftest, 6/6 passed - measured armor size 131.56x55mm, solver NaN guard + ITERATIVE fallback
105 lines
4.0 KiB
C++
105 lines
4.0 KiB
C++
#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;
|
|
|
|
/** 大装甲板：英雄（一号）。W=230mm 为官方灯条中心距名义值，
|
|
* 等实测大板后再校正；L=55mm 与小装甲灯条同长 */
|
|
static ArmorSize big() { return ArmorSize{230.0, 55.0}; }
|
|
/** 小装甲板：2/3/4/5 号、哨兵。2026-09-16 实测 3 号板：
|
|
* W=131.56mm（官方规格灯条中心距 131.5mm，实测一致），L=55.0mm */
|
|
static ArmorSize small() { return ArmorSize{131.56, 55.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; }
|
|
|
|
/**
|
|
* 四个角点 → 装甲板位姿
|
|
* 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 → 筛掉
|
|
* 「非有限值」「z<=0（相机后方）」「重投影超差」→ 取重投影最小者；
|
|
* 若 IPPE 数值退化（全被筛掉），自动退回 SOLVEPNP_ITERATIVE。
|
|
* @param corners 图像坐标（原图，含畸变），顺序：左下、左上、右上、右下
|
|
* @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
|