PnP(Perspective-n-Point):当知道n个3D空间点及其投影位置时，估计相机位姿。
2D-2D的对极几何方法需要八个或八个以上的点对(以八点法为例),且存在着初始化、纯旋转和尺度的问题。然而,如果两张图像中,其中一张特征点的 3D 位置已知（特征点的 3D 位置可以由三角化,或者由 RGB-D 相机的深度图确定）。那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。
因此,在双目或 RGB-D 的视觉里程计中,我们可以直接使用 PnP 估计相机运动。
而在单目视觉里程计中,必须先进行初始化,然后才能使用 PnP。
3D-2D 方法不需要使用对极约束,又可以在很少的匹配点中获得较好的运动估计,是最重要的一种姿态估计方法。
PnP有多种方法求解：直线线性变换（DLT）、P3P还有非线性方法，构建最小二乘问题并迭代求解（Bundle Adjustment）。
直线线性变换（DLT）
空间点P，齐次坐标为$P=left(X,Y,Z,1 ight)^{T}$
在图像$I_1$中，投影到特征点$x_1=left(u_1,v_1,1 ight)^{T}$(归一化平面齐次坐标表示)，并没有用到相机内参K，所以是归一化坐标而不是像素坐标。
定义增广矩阵$left[R|t ight]$为一个3X4的矩阵，包含旋转与平移信息。
$segin{bmatrix} u_1 \ v_1 \ 1 end{bmatrix} = egin{bmatrix} t_1 & t_2 & t_3 & t_4 \ t_5 & t_6 & t_7 & t_8 \ t_9 & t_{10} & t_{11} & t_{12} end{bmatrix}egin{bmatrix} X \ Y \ Z \1 end{bmatrix}$
$su_1 = t_1X + t_2Y + t_3Z + t_4$
$sv_1 = t_5X + t_6Y + t_7Z + t_8$
$s = t_9X + t_{10}Y + t_{11}Z + t_{12}$
把s消去：
$u_1 = frac{t_1X + t_2Y + t_3Z + t_4}{t_9X + t_{10}Y + t_{11}Z + t_{12}}$,$v_1 = frac{t_5X + t_6Y + t_7Z + t_8}{t_9X + t_{10}Y + t_{11}Z + t_{12}}$
$u_1 =frac{ extbf{t}_{1}^{T}P}{ extbf{t}_{3}^{T}P}$,$v_1 =frac{ extbf{t}_{2}^{T}P}{ extbf{t}_{3}^{T}P}$
一个特征点提供了两个关于t的约束。
t有１２维，最少需要６对匹配点求$left[R|t ight]$，为直接线性变换。
可由QR分解完成。
P3P
见书
Bundle Adjustment
除了以上DLT线性方法外，还可以构建非线性最小二乘问题。
线性方法先求相机位姿，再求空间点位置。
非线性优化则把相机位姿和空间点位置都当做优化变量，一起优化。
最小化重投影误差（Reprojection error）
n个三维空间点P及其投影p,位姿为R,t。李代数表示为$zeta$。
考虑n个三维空间点P及其投影p。
假设某空间点坐标为$P_i = left[X_i,Y_i,Z_i ight]^{T}$,投影坐标为$u_i=left[u_i,v_i ight]^{T}$
$expleft(zeta^{wedge} ight)P_i$结果是 4 × 1 的,而它左侧的 K 是 3 × 3 的,所以必须把$expleft(zeta^{wedge} ight)P_i$的前三维取出来,变成三维的非齐次坐标。
将像素坐标（观测到的投影位置）与３D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，即重投影误差。
使用牛顿高斯法需要知道每个误差关于优化变量的导数，也是线性化：
$eleft(x+Delta ight) hickapprox eleft(x ight) + JDelta{x}$,其中e为像素坐标误差。
优化相机位姿$zeta$
相机坐标系下的空间点坐标$P^{'}$
其中$delta{zeta}$是位姿增量相当于牛顿法中的$Delta{x}$,$eleft(deltazetaopluszeta ight)$相于与$fleft(x+Delta{x} ight)$,f为误差函数，$oplus$为李代数上的左扰动。
设观测值$left[u^{'},v^{'} ight]^{T}$。
$e = left[u^{'}-left(f_xfrac{X^{'}}{Z^{'}}+c_x ight), v^{'}-left(f_yfrac{Y^{'}}{Z^{'}}+c_y ight) ight]^{T}$
$P^{'}=left[X^{'},Y^{'},Z^{'} ight]^{T}$
$frac{partial{P}^{'}}{partialdeltazeta}=frac{partialleft({TP} ight)}{partialdeltazeta}$
以下推导过程$p$表示$P$,即空间点。
其中$deltazeta = left[delta ho,deltaphi ight]^{T}$,$a^{wedge}b = -b^{wedge}a$
优化空间点$P$
其中$frac{partial{P{'}}}{partial{deltazeta}}$雅克比的推导方法也可参考：https://blog.csdn.net/zhubaohua_bupt/article/details/74011005
$delta{zeta}=left[delta ho,deltaphi ight]$,$P^{'}=left[X^{'},Y^{'},Z^{'} ight]$
$delta{zeta}=left[delta ho_1,delta ho_2,delta ho_3,deltaphi_1,deltaphi_2,deltaphi_3 ight]$
$delta{phi}^{wedge} = egin{bmatrix} 0 & -deltaphi_3 & deltaphi_2 \ deltaphi_3 & 0 & -deltaphi_1 \ -deltaphi_2 & deltaphi_1 & 0 end {bmatrix}$
$delta{phi}^{wedge}P^{'}+delta ho = egin{bmatrix} -deltaphi_3x_2+deltaphi_2x_3 + delta ho_1 \ deltaphi_3x_1- deltaphi_1x_3 + delta ho_2 \ -deltaphi_2x_1+deltaphi_1x_2 + delta ho_3 end{bmatrix}$
对$delta{zeta}=left[delta ho_1,delta ho_2,delta ho_3,deltaphi_1,deltaphi_2,deltaphi_3 ight]$求导，得$egin {bmatrix} 1 & 0 & 0 & 0 & -Z^{'} & Y^{'} \ 0 & 1 & 0 & Z^{'} & 0 & -X^{'} \ 0 & 0 & 1 & -Y^{'} & X^{'} & 0 end{bmatrix}$，即$left[I_{3x3},-P_{3x3}^{'wedge} ight]$