引入
当知道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