第9章 PnP 位姿解算¶

本章目标：理解如何从装甲板在图像中的 2D 像素位置，反推出它相对于相机的 3D 空间位置和姿态——这是自动瞄准的核心数学基础。本章将结合 RM_Vision_2027 和 sp_vision_25 两个项目的实际代码进行讲解。

前置知识

本章涉及相机成像模型和 3D 几何。遇到不懂的概念参考： - 📖 ch05 OpenCV 透视变换 - 📖 ch11 标定系统（相机内参、畸变系数）

9.1 什么是位姿解算¶

在 RoboMaster 比赛中，摄像头只给我们一张 2D 图像。我们知道图像上装甲板灯条的像素坐标，但云台需要知道的是：

目标离我多远（平移 / Translation）
目标朝向哪个角度（旋转 / Rotation）

从 2D 图像恢复 3D 位姿的过程，就叫 位姿解算（Pose Estimation）。

graph LR A["2D 像素坐标
(u, v)"] --> B["PnP 位姿解算"] C["已知 3D 模型
(装甲板尺寸)"] --> B B --> D["旋转向量 rvec
(3x1)"] B --> E["平移向量 tvec
(3x1)"] D --> F["目标在相机坐标系
下的 6DoF 位姿"] E --> F

位姿解算的输入是 2D-3D 点对：

输入	说明
2D 像素点	灯条四个角点的 `(u, v)` 坐标
3D 模型点	装甲板的物理尺寸（mm 或 m 级）
相机内参	焦距、主点（由标定得到）

输出是目标相对于相机的旋转和平移。

9.2 相机成像模型¶

9.2.1 针孔模型¶

相机的成像过程可以用 针孔模型 描述：

        世界点 P_w(X, Y, Z)
              \
               \  投影
                \
                 \   焦距 f
                  \
                   \
    ───────────────●──────────────  成像平面
                   ↑ 主点 (cx, cy)

一个三维点 $P_w = (X, Y, Z)$ 投影到像素平面上的过程：

\[ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \boldsymbol{K} \cdot [\boldsymbol{R} \mid \boldsymbol{t}] \cdot \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} \]

其中 $s$ 是尺度因子，$\boldsymbol{K}$ 是内参矩阵，$[\boldsymbol{R} \mid \boldsymbol{t}]$ 是外参矩阵。

9.2.2 内参矩阵 K¶

内参矩阵 $\boldsymbol{K}$ 只和相机本身有关：

\[ \boldsymbol{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \]

📖 ch11 相机内参标定详解

参数	含义
$f_x$, $f_y$	焦距（以像素为单位），通常 $f_x \approx f_y$
$c_x$, $c_y$	光心 / 主点坐标（图像中心附近）

这些参数通过 相机标定 获得（见第 11 章），在运行时固定不变。

9.2.3 畸变系数¶

真实镜头存在畸变，最常见的是 径向畸变 和 切向畸变：

\[ \begin{aligned} r^2 &= x^2 + y^2 \\ x_{\text{distorted}} &= x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1 xy + p_2(r^2 + 2x^2) \\ y_{\text{distorted}} &= y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2p_2 xy \end{aligned} \]

系数	含义
$k_1, k_2, k_3$	径向畸变（桶形/枕形）
$p_1, p_2$	切向畸变

关于畸变处理：OpenCV 的 cv::solvePnP() 可以直接接收畸变系数 dist_coeffs，内部会自动处理畸变。两个项目的代码都是直接将畸变系数传给 solvePnP（见 RM_Vision_2027 的 pnp_solver.cpp 和 sp_vision_25 的 solver.cpp），而不是先手动去畸变。这是更简洁的做法。

9.3 solvePnP 算法¶

9.3.1 PnP 问题定义¶

PnP（Perspective-n-Point） 问题：已知 $n$ 个 3D 空间点及其在图像中的 2D 投影，求解相机相对于这些点的旋转 $\boldsymbol{R}$ 和平移 $\boldsymbol{t}$。

最少需要 4 个共面点 才能求解。IPPE 算法正是为共面情况设计的封闭解法，比迭代法更快更稳定。

9.3.2 OpenCV 接口¶

cv::solvePnP(
    object_points,  // 输入: 3D 点, N×3, float
    image_points,   // 输入: 2D 像素点, N×2, float
    camera_matrix,  // 输入: 内参矩阵 K, 3×3
    dist_coeffs,    // 输入: 畸变系数
    rvec,           // 输出: 旋转向量 (Rodrigues), 3×1
    tvec,           // 输出: 平移向量, 3×1
    useExtrinsicGuess,  // 是否用上次结果作为初值
    flags           // 算法选择
);

9.3.3 为什么选 SOLVEPNP_IPPE¶

OpenCV 提供多种 PnP 求解方法：

方法	适用场景	说明
`SOLVEPNP_ITERATIVE`	通用	Levenberg-Marquardt 迭代优化，需要好的初值
`SOLVEPNP_P3P`	3 点	最少只需 3 点，但解不唯一
`SOLVEPNP_IPPE`	共面点	专门优化共面情况，速度快且稳定
`SOLVEPNP_AP3P`	3 点	改进的 P3P

装甲板的四个灯条角点在物理上是共面的（都在同一块装甲板平面上），因此使用 SOLVEPNP_IPPE 有两大优势：

计算效率高：针对共面几何的代数解法，比迭代法快
数值稳定：共面退化情况下不会出现迭代法的收敛问题

9.4 代码走读 -- PnPSolver（RM_Vision_2027）¶

本节基于 RM_Vision_2027/rm_auto_aim/armor_detector 中的真实代码。

9.4.1 头文件：类定义¶

// RM_Vision_2027/rm_auto_aim/armor_detector/include/armor_detector/pnp_solver.hpp

#ifndef ARMOR_DETECTOR__PNP_SOLVER_HPP_
#define ARMOR_DETECTOR__PNP_SOLVER_HPP_

#include <geometry_msgs/msg/point.hpp>
#include <opencv2/core.hpp>
#include <array>
#include <vector>
#include "armor_detector/armor.hpp"

namespace rm_auto_aim
{
class PnPSolver
{
public:
  PnPSolver(
    const std::array<double, 9> & camera_matrix,
    const std::vector<double> & distortion_coefficients);

  // Get 3d position
  bool solvePnP(const Armor & armor, cv::Mat & rvec, cv::Mat & tvec);

  // Calculate the distance between armor center and image center
  float calculateDistanceToCenter(const cv::Point2f & image_point);

private:
  cv::Mat camera_matrix_;
  cv::Mat dist_coeffs_;

  // Unit: mm
  static constexpr float SMALL_ARMOR_WIDTH = 135;
  static constexpr float SMALL_ARMOR_HEIGHT = 55;
  static constexpr float LARGE_ARMOR_WIDTH = 225;
  static constexpr float LARGE_ARMOR_HEIGHT = 55;

  // Four vertices of armor in 3d
  std::vector<cv::Point3f> small_armor_points_;
  std::vector<cv::Point3f> large_armor_points_;
};

}  // namespace rm_auto_aim
#endif

逐行解读

std::array<double, 9> — 内参矩阵以 9 个 double 的扁平数组传入（3x3 矩阵按行展开），避免直接依赖 ROS 消息类型，方便单元测试
cv::Mat camera_matrix_ / dist_coeffs_ — 成员变量保存内参和畸变，运行期间不变
static constexpr float SMALL_ARMOR_WIDTH = 135 — 小装甲板宽度 135mm、高度 55mm；大装甲板宽度 225mm、高度 55mm。用 constexpr 编译期常量，零运行时开销
small_armor_points_ / large_armor_points_ — 预计算的 3D 模型点向量，在构造函数中初始化一次，后续所有 PnP 调用复用
calculateDistanceToCenter — 计算图像点到光心的像素距离，用于选择"离画面中心最近"的装甲板优先打击

9.4.2 构造函数：初始化 3D 模型点¶

// RM_Vision_2027/rm_auto_aim/armor_detector/src/pnp_solver.cpp

PnPSolver::PnPSolver(
  const std::array<double, 9> & camera_matrix, const std::vector<double> & dist_coeffs)
: camera_matrix_(cv::Mat(3, 3, CV_64F, const_cast<double *>(camera_matrix.data())).clone()),
  dist_coeffs_(cv::Mat(1, 5, CV_64F, const_cast<double *>(dist_coeffs.data())).clone())
{
  // Unit: m
  constexpr double small_half_y = SMALL_ARMOR_WIDTH / 2.0 / 1000.0;
  constexpr double small_half_z = SMALL_ARMOR_HEIGHT / 2.0 / 1000.0;
  constexpr double large_half_y = LARGE_ARMOR_WIDTH / 2.0 / 1000.0;
  constexpr double large_half_z = LARGE_ARMOR_HEIGHT / 2.0 / 1000.0;

  // Start from bottom left in clockwise order
  // Model coordinate: x forward, y left, z up
  small_armor_points_.emplace_back(cv::Point3f(0, small_half_y, -small_half_z));
  small_armor_points_.emplace_back(cv::Point3f(0, small_half_y, small_half_z));
  small_armor_points_.emplace_back(cv::Point3f(0, -small_half_y, small_half_z));
  small_armor_points_.emplace_back(cv::Point3f(0, -small_half_y, -small_half_z));

  large_armor_points_.emplace_back(cv::Point3f(0, large_half_y, -large_half_z));
  large_armor_points_.emplace_back(cv::Point3f(0, large_half_y, large_half_z));
  large_armor_points_.emplace_back(cv::Point3f(0, -large_half_y, large_half_z));
  large_armor_points_.emplace_back(cv::Point3f(0, -large_half_y, -large_half_z));
}

逐行解读

cv::Mat(3, 3, CV_64F, const_cast<double*>(camera_matrix.data())).clone() — 将 std::array<double,9> 的裸指针包装为 3x3 cv::Mat，.clone() 拷贝一份独立数据，防止外部数组析构后悬空
SMALL_ARMOR_WIDTH / 2.0 / 1000.0 — 常量以 mm 定义（135mm），这里除以 1000 转为米，与标定数据单位一致
坐标系约定：x forward, y left, z up — 装甲板平面在 yz 平面上（x=0），这是 RM_Vision_2027 的模型坐标系
点序：从左下角开始，顺时针排列（bottom-left -> top-left -> top-right -> bottom-right），与后续角点提取顺序严格对应
每个 emplace_back 的 y 分量 = 宽度的一半（左右对称），z 分量 = 高度的一半（上下对称），x 分量 = 0（共面）

9.4.3 solvePnP：核心求解函数¶

bool PnPSolver::solvePnP(const Armor & armor, cv::Mat & rvec, cv::Mat & tvec)
{
  std::vector<cv::Point2f> image_armor_points;

  // Use YOLO corner points if available, otherwise use lightbar points
  if (!armor.points.empty() && armor.points.size() == 4) {
    // YOLO keypoints: top-left, top-right, bottom-right, bottom-left
    // PnP expects: bottom-left, top-left, top-right, bottom-right (clockwise from bottom-left)
    image_armor_points.emplace_back(armor.points[3]);  // bottom-left
    image_armor_points.emplace_back(armor.points[0]);  // top-left
    image_armor_points.emplace_back(armor.points[1]);  // top-right
    image_armor_points.emplace_back(armor.points[2]);  // bottom-right
  } else {
    image_armor_points.emplace_back(armor.left_light.bottom);
    image_armor_points.emplace_back(armor.left_light.top);
    image_armor_points.emplace_back(armor.right_light.top);
    image_armor_points.emplace_back(armor.right_light.bottom);
  }

  // Solve pnp
  auto object_points = armor.type == ArmorType::SMALL ? small_armor_points_ : large_armor_points_;
  return cv::solvePnP(
    object_points, image_armor_points, camera_matrix_, dist_coeffs_, rvec, tvec, false,
    cv::SOLVEPNP_IPPE);
}

逐行解读

双路径角点获取：优先使用 YOLO 网络输出的 4 个关键点（armor.points），如果不存在则回退到传统灯条端点
角点顺序重排：YOLO 输出的顺序是 top-left(0)、top-right(1)、bottom-right(2)、bottom-left(3)，但构造函数中 3D 点的顺序是 bottom-left -> top-left -> top-right -> bottom-right，所以需要重排：points[3] -> points[0] -> points[1] -> points[2]
灯条回退路径：left_light.bottom -> left_light.top -> right_light.top -> right_light.bottom，同样是从左下角顺时针
armor.type == ArmorType::SMALL — 根据分类结果选择小/大装甲板的 3D 模型点
cv::SOLVEPNP_IPPE — 使用 IPPE 算法，专门针对共面 4 点优化，速度快且稳定
false — 不使用上一帧的 rvec/tvec 作为外参猜测，每帧独立求解

9.4.4 辅助函数：距离计算¶

float PnPSolver::calculateDistanceToCenter(const cv::Point2f & image_point)
{
  float cx = camera_matrix_.at<double>(0, 2);
  float cy = camera_matrix_.at<double>(1, 2);
  return cv::norm(image_point - cv::Point2f(cx, cy));
}

逐行解读

camera_matrix_.at<double>(0, 2) — 从内参矩阵 K 中取出 $c_x$（第 0 行第 2 列），at<double>(1, 2) 取出 $c_y$
cv::norm(image_point - cv::Point2f(cx, cy)) — 计算装甲板像素中心到图像光心的欧氏距离（像素单位）
用途：当视野中有多个装甲板时，优先选择离画面中心最近的目标——画面边缘的畸变更大，中心的目标精度更高

9.5 代码走读 -- Solver（sp_vision_25）¶

本节基于 sp_vision_25/tasks/auto_aim/ 中的真实代码。与 RM_Vision_2027 相比，sp_vision_25 将 PnP 求解、坐标变换、yaw 优化整合到了同一个 Solver 类中。

9.5.1 头文件：类定义¶

// sp_vision_25/tasks/auto_aim/solver.hpp

#ifndef AUTO_AIM__SOLVER_HPP
#define AUTO_AIM__SOLVER_HPP

#include <Eigen/Dense>  // 必须在opencv2/core/eigen.hpp上面
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include "armor.hpp"

namespace auto_aim
{
class Solver
{
public:
  explicit Solver(const std::string & config_path);

  Eigen::Matrix3d R_gimbal2world() const;
  void set_R_gimbal2world(const Eigen::Quaterniond & q);

  void solve(Armor & armor) const;

  std::vector<cv::Point2f> reproject_armor(
    const Eigen::Vector3d & xyz_in_world, double yaw, ArmorType type, ArmorName name) const;

  double oupost_reprojection_error(Armor armor, const double & picth);
  std::vector<cv::Point2f> world2pixel(const std::vector<cv::Point3f> & worldPoints);

private:
  cv::Mat camera_matrix_;
  cv::Mat distort_coeffs_;
  Eigen::Matrix3d R_gimbal2imubody_;
  Eigen::Matrix3d R_camera2gimbal_;
  Eigen::Vector3d t_camera2gimbal_;
  Eigen::Matrix3d R_gimbal2world_;

  void optimize_yaw(Armor & armor) const;
  double armor_reprojection_error(const Armor & armor, double yaw, const double & inclined) const;
  double SJTU_cost(
    const std::vector<cv::Point2f> & cv_refs, const std::vector<cv::Point2f> & cv_pts,
    const double & inclined) const;
};

}  // namespace auto_aim
#endif

逐行解读

#include <Eigen/Dense> — 引入 Eigen 线性代数库，用于矩阵/四元数运算。注意注释：必须在 opencv2/core/eigen.hpp 之前 include，否则会编译报错
Eigen::Matrix3d R_camera2gimbal_ — 相机到云台的旋转矩阵（手眼标定外参）
Eigen::Vector3d t_camera2gimbal_ — 相机到云台的平移向量
Eigen::Matrix3d R_gimbal2imubody_ — 云台到 IMU 机体坐标系的旋转（机械安装偏差补偿）
Eigen::Matrix3d R_gimbal2world_ — 云台到世界坐标系的旋转，由 IMU 四元数实时更新
optimize_yaw — yaw 角优化，解决 PnP 多义性问题
SJTU_cost — 上海交大提出的加权重投影误差代价函数，角度和平移误差的加权组合

9.5.2 构造函数：从 YAML 加载所有参数¶

// sp_vision_25/tasks/auto_aim/solver.cpp

constexpr double LIGHTBAR_LENGTH = 55e-3;     // m
constexpr double BIG_ARMOR_WIDTH = 225e-3;    // m
constexpr double SMALL_ARMOR_WIDTH = 135e-3;  // m

const std::vector<cv::Point3f> BIG_ARMOR_POINTS{
  {0, BIG_ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2},
  {0, -BIG_ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2},
  {0, -BIG_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2},
  {0, BIG_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2}};
const std::vector<cv::Point3f> SMALL_ARMOR_POINTS{
  {0, SMALL_ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2},
  {0, -SMALL_ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2},
  {0, -SMALL_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2},
  {0, SMALL_ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2}};

Solver::Solver(const std::string & config_path) : R_gimbal2world_(Eigen::Matrix3d::Identity())
{
  auto yaml = YAML::LoadFile(config_path);

  auto R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
  auto R_camera2gimbal_data = yaml["R_camera2gimbal"].as<std::vector<double>>();
  auto t_camera2gimbal_data = yaml["t_camera2gimbal"].as<std::vector<double>>();
  R_gimbal2imubody_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_gimbal2imubody_data.data());
  R_camera2gimbal_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_camera2gimbal_data.data());
  t_camera2gimbal_ = Eigen::Matrix<double, 3, 1>(t_camera2gimbal_data.data());

  auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
  auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> camera_matrix(camera_matrix_data.data());
  Eigen::Matrix<double, 1, 5> distort_coeffs(distort_coeffs_data.data());
  cv::eigen2cv(camera_matrix, camera_matrix_);
  cv::eigen2cv(distort_coeffs, distort_coeffs_);
}

逐行解读

55e-3 — 灯条长度 55mm，用科学计数法直接以米为单位（0.055m）。sp_vision_25 统一用米制，而 RM_Vision_2027 用 mm 后转换
3D 点坐标系：与 RM_Vision_2027 相同（x forward, y left, z up），x=0 表示装甲板在 yz 平面上。但点序不同：这里从 top-left 开始顺时针（top-left -> top-right -> bottom-right -> bottom-left）
Eigen::Matrix3d::Identity() — R_gimbal2world_ 初始化为单位矩阵，表示初始时刻云台坐标系与世界坐标系对齐
YAML::LoadFile(config_path) — 从 YAML 配置文件读取所有标定参数，包括内参、畸变、外参矩阵
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> — 指定 行优先 存储，与 YAML 中数据的排列顺序匹配（Eigen 默认列优先）
cv::eigen2cv(camera_matrix, camera_matrix_) — 将 Eigen 矩阵转为 OpenCV cv::Mat，供 solvePnP 使用

9.5.3 坐标变换：set_R_gimbal2world¶

void Solver::set_R_gimbal2world(const Eigen::Quaterniond & q)
{
  Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix();
  R_gimbal2world_ = R_gimbal2imubody_.transpose() * R_imubody2imuabs * R_gimbal2imubody_;
}

逐行解读

Eigen::Quaterniond & q — IMU 输出的四元数，表示机体在绝对坐标系中的姿态
q.toRotationMatrix() — 四元数转旋转矩阵 $\boldsymbol{R}_{\text{imubody} \to \text{imuabs}}$
R_gimbal2imubody_.transpose() — 由于旋转矩阵是正交阵，转置即为逆，即 $\boldsymbol{R}_{\text{imubody} \to \text{gimbal}}$
变换链：$\boldsymbol{R}_{\text{gimbal} \to \text{world}} = \boldsymbol{R}_{\text{imubody} \to \text{gimbal}}^T \cdot \boldsymbol{R}_{\text{imubody} \to \text{imuabs}} \cdot \boldsymbol{R}_{\text{gimbal} \to \text{imubody}}$——先将云台轴角转到 IMU 机体坐标系，再由 IMU 转到绝对坐标系，最后转回云台表示。这样做的原因是 IMU 的安装位置与云台旋转中心不重合

9.5.4 solve：完整的位姿求解流水线¶

void Solver::solve(Armor & armor) const
{
  const auto & object_points =
    (armor.type == ArmorType::big) ? BIG_ARMOR_POINTS : SMALL_ARMOR_POINTS;

  cv::Vec3d rvec, tvec;
  cv::solvePnP(
    object_points, armor.points, camera_matrix_, distort_coeffs_, rvec, tvec, false,
    cv::SOLVEPNP_IPPE);

  Eigen::Vector3d xyz_in_camera;
  cv::cv2eigen(tvec, xyz_in_camera);
  armor.xyz_in_gimbal = R_camera2gimbal_ * xyz_in_camera + t_camera2gimbal_;
  armor.xyz_in_world = R_gimbal2world_ * armor.xyz_in_gimbal;

  cv::Mat rmat;
  cv::Rodrigues(rvec, rmat);
  Eigen::Matrix3d R_armor2camera;
  cv::cv2eigen(rmat, R_armor2camera);
  Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera;
  Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal;
  armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0);
  armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0);

  armor.ypd_in_world = tools::xyz2ypd(armor.xyz_in_world);

  // 平衡不做yaw优化，因为pitch假设不成立
  auto is_balance = (armor.type == ArmorType::big) &&
                    (armor.name == ArmorName::three || armor.name == ArmorName::four ||
                     armor.name == ArmorName::five);
  if (is_balance) return;

  optimize_yaw(armor);
}

逐行解读

cv::Vec3d rvec, tvec — PnP 输出的旋转向量（Rodrigues）和平移向量
cv::solvePnP(..., cv::SOLVEPNP_IPPE) — 与 RM_Vision_2027 一致，使用 IPPE 共面算法
cv::cv2eigen(tvec, xyz_in_camera) — 将 OpenCV 的 Vec3d 转为 Eigen 的 Vector3d
R_camera2gimbal_ * xyz_in_camera + t_camera2gimbal_ — 相机到云台的刚性变换：先旋转再平移，$P_g = R_{c2g} P_c + t_{c2g}$
R_gimbal2world_ * armor.xyz_in_gimbal — 云台到世界变换：$P_w = R_{g2w} P_g$（无额外平移，因为原点约定）
cv::Rodrigues(rvec, rmat) — 将 3 维旋转向量转为 3x3 旋转矩阵（Rodrigues 公式）
R_armor2gimbal = R_camera2gimbal_ * R_armor2camera — 装甲板朝向从相机系变换到云台系
R_armor2world = R_gimbal2world_ * R_armor2gimbal — 再从云台系变换到世界系
tools::eulers(R, 2, 1, 0) — 从旋转矩阵提取欧拉角（ZYX 顺序，即 yaw-pitch-roll）
tools::xyz2ypd — 将三维坐标转为 (yaw, pitch, distance) 极坐标表示
平衡步兵跳过 yaw 优化：三号、四号、五号步兵的大装甲板可能是平衡步兵，其装甲板 pitch 角假设（15 度）不成立，所以跳过优化

9.5.5 optimize_yaw：解决 PnP yaw 角多义性¶

void Solver::optimize_yaw(Armor & armor) const
{
  Eigen::Vector3d gimbal_ypr = tools::eulers(R_gimbal2world_, 2, 1, 0);

  constexpr double SEARCH_RANGE = 140;  // degree
  auto yaw0 = tools::limit_rad(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0);

  auto min_error = 1e10;
  auto best_yaw = armor.ypr_in_world[0];

  for (int i = 0; i < SEARCH_RANGE; i++) {
    double yaw = tools::limit_rad(yaw0 + i * CV_PI / 180.0);
    auto error = armor_reprojection_error(armor, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0);

    if (error < min_error) {
      min_error = error;
      best_yaw = yaw;
    }
  }

  armor.yaw_raw = armor.ypr_in_world[0];
  armor.ypr_in_world[0] = best_yaw;
}

逐行解读

tools::eulers(R_gimbal2world_, 2, 1, 0) — 提取云台当前的 yaw-pitch-roll
SEARCH_RANGE = 140 — 搜索范围 140 度，以云台朝向为中心，左右各 70 度
yaw0 = gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0 — 搜索起点 = 云台 yaw - 70 度
tools::limit_rad — 将角度限制在 $[-\pi, \pi]$ 范围内，避免角度溢出
穷举搜索：以 1 度步长遍历 140 个候选 yaw 值，对每个 yaw 计算重投影误差
(i - SEARCH_RANGE / 2) * CV_PI / 180.0 — 当前搜索角与中心的偏差（弧度），传入 armor_reprojection_error 作为 "inclined" 参数
armor.yaw_raw — 保存 PnP 原始 yaw 值，供调试对比
armor.ypr_in_world[0] = best_yaw — 用最优 yaw 替换原始值

9.5.6 reproject_armor：从 yaw 重建旋转矩阵并重投影¶

std::vector<cv::Point2f> Solver::reproject_armor(
  const Eigen::Vector3d & xyz_in_world, double yaw, ArmorType type, ArmorName name) const
{
  auto sin_yaw = std::sin(yaw);
  auto cos_yaw = std::cos(yaw);

  auto pitch = (name == ArmorName::outpost) ? -15.0 * CV_PI / 180.0 : 15.0 * CV_PI / 180.0;
  auto sin_pitch = std::sin(pitch);
  auto cos_pitch = std::cos(pitch);

  // clang-format off
  const Eigen::Matrix3d R_armor2world {
    {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch},
    {sin_yaw * cos_pitch,  cos_yaw, sin_yaw * sin_pitch},
    {         -sin_pitch,        0,           cos_pitch}
  };
  // clang-format on

  // get R_armor2camera t_armor2camera
  const Eigen::Vector3d & t_armor2world = xyz_in_world;
  Eigen::Matrix3d R_armor2camera =
    R_camera2gimbal_.transpose() * R_gimbal2world_.transpose() * R_armor2world;
  Eigen::Vector3d t_armor2camera =
    R_camera2gimbal_.transpose() * (R_gimbal2world_.transpose() * t_armor2world - t_camera2gimbal_);

  // get rvec tvec
  cv::Vec3d rvec;
  cv::Mat R_armor2camera_cv;
  cv::eigen2cv(R_armor2camera, R_armor2camera_cv);
  cv::Rodrigues(R_armor2camera_cv, rvec);
  cv::Vec3d tvec(t_armor2camera[0], t_armor2camera[1], t_armor2camera[2]);

  // reproject
  std::vector<cv::Point2f> image_points;
  const auto & object_points = (type == ArmorType::big) ? BIG_ARMOR_POINTS : SMALL_ARMOR_POINTS;
  cv::projectPoints(object_points, rvec, tvec, camera_matrix_, distort_coeffs_, image_points);
  return image_points;
}

逐行解读

pitch 硬编码为 15 度：普通装甲板向上倾斜约 15 度（安装在机器人上），前哨站向下 -15 度。这是 yaw 优化的关键假设——先固定 pitch 和 roll，只搜索 yaw
R_armor2world — 从 yaw 和 pitch 直接构建旋转矩阵，使用 ZYX 欧拉角公式。roll 固定为 0
R_camera2gimbal_.transpose() — 旋转矩阵正交，转置 = 逆，即 $R_{g2c}$，从云台系反变换回相机系
R_gimbal2world_.transpose() * R_armor2world — 从世界系反变换回云台系再变换到相机系
`t_armor2camera = R_{g2c} (R_{w2g} t_{w} - t_{c2g})$ — 反向传播平移向量
cv::Rodrigues(R_armor2camera_cv, rvec) — 3x3 旋转矩阵转回 3 维旋转向量
cv::projectPoints(...) — 将 3D 模型点投影回 2D 像素坐标，用于计算重投影误差

9.5.7 armor_reprojection_error：重投影误差计算¶

double Solver::armor_reprojection_error(
  const Armor & armor, double yaw, const double & inclined) const
{
  auto image_points = reproject_armor(armor.xyz_in_world, yaw, armor.type, armor.name);
  auto error = 0.0;
  for (int i = 0; i < 4; i++) error += cv::norm(armor.points[i] - image_points[i]);
  // auto error = SJTU_cost(image_points, armor.points, inclined);
  return error;
}

逐行解读

reproject_armor(armor.xyz_in_world, yaw, ...) — 用候选 yaw 重投影，得到 4 个预测像素点
cv::norm(armor.points[i] - image_points[i]) — 计算每个角点的欧氏像素距离
error += ... — 累加 4 个角点的误差（不是平均值，但不影响 argmin 结果）
注释掉的 SJTU_cost — 上海交大的加权代价函数，结合了像素误差和斜率角度误差，在某些场景下更鲁棒，但当前使用简单欧氏距离

9.6 两个项目的完整坐标变换链¶

9.6.1 变换链总览¶

9.6.2 数学公式¶

相机坐标系到云台坐标系（刚性变换，手眼标定得到）：

\[ P_g = \boldsymbol{R}_{c2g} \cdot P_c + \boldsymbol{t}_{c2g} \]

云台坐标系到世界坐标系（由 IMU 四元数实时更新）：

\[ P_w = \boldsymbol{R}_{g2w} \cdot P_g \]

旋转矩阵更新（IMU 四元数 $q$）：

\[ \boldsymbol{R}_{g2w} = \boldsymbol{R}_{\text{imubody} \to \text{gimbal}}^T \cdot \boldsymbol{R}_{q} \cdot \boldsymbol{R}_{\text{gimbal} \to \text{imubody}} \]

目标角度计算：

\[ \text{yaw} = \arctan\left(\frac{x_g}{z_g}\right), \quad \text{pitch} = \arctan\left(\frac{y_g}{\sqrt{x_g^2 + z_g^2}}\right) \]

9.6.3 sp_vision_25 的完整变换代码¶

sp_vision_25 的 solve 函数将整个变换链写在一个函数中，包括位置和姿态两个维度：

// 位置变换：相机 -> 云台 -> 世界
armor.xyz_in_gimbal = R_camera2gimbal_ * xyz_in_camera + t_camera2gimbal_;
armor.xyz_in_world  = R_gimbal2world_ * armor.xyz_in_gimbal;

// 姿态变换：相机 -> 云台 -> 世界
Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera;
Eigen::Matrix3d R_armor2world  = R_gimbal2world_ * R_armor2gimbal;

// 提取欧拉角
armor.ypr_in_gimbal = tools::eulers(R_armor2gimbal, 2, 1, 0);
armor.ypr_in_world  = tools::eulers(R_armor2world, 2, 1, 0);

// 转极坐标
armor.ypd_in_world  = tools::xyz2ypd(armor.xyz_in_world);

RM_Vision_2027 的区别

RM_Vision_2027 的 PnPSolver 类 只负责 PnP 求解，不包含坐标变换和 yaw 优化。这些功能由上游的 tracker / sender 节点处理。这是一种更模块化的设计——PnPSolver 是纯粹的数学工具类。

9.7 对比：RM_Vision_2027 vs sp_vision_25¶

9.7.1 架构对比¶

维度	RM_Vision_2027 (PnPSolver)	sp_vision_25 (Solver)
职责范围	仅 PnP 求解	PnP + 坐标变换 + yaw 优化
单位	米（mm 常量 / 1000）	米（直接用 `55e-3`）
3D 点顺序	bottom-left 顺时针	top-left 顺时针
配置方式	构造函数参数传入	YAML 文件加载
角点来源	YOLO 关键点优先，灯条回退	直接使用 `armor.points`
坐标变换	不在本类中	集成在 `solve()` 内
yaw 优化	无	140 度穷举搜索
数学库	纯 OpenCV	Eigen + OpenCV

9.7.2 3D 模型点对比¶

// RM_Vision_2027: 小装甲板 (135mm x 55mm, 大装甲板 225mm x 55mm)
// 顺序: bottom-left -> top-left -> top-right -> bottom-right
{0, 0.0675, -0.0275},   // bottom-left  (y=+W/2, z=-H/2)
{0, 0.0675,  0.0275},   // top-left     (y=+W/2, z=+H/2)
{0,-0.0675,  0.0275},   // top-right    (y=-W/2, z=+H/2)
{0,-0.0675, -0.0275}    // bottom-right (y=-W/2, z=-H/2)

// sp_vision_25: 小装甲板 (135mm x 56mm, 大装甲板 230mm x 56mm) ⚠️ 与 RM_Vision_2027 不同！

!!! warning "两个仓库的尺寸常量不同"
    sp_vision_25 使用 56mm 和 230mm（同济原始值），RM_Vision_2027 使用 55mm 和 225mm（你们的值）。
    这个差异会导致 PnP 解算结果有微小偏差。实际比赛中以你们的仓库为准。
// 顺序: top-left -> top-right -> bottom-right -> bottom-left
{0, 0.0675,  0.0275},    // top-left     (y=+W/2, z=+H/2)
{0,-0.0675,  0.0275},    // top-right    (y=-W/2, z=+H/2)
{0,-0.0675, -0.0275},    // bottom-right (y=-W/2, z=-H/2)
{0, 0.0675, -0.0275}     // bottom-left  (y=+W/2, z=-H/2)

点序必须严格匹配

3D 模型点和 2D 像素点的对应顺序必须 完全一致。如果 3D 点第 0 个是左下角，那 2D 点第 0 个也必须是左下角。顺序错乱会导致 PnP 返回完全错误的解。

9.7.3 Yaw 优化策略对比¶

特性	sp_vision_25 实现
搜索范围	140 度（云台 yaw 为中心，左右各 70 度）
步长	1 度
搜索次数	140 次 `reproject_armor` 调用
误差度量	4 个角点欧氏距离之和
pitch 假设	普通装甲板 +15 度，前哨站 -15 度
跳过条件	平衡步兵（大装甲板 + 3/4/5 号）

graph TD A["PnP 解算得到 rvec, tvec"] --> B["提取原始 yaw"] B --> C["固定 pitch=15deg, roll=0"] C --> D["遍历 yaw: 140 个候选值"] D --> E["对每个 yaw:
重建 R_armor2world
-> 反变换回相机系
-> projectPoints 重投影"] E --> F["计算 4 角点误差之和"] F --> G["选择误差最小的 yaw"] G --> H["替换 armor.ypr_in_world[0]"] style D fill:#ffecb3 style G fill:#c8e6c9

9.8 调试技巧与常见问题¶

9.8.1 验证 PnP 结果¶

将 PnP 解算的 3D 点重新投影回图像，检查是否和原始角点吻合：

std::vector<cv::Point2f> reprojected;
cv::projectPoints(object_points, rvec, tvec,
                  camera_matrix_, dist_coeffs, reprojected);

// 计算重投影误差
double error = 0;
for (int i = 0; i < 4; i++) {
    error += cv::norm(reprojected[i] - image_points[i]);
}
error /= 4.0;

// 经验值: error < 2.0 像素为良好
if (error > 5.0) {
    RCLCPP_WARN(logger, "Reprojection error too large: %.2f px", error);
}

9.8.2 常见问题排查¶

现象	可能原因	解决方案
距离偏大	内参焦距偏小	重新标定相机
距离偏小	内参焦距偏大	重新标定相机
yaw 频繁跳变	角点顺序错误	检查 3D/2D 点序是否严格一致
PnP 失败返回	点数不足或共线	检查检测结果是否完整
远处偏差大	畸变未正确消除	确认畸变系数与标定结果匹配
yaw 优化后偏差更大	pitch 假设不成立	检查是否为平衡步兵，需跳过优化
单位不一致	mm 与 m 混用	确认 3D 点单位、tvec 单位、外参单位三者一致

9.8.3 单位陷阱¶

PnP 解算中，单位一致性至关重要：

3D 模型点 (m)   +  内参 (像素)  →  tvec (m)     ✓ sp_vision_25
3D 模型点 (mm)  +  内参 (像素)  →  tvec (mm)    ✓ RM_Vision_2027 (转换前)
3D 模型点 (m)   +  内参不匹配   →  结果完全错误  ⚠️

RM_Vision_2027 的单位注意

RM_Vision_2027 的头文件中常量以 mm 定义（SMALL_ARMOR_WIDTH = 135），但在构造函数中除以 1000 转为米。因此 solvePnP 的 tvec 输出单位也是米。如果后续代码假设 tvec 为 mm，将引入 1000 倍的误差。

9.9 本章小结¶

graph TD A["相机拍摄图像"] --> B["角点检测
(YOLO 或灯条端点)"] B --> C["获取 4 个 2D 像素点"] D["装甲板 3D 模型
(135x55mm / 225x55mm)"] --> E["solvePnP
(SOLVEPNP_IPPE)"] C --> E F["相机内参 K + 畸变"] --> E E --> G["rvec + tvec"] G --> H["camera2gimbal 外参变换"] H --> I["目标在云台坐标系"] I --> J["gimbal2world IMU 变换"] J --> K["目标在世界坐标系"] K --> L["optimize_yaw
(sp_vision_25 特有)"] L --> M["计算 yaw / pitch"] M --> N["发送给云台电机"] style E fill:#ffecb3 style L fill:#ffcdd2 style H fill:#c8e6c9 style J fill:#c8e6c9

关键公式回顾：

投影方程：$s[u, v, 1]^T = \boldsymbol{K} \cdot [\boldsymbol{R} \mid \boldsymbol{t}] \cdot [X, Y, Z, 1]^T$
内参矩阵：$f_x, f_y, c_x, c_y$（标定得到）
PnP 输出：旋转 rvec + 平移 tvec
相机到云台：$P_g = R_{c2g} P_c + t_{c2g}$
云台到世界：$P_w = R_{g2w} P_g$
角度计算：$\text{yaw} = \text{atan2}(y, x)$，$\text{pitch} = \text{atan2}(z, \sqrt{x^2+y^2})$

两个项目的核心区别：

	RM_Vision_2027	sp_vision_25
设计哲学	职责单一，PnP 只做求解	大而全，一站式位姿服务
角点来源	YOLO + 灯条双路径	仅使用 `armor.points`
yaw 优化	无	140 度穷举搜索
数学库	纯 OpenCV	Eigen + OpenCV
适用场景	模块化 ROS2 架构	紧凑集成式架构

下一章预告：第 10 章将介绍弹道解算——如何根据目标距离、弹速和重力补偿，计算出实际的射击角度。

参数	含义
\(f_x\), \(f_y\)	焦距（以像素为单位），通常 \(f_x \approx f_y\)
\(c_x\), \(c_y\)	光心 / 主点坐标（图像中心附近）

系数	含义
\(k_1, k_2, k_3\)	径向畸变（桶形/枕形）
\(p_1, p_2\)	切向畸变