title: 如何通过图像中已知角点坐标反求相机位置：PnP外参估计实践
slug: pnp-extrinsic-calibration-practice

如何通过图像中已知角点坐标反求相机位置：PnP外参估计实践

摘要

已知相机内参，已知场景中几个点的 3D 坐标，从图像中找到这些点的像素位置，求解相机在三维世界中的位姿——这就是 Perspective-n-Point（PnP）问题。

本文围绕 PnP 展开四部分内容：问题的数学定义与求解方法，solvePnP 输出 tvec 的常见误解，一个将完整流程工程化的开源工具 cam2body-calib，以及坐标系相关的一組实践问题。PnP 算法本身已经非常成熟，实际工作中出问题的地方几乎都在坐标系定义上。

本文涉及的工具已开源：ruali-dev/cam2body-calib（Apache 2.0），提供 Web UI 和 CLI 两种使用方式。

写在前面

这个开源的工具是实习任务中遇到了这种问题，写出来的小工具的演化版本。

1. 问题的数学定义

已知：

相机内参 K（通过棋盘格标定获得）
畸变系数 D
N 个标志物点在世界坐标系下的 3D 坐标：P_world_i = [X_i, Y_i, Z_i]
这 N 个标志物点在图像中的 2D 像素坐标：p_i = [u_i, v_i]

求：

相机相对于世界坐标系的位姿，即旋转矩阵 R 和平移向量 t（合称外参）

这个问题称为 Perspective-n-Point（PnP）。

PnP 问题示意图：已知 3D 标志物点和图像 2D 点，求解相机位姿

2. PnP 是怎么解的？

2.1 基本思路

OpenCV 的 solvePnP 完成这件事：建立 3D 点到 2D 点的对应关系，通过最小化重投影误差来优化 R 和 t。

min_{R,t} Σ_i || project(K, R, t, P_world_i) - p_i ||²

其中 project 表示用针孔模型将世界点投影到图像平面。

2.2 RANSAC 的作用

当点对数超过 4 对且可能存在误匹配时，使用 solvePnPRansac。它随机采样多个子集分别求解，选出内点数量最多的结果，自动排除 outlier。

2.3 最少需要几个点？

P3P 只需要 3 个点，但 3 个点存在多解歧义，需要第 4 个点来确定唯一解。工程中至少使用 4 个点，点数越多结果越稳定。

3. 旋转矩阵、平移向量与相机外参

3.1 关键误区：solvePnP 输出的是什么？

solvePnP 返回的 rvec 和 tvec 满足：

P_camera = R · P_world + t

这个等式的含义是将世界坐标系的点变换到相机坐标系。solvePnP 输出的是"世界在相机坐标系下的表示"，而不是"相机在世界坐标系下的位姿"。

写成齐次变换矩阵：

T_cam_world = [ R  t ]
              [ 0  1 ]

这描述的是从世界坐标系到相机坐标系的变换。

3.2 真正的相机位置

相机在世界坐标系中的坐标需要取逆得到：

T_world_cam = inv(T_cam_world)

T_world_cam[:3, 3] 才是相机在世界坐标系中的位置。solvePnP 给出的是"世界到相机"，需要的是"相机在世界"，做一个矩阵求逆即可。

T_cam_world 和 T_world_cam 的关系示意图

4. 重投影误差：如何判断结果好坏？

4.1 什么是重投影误差

用求解得到的 R 和 t，将已知的 3D 标志物点重新投影回图像，与原始检测到的像素坐标比较：

reproj_error_i = || project(K, R, t, P_world_i) - p_i ||

4.2 判断标准

mean reprojection error	评价
< 0.5 px	优秀
< 1.5 px	良好
< 3.0 px	一般，需要排查
> 3.0 px	不可信，大概率存在错误

4.3 除了数值还要看图

误差小不代表结果正确。需要检查可视化结果：检测到的角点和重投影点是否基本重合。如果图像对齐良好且误差小，PnP 主链路可以认为正常工作。如果图像明显歪斜但误差很小，通常是内参或 3D 坐标填写有误。

5. 工程实现思路

5.1 核心流程

输入图像 → 去畸变（如果是鱼眼）→ 手动点选标志物角点
           → 读取标志物 3D 坐标
           → solvePnPRansac → solvePnPRefineLM（可选）
           → 构建 T_cam_world
           → 取逆得到 T_world_cam
           → 计算相机位置和 RPY
           → 重投影误差分析
           → 可视化
           → 导出结果

5.2 手动点选的交互方式

基于 OpenCV 鼠标回调实现：

左键添加点，右键撤销上一个
滚轮缩放，中键拖拽平移
右下角放大镜辅助精确定位
S 键保存并退出

对于鱼眼相机，先去畸变再点选。去畸变后的图像是常规视角，角点定位更容易。

5.3 模块化设计

将功能拆分为独立模块而非集中在单一文件中：

相机模型：管理 K、D、图像尺寸、去畸变逻辑
标志物布局：读取 3D 坐标配置
PnP 求解器：封装 solvePnP + RANSAC + refine
可视化：绘制角点、重投影点、坐标轴
导出器：将标准结果转换为下游所需格式

后续更换标志物类型或输出格式时只需修改对应模块。

5.4 Export Profile 模式

核心计算在标准右手系下完成，下游系统可能需要左手系或其他格式的结果。通过导出适配层解耦：

核心层：标准 PoseResult（右手系，T_world_cam）

导出适配层（Export Profile）：
  - right_handed_pose6   → x/y/z/roll/pitch/yaw
  - opencv_optical_matrix → 4×4 矩阵
  - left_handed_pose6     → 左手系的 position + RPY

核心逻辑保持干净，适配层处理格式转换。

6. cam2body-calib：一个实际的 PnP 外参估计工具

以上是理论框架和设计思路。本节介绍基于这些想法实现的工具：cam2body-calib。

6.1 功能概述

工具的工作流：上传相机内参 → 拖入标定图像 → 在图像上点击四个标志物角点 → 输入这四个角点在车体坐标系下的 3D 坐标 → 一键求解相机 6-DoF 位姿。

典型应用场景：相机内参已通过棋盘格标定获得，需要确定相机在车体上的精确安装位置和朝向。平移量（x/y/z）可以用尺子近似测量，但姿态角（roll/pitch/yaw）难以人工量准。此工具通过在地面放置已知尺寸的标定板（或利用车体上已知坐标的固定点），拍摄单张图像后手动点选四个角点，即可解算出相机外参。

cam2body-calib 工具界面：左侧上传图片并点选标志物角点，右侧显示解算结果和导出选项

6.2 使用方式

Web UI 模式（推荐日常使用）：

git clone https://github.com/ruali-dev/cam2body-calib.git
cd cam2body-calib
uv sync
uv run cam2body-calib ui

启动后在浏览器中完成全部操作——上传图片、标注角点、填写 3D 坐标、运行 solvePnP、查看重投影误差、选择导出格式。

6.3 工程结构

cam2body-calib/
├── src/cam2body_calib/
│   ├── camera/model.py          # pinhole + fisheye，自动去畸变
│   ├── estimation/pnp_solver.py # solvePnPRansac + refine
│   ├── geometry/                # 4×4 变换、RPY、左右手系转换
│   ├── exporters/               # right_handed / left_handed 双 profile
│   ├── interactive/annotator.py # OpenCV 手动标注工具
│   ├── web/                     # FastAPI 后端 + 前端 UI
│   └── cli.py                   # Typer CLI 入口
├── demo/                        # 可直接运行的示例（内参 + 图片 + 3D 坐标）
└── tests/                       # 31 个 pytest

几个设计要点：

鱼眼与针孔统一处理：无论镜头类型，统一经过去畸变后当作 pinhole 模型处理。使用 cv2.fisheye.estimateNewCameraMatrixForUndistortRectify + initUndistortRectifyMap 完成，模型标记自动从 fisheye 切换到 pinhole。
PnP 求解链路：solvePnPRansac（鲁棒初值）→ solvePnPRefineLM（基于 inlier 精化）→ 构建 T_cam_body → 取逆得到 T_body_camera → 轴变换到 camera_link → 重投影误差分析。
坐标系隔离：核心 PnP 计算全部在右手系下完成，左手系仅作为导出层的格式适配。导出时使用 S = diag([1, -1, 1]) 做 S @ R @ S 转换，保证行列式为 +1。

6.4 导出格式

Profile	Body 坐标系	yaw 正方向	适用场景
`right_handed`	x=前, y=左, z=上	yaw>0=左转	ROS REP-103
`left_handed`	x=前, y=右, z=上	yaw>0=右转	部分车辆工程系统

yaw 和 pitch 不依赖于欧拉角分解，而是直接从相机前轴向量计算，避免万向节锁问题。导出结果为 YAML 格式，可直接供下游系统读取。

6.5 精度参考

demo 目录中提供了可运行的示例：960×768 的虚拟针孔图像，四个标定点，重投影误差约 1-2 px，位置估计与安装尺寸吻合。实际工程场景中的精度受内参质量、标定点测量准确度和图像清晰度影响，但几何链路本身已经过充分验证。

7. 坐标系——实践中最易出错的部分

PnP 算法本身已经非常成熟。在实际工作中，绝大多数问题出现在坐标系定义上。

7.1 两个关键坐标系

OpenCV optical frame（solvePnP 使用的坐标系）：

x 轴指向右，y 轴指向下，z 轴指向前（相机观察方向）

camera_link frame（ROS 和车辆工程中常用的坐标系）：

x 轴指向前，y 轴指向左，z 轴指向上

两者的轴方向完全不同。直接对 OpenCV optical frame 的旋转矩阵提取 RPY 会得到反直觉的数值：相机水平朝前时，optical frame 的 roll 约为 -90°，而非 0°。

处理方式：工具内部同时计算 optical RPY 和 link RPY。optical 用于 PnP 计算与调试，link 用于结果展示和下游对接。

7.2 左手系与右手系

ROS 车体坐标系通常使用右手系（x 前, y 左, z 上）。部分车辆工程系统使用左手系（x 前, y 右, z 上）。两者的差异在于 y 轴方向。

7.3 左手系下提取 RPY 的正确方法

在右手系中得到旋转矩阵 R（det = +1），直接翻转 y 轴换到左手系：

R_lh = S @ R     其中 S = diag([1, -1, 1])

此时：

det(R_lh) = det(S) × det(R) = (-1) × (+1) = -1

行列式为 -1 的矩阵不是纯旋转，而是旋转加镜像。scipy 的 Rotation.from_matrix 会拒绝处理此类矩阵。

正确做法是同时镜像父坐标系和子坐标系：

R_lh = S @ R @ S
det(R_lh) = (-1) × (+1) × (-1) = +1

这样得到的矩阵行列式为 +1，可以安全地进行欧拉角分解。

OpenCV optical frame 和 camera_link frame 的轴方向对比

右手系与左手系车辆坐标系对比：y 轴方向翻转

7.4 万向节锁与 forward axis 方法

即使行列式正确，RPY 也存在万向节锁问题：当 pitch 接近 ±90° 时，roll 和 yaw 会耦合，数值失真。

对于相机外参估计，更可靠的方法是不依赖 RPY 矩阵分解，而是直接从相机前轴向量计算姿态角：

forward = R[:, 0]

yaw   = atan2(forward_y, forward_x)
pitch = atan2(forward_z, sqrt(forward_x² + forward_y²))

这种方法不受万向节锁影响，物理含义也更明确：yaw 表示相机在水平面上的指向，pitch 表示相机的俯仰角度。

8. 小结

PnP 是已知 3D 点坐标、2D 像素坐标和相机内参，求解相机外参的经典方法。
solvePnP 返回的 tvec 是从世界坐标系到相机坐标系的平移向量，相机在世界坐标系中的位置需要通过矩阵求逆获得。
cam2body-calib 将 PnP 外参估计流程工程化为一个完整工具，支持 Web UI 和 CLI 两种使用方式，内置鱼眼适配、坐标系转换和多种导出格式。
重投影误差是主要的精度指标，但需要结合可视化结果综合判断。
坐标系问题是 PnP 实践中最容易出错的环节：optical frame 与 link frame 轴方向不同，左手系下需要 S @ R @ S 转换才能正确提取 RPY，forward axis 方法可以规避万向节锁。