mzxwww
19ac43da6f
- 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
103 lines
3.8 KiB
C++
103 lines
3.8 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;
|
|
|
|
/** 大装甲板：英雄（一号）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; }
|
|
|
|
/**
|
|
* 四个角点 → 装甲板位姿
|
|
* 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 →
|
|
* 依次筛掉「z<=0（相机后方）」「重投影超差」→ 取重投影最小者。
|
|
* 注：角点在解算前先做去畸变（IPPE+畸变+板内旋转组合有数值坑）。
|
|
* @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
|