PnP——根据3D与2d图片估计相机运动

引入

当知道n个3D空间点及其投影位置和2d像素点时,如何估计相机的位姿。PnP(Perspective-n-Point) 是求解3D到2D点对运动的方法。

直接线性变换(DLT)

问题描述

已知一组3D点 P i = ( X i , Y i , Z i , 1 ) ⊤ \mathbf{P}_i = (X_i, Y_i, Z_i, 1)^\top Pi=(Xi,Yi,Zi,1)⊤ 及其在相机中的投影 x i = ( u i , v i , 1 ) ⊤ \mathbf{x}_i = (u_i, v_i, 1)^\top xi=(ui,vi,1)⊤,求相机的旋转矩阵 R \mathbf{R} R 和平移向量 t \mathbf{t} t。


求解步骤

1. 构建投影方程

齐次坐标投影关系:
s i [ u i v i 1 ] = [ R ∣ t ] P i s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = [\mathbf{R} |\mathbf{t}]\mathbf{P}_i si uivi1 =[R∣t]Pi

其中 R = [ r 1 , r 2 , r 3 ] ⊤ R=[r_1,r_2,r_3]^\top R=[r1,r2,r3]⊤ t = ( t x , t y , t z ) ⊤ \mathbf{t} = (t_x, t_y, t_z)^\top t=(tx,ty,tz)⊤
T = [ R ∣ t ] = [ T 1 , T 2 , T 3 ] ⊤ T=[\mathbf{R} |\mathbf{t}]=[\mathbf{T_1},\mathbf{T_2},\mathbf{T_3}]^\top T=[R∣t]=[T1,T2,T3]⊤

消去尺度因子 s i s_i si,得到两个方程:
{ T 1 T P i − T 3 T P i u i = 0 T 2 T P i − T 3 T P i v i = 0 \begin{cases} \mathbf{T_1^{T}}\mathbf{P}_i -\mathbf{T_3^{T}}\mathbf{P}_iu_i=0\\ \mathbf{T_2^{T}}\mathbf{P}_i -\mathbf{T_3^{T}}\mathbf{P}_iv_i=0 \end{cases} {T1TPi−T3TPiui=0T2TPi−T3TPivi=0


2. 构造线性方程组

对每个3D点 P i \mathbf{P}_i Pi,构造矩阵 A \mathbf{A} A 的两行:

P i 0 − u i P i 0 P i − v i P i \] \\begin{bmatrix} \\mathbf{P}_i \& 0 \& -u_i \\mathbf{P}_i \\\\ 0 \& \\mathbf{P}_i \& -v_i \\mathbf{P}_i \\end{bmatrix} \[Pi00Pi−uiPi−viPi

最终超定方程组为:
A T = 0 \mathbf{A} \mathbf{T} = \mathbf{0} AT=0


3. SVD求解投影矩阵

对 A \mathbf{A} A 进行奇异值分解:
A = U Σ V ⊤ \mathbf{A} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top A=UΣV⊤

取 V \mathbf{V} V 的最后一列作为解 T \mathbf{T} T。


4. 分解 R \mathbf{R} R 和 t \mathbf{t} t
  • 提取子矩阵
    T \mathbf{T} T的前三列为 R \mathbf{R} R,最后一列为 t \mathbf{t} t:
    R = [ r 1 ⊤ r 2 ⊤ r 3 ⊤ ] , t = [ t x t y t z ] \mathbf{R} = \begin{bmatrix} \mathbf{r}_1^\top \\ \mathbf{r}_2^\top \\ \mathbf{r}_3^\top \end{bmatrix}, \quad \mathbf{t} = \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix} R= r1⊤r2⊤r3⊤ ,t= txtytz
  • 正交化 R \mathbf{R} R
    在DLT求解中,我们直接将T矩阵看成了12个未知数,忽略了它们之间的联系。因为旋转矩阵R∈SO(3),用DLT求出的解不一定满足该约束。对于旋转矩阵R,我们必须针对DLT估计的T左边3×3的矩阵块,寻找一个最好的旋转矩阵对它进行近似。具体的做法是:
    对 R \mathbf{R} R 进行SVD分解:
    R = U Σ V ⊤ ⇒ R 正交 = U V ⊤ \mathbf{R} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top \quad \Rightarrow \quad \mathbf{R}_{\text{正交}} = \mathbf{U} \mathbf{V}^\top R=UΣV⊤⇒R正交=UV⊤
    若 det ⁡ ( R ) < 0 \det(\mathbf{R}) < 0 det(R)<0,需将 R \mathbf{R} R 的第三列取反。

最终答案

相机位姿的解为:
R , t \boxed{ \mathbf{R}, \ \mathbf{t} } R, t

相关推荐
lucky_lyovo43 分钟前
自然语言处理NLP---预训练模型与 BERT
人工智能·自然语言处理·bert
fantasy_arch1 小时前
pytorch例子计算两张图相似度
人工智能·pytorch·python
AndrewHZ2 小时前
【3D重建技术】如何基于遥感图像和DEM等数据进行城市级高精度三维重建?
图像处理·人工智能·深度学习·3d·dem·遥感图像·3d重建
飞哥数智坊2 小时前
Coze实战第18讲:Coze+计划任务,我终于实现了企微资讯简报的定时推送
人工智能·coze·trae
Code_流苏3 小时前
AI热点周报(8.10~8.16):AI界“冰火两重天“,GPT-5陷入热议,DeepSeek R2模型训练受阻?
人工智能·gpt·gpt5·deepseek r2·ai热点·本周周报
赴3353 小时前
矿物分类案列 (一)六种方法对数据的填充
人工智能·python·机器学习·分类·数据挖掘·sklearn·矿物分类
大模型真好玩3 小时前
一文深度解析OpenAI近期发布系列大模型:意欲一统大模型江湖?
人工智能·python·mcp
双翌视觉3 小时前
工业视觉检测中的常见的四种打光方式
人工智能·计算机视觉·视觉检测
念念01073 小时前
基于MATLAB多智能体强化学习的出租车资源配置优化系统设计与实现
大数据·人工智能·matlab
nonono4 小时前
深度学习——常见的神经网络
人工智能·深度学习·神经网络