OpenCV：基于空间参考点估算相机相对世界坐标系的位置

你现有采集方案的核心逻辑是光轴共线约束，和solvePnP依赖的「3D点-2D像素投影约束」输入形式不匹配，所以无法直接套用OpenCV的PnP接口，可按以下流程实现求解：

前置约束梳理 #

• 已知固定输入：6个参考点的世界坐标 P_i = (X_i, Y_i, Z_i) (i=1~6)，每个参考点对准画面中心时相机返回的水平转角pan_i、垂直转角tilt_i
• 待求参数：相机光心在世界坐标系下的3维坐标C=(Cx, Cy, Cz)，相机坐标系与世界坐标系之间的固定旋转偏移R（相机安装完成后该值为常量，对应初始对准世界零点时的朝向偏差，共3个自由度）
• 核心几何关系：当参考点被对准到画面中心时，参考点恰好落在相机光轴上，即「相机光心指向参考点的世界坐标射线」与「当前pan/tilt角度对应的光轴朝向」完全重合，该约束天然规避了径向畸变、相机内参焦距/主点的干扰。

角度到方向向量的映射 #

先匹配你所用PTZ相机的角度定义（行业通用规则如下，若设备定义方向相反仅需调整对应项符号即可）：

• pan为绕相机垂直轴的水平旋转角，0值对应初始对准世界零点的水平朝向
• tilt为绕相机水平横轴的垂直旋转角，0值对应初始对准世界零点的垂直朝向
  任意一组(pan_i, tilt_i)对应的相机坐标系下光轴单位方向向量为：

d_cam_i = [
    sin(pan_i) * cos(tilt_i),
    cos(pan_i) * cos(tilt_i),
    sin(tilt_i)
]

结合固定旋转矩阵R，可得到世界坐标系下的共线约束：

(P_i - C) / ||P_i - C|| = R · d_cam_i
式中||·||表示向量求模长。

求解实现流程 #

该问题为超定非线性优化问题，无需依赖PnP类接口，直接构造最小二乘问题求解即可：

1. 参数初始化
   • 光心坐标C的初值设为世界坐标系原点，和你初始对准零点的操作匹配，初值离真实值偏差极小
   • 旋转矩阵R的初值设为3阶单位矩阵，对应初始状态光轴与世界坐标系Z轴重合的假设
2. 残差构造
   对每个参考点i，构造残差项为|| (P_i - C)/||P_i - C|| - R · d_cam_i ||，优化目标为所有参考点的残差平方和最小
3. 迭代求解
   调用非线性最小二乘求解器迭代至残差收敛即可：仅依赖OpenCV的话可使用cv::LMSolver实现列文伯格-马夸尔特迭代，若有其他优化库（Ceres、G2O）也可直接替换。

实操避坑点 #

• 求解前必须核对相机返回的pan、tilt角度正方向：多数商用PTZ相机tilt角0值为水平朝前、向下旋转角度为正，和前文给出的方向向量公式存在符号差异，可先取2个已知相对位置的点做方向校验，避免因角度方向错误导致优化不收敛
• 6个参考点不要共面布设，尽量在空间内沿X/Y/Z三个轴向错开分布，避免参数退化导致求解歧义
• 不要额外引入内参、畸变系数作为待求量，你的采集方案已经完全排除了这类参数的影响，多余待求量反而会导致优化不稳定。

内容的提问来源于stack exchange，提问作者SyMe