引入

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

直接线性变换（DLT）

问题描述

已知一组3D点 ${\mathbf{P}}_i=(X_i,Y_i,Z_i,1{)}^{\top }$ 及其在相机中的投影 ${\mathbf{x}}_i=(u_i,v_i,1{)}^{\top }$，求相机的旋转矩阵 $\mathbf{R}$ 和平移向量 $\mathbf{t}$。

---

求解步骤

1. 构建投影方程

齐次坐标投影关系：
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=[\mathbf{R}|\mathbf{t}]{\mathbf{P}}_i$$
其中 $$R=[r_1,r_2,r_3{]}^{\top }$$$$\mathbf{t}=(t_x,t_y,t_z{)}^{\top }$$
$$T=[\mathbf{R}|\mathbf{t}]=[{\mathbf{T}}_1,{\mathbf{T}}_2,{\mathbf{T}}_3{]}^{\top }$$
消去尺度因子 $s_i$，得到两个方程：
$$\{\begin{array}{l}{\mathbf{T}}_1^{\mathbf{T}}{\mathbf{P}}_i-{\mathbf{T}}_3^{\mathbf{T}}{\mathbf{P}}_i{u}_i=0\\ {\mathbf{T}}_2^{\mathbf{T}}{\mathbf{P}}_i-{\mathbf{T}}_3^{\mathbf{T}}{\mathbf{P}}_i{v}_i=0\end{array}$$

---

2. 构造线性方程组

对每个3D点 ${\mathbf{P}}_i$，构造矩阵 $\mathbf{A}$ 的两行：
$$\left[\begin{array}{ccc}{\mathbf{P}}_i& 0& -u_i{\mathbf{P}}_i\\ 0& {\mathbf{P}}_i& -v_i{\mathbf{P}}_i\end{array}\right]$$
最终超定方程组为：
$$\mathbf{AT}=0$$

---

3. SVD求解投影矩阵

对 $\mathbf{A}$ 进行奇异值分解：
$$\mathbf{A}=\mathbf{U\Sigma }{\mathbf{V}}^{\top }$$
取 $\mathbf{V}$ 的最后一列作为解 $\mathbf{T}$。

---

4. 分解 $\mathbf{R}$ 和 $\mathbf{t}$

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

---

最终答案

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