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

相关推荐
九亿AI算法优化工作室&19 分钟前
结合大语言模型的机械臂抓取操作学习
人工智能·学习·语言模型·自然语言处理
kaamelai19 分钟前
Kaamel视角下的MCP安全最佳实践
大数据·人工智能·安全
我要学脑机23 分钟前
基于常微分方程的神经网络(Neural ODE)
人工智能·深度学习·神经网络
有颜有货34 分钟前
2025汽车制造企业数字化转型路径参考
人工智能·汽车·制造·数字化转型
阿星AI工作室37 分钟前
小白也能用AI开发「小红书自动归档多维表格」采集神器,躺平整理笔记真香
人工智能
云天徽上38 分钟前
【数据可视化-42】杂货库存数据集可视化分析
人工智能·机器学习·信息可视化·数据挖掘·数据分析
大模型真好玩41 分钟前
初学者必看大模型微调指南:Unsloth官方微调技巧大公开!
人工智能·python
自由随风飘1 小时前
机器学习第三篇 模型评估(交叉验证)
人工智能·机器学习
vocal1 小时前
谷歌第七版Prompt Engineering—第三部分
人工智能·后端
ConardLi1 小时前
要给大家泼盆冷水了,使用 MCP 绝对不容忽视的一个问题!
前端·人工智能·后端