视觉SLAM前端——PnP

如果我们已知多对匹配好的2D/3D特征点，我们可以用PnP方法去求得相机的运动。对于单目相机来说，我们可以通过利用对极约束或者单应性求得相机的运动，然后三角化获得3D点；对于双目可以通过视差恢复深度信息从而获得3D点；对于RGB-D相机来说深度信息可以直接测量，因此3D点可以根据内参直接算出。

直接变换法（DLT）

如果我们知道空间中的某个点在世界坐标系下（或者上一帧相机坐标系下）的齐次坐标为$\mathbf{P}=[x,y,z,1{]}^T$，其在当前帧相机坐标系下的归一化坐标为$x$（其与像素坐标差了一个内参），当前帧相机坐标系相对世界坐标系（或者上一帧相机坐标系）的相对位姿为旋转矩阵$\mathbf{R}$和平移向量$\mathbf{t}$（这正是我们需要计算的量），那么根据相机模型有：
$$s\mathbf{x}=[\mathbf{R}|\mathbf{t}]\mathbf{P}⟹s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathbf{r}}_1^{\mathbf{T}}\\ {\mathbf{r}}_2^{\mathbf{T}}\\ {\mathbf{r}}_3^{\mathbf{T}}\end{array}\right]\mathbf{P}$$
其中$s$是空间点在当前帧相机坐标系下的深度，${\mathbf{r}}^T$表示矩阵$[\mathbf{R}|\mathbf{t}]$的行向量。通过消去$s$可得两个约束方程（这里用到了向量的点乘满足交换律的性质）：
$${\mathbf{r}}_1^{\mathbf{T}}\mathbf{P}-su={\mathbf{r}}_1^{\mathbf{T}}\mathbf{P}-{\mathbf{r}}_3^{\mathbf{T}}\mathbf{P}u={\mathbf{P}}^T{\mathbf{r}}_1-u{\mathbf{P}}^T{\mathbf{r}}_3=0$$$${\mathbf{r}}_2^{\mathbf{T}}\mathbf{P}-sv={\mathbf{r}}_2^{\mathbf{T}}\mathbf{P}-{\mathbf{r}}_3^{\mathbf{T}}\mathbf{P}v={\mathbf{P}}^T{\mathbf{r}}_2-v{\mathbf{P}}^T{\mathbf{r}}_3=0$$
那么对于$n$对匹配的点可得如下线性方程组：
$$\left(\begin{array}{ccc}{\mathbf{P}}_1^{\mathrm{T}}& 0& -u_1{\mathbf{P}}_1^{\mathrm{T}}\\ 0& {\mathbf{P}}_1^{\mathrm{T}}& -v_1{\mathbf{P}}_1^{\mathrm{T}}\\ ⋮& ⋮& ⋮\\ {\mathbf{P}}_n^{\mathrm{T}}& 0& -u_n{\mathbf{P}}_n^{\mathrm{T}}\\ 0& {\mathbf{P}}_n^{\mathrm{T}}& -v_n{\mathbf{P}}_n^{\mathrm{T}}\end{array}\right)\left(\begin{array}{l}{\mathbf{r}}_1\\ {\mathbf{r}}_2\\ {\mathbf{r}}_3\end{array}\right)=0$$
至少需要6对匹配的点通过SVD分解即可求得上述方程组的最小二乘解，但是由于没有考虑旋转矩阵的约束条件，求得的$\mathbf{R}$不一定是正交群，可以通过QR分解或者取$(\mathbf{R}{\mathbf{R}}^{\mathbf{T}}{)}^{-\frac{1}{2}}\mathbf{R}$。

P3P

根据3对匹配点，利用三个三角形相似性，根据余弦定理可以构造两个二元二次方程，通过吴消元可以得到四个解析解，然后通过验证点来获取最适合的解。根据方程的解，可以获取3个点在相机坐标系下的3D坐标，此时可以把问题转换为3D/3D问题，利用ICP求出相机运动。由于P3P只能利用三对点的信息，如果存在误匹配，对结果影响很大。

EPnP

EPnP跟P3P相似，也是先求得特征点在相机坐标系下的3D坐标，然后利用ICP求解，不同是的EPnP是利用4个具有代表性的控制点来间接求得3D坐标的。由于空间中的任意一点可以由4个不共面的点进行加权平均，对于世界坐标系下的某个点$p_i^w$可表达为：
$$P_i^w=\sum _{j=1}^4{\alpha }_i{\mathbf{c}}_j^w,\sum _{j=1}^4{\alpha }_i=1$$
对应的相机坐标系下的点可表示为：
$$P_i^c=\sum _{j=1}^4{\alpha }_i{\mathbf{c}}_j^c$$
也就是说，对于某个空间点，其在世界坐标系下的坐标可由4个世界坐标系下的控制点进行加权平均，其在相机坐标系下的坐标也可由这4个控制点在相机坐标系下进行加权平均，并且两者权重是相同的。如果我们知道了权重和控制点在相机坐标系下的坐标，就可以求得特征点在相机坐标系下的3D坐标，然后使用ICP求解相机运动。
通常选择所有特征点的重心作为第一个控制点，即：$c_1^w=\frac{1}{n}{\sum }_{i=1}^n{P}_i^w$，然后通过每个世界坐标点与控制点作差得到到矩阵$A$，其中${\mathbf{a}}_i=p_i^w-c_1^w$。设$A^{T}A$的特征值和特征向量分别为${\lambda }_{c,j}$$和$${\mathbf{v}}_{c,j}$，那么剩下3个控制点可通过下式获得：
$$c_j^w=c_1^w+{\lambda }_{c,j-1}^{\frac{1}{2}}{\mathbf{v}}_{c,j-1},j=2,3,4$$
有了控制点之后，对于每一个特征点我们都可以计算出对应的权重。其权重在相机坐标系下依然适用，我们可以利用多个点的权重和其在相机坐标系下的像素坐标（归一化坐标），计算出在相机坐标系下的控制点坐标：
$$s{x}_i^c=P_i^c=\sum _{j=1}^4{\alpha }_i{\mathbf{c}}_j^c⟹s\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\sum _{j=1}^4{\alpha }_i\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]$$
每个特征点可以提供两个方程，共有12个未知数，通过提供至少6个点可解得相机坐标系下的控制点。

非线性优化方法（最小化重投影误差）

假如我们知道了相机运动$R$、$t$，就可以把世界坐标系中的某个点$P_i^w$投影到相机的成像平面上，但由于相机的运动估计的不是那么准确，可能与实际像素位置存在偏差，这个偏差称为重投影误差。
$${\mathbf{e}}_i=s_i{\mathbf{p}}_i-K{P}_i^c=s_i{\mathbf{p}}_i-K(R{P}_i^w+t)=s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]-K(R\left[\begin{array}{c}{x}_i^w\\ y_i^w\\ z_i^w\end{array}\right]+t)$$
我们可以通过优化方法不断迭代来使所有特征点的重投影误差达到最小，从而估计出最优的相机运动结果。优化目标如下：
$$T^{\ast }=\underset{R,t}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{e}}_i{\Vert }_2^2$$
为了更新优化变量，我们需要求得误差项关于优化变量的一阶导数，即雅克比矩阵。根据链式法则有：
$$\frac{\partial e_i}{\partial R}=\frac{\partial e_i}{\partial P_i^c}\frac{\partial P_i^c}{\partial R}，\frac{\partial e_i}{\partial t}=\frac{\partial e_i}{\partial P_i^c}\frac{\partial P_i^c}{\partial t}$$
$$\frac{\partial e_i}{\partial P_i^c}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial x_i^c}& \frac{\partial u}{\partial y_i^c}& \frac{\partial u}{\partial z_i^c}\\ \frac{\partial v}{\partial x_i^c}& \frac{\partial v}{\partial y_i^c}& \frac{\partial v}{\partial z_i^c}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{z_i^c}& 0& -\frac{f_x{x}_i^c}{z_i^{c2}}\\ 0& \frac{f_y}{z_i^c}& -\frac{f_y{y}_i^c}{z_i^{c2}}\end{array}\right]$$
$$\frac{\partial P_i^c}{\partial R}=\frac{\partial (R{P}_i^w+t)}{\partial R}=-(R{P}_i^w{)}^{\wedge }=-(P_i^c{)}^{\wedge }=\left[\begin{array}{ccc}0& -z_i^c& y_i^c\\ z_i^c& 0& -x_i^c\\ -y_i^c& x_i^c& 0\end{array}\right]，\frac{\partial P_i^c}{\partial t}=I$$
非线性优化的方法对于初始值比较敏感，我们可以先试用EPnP求解出相机的位姿，然后再使用优化的方式对位姿进行微调。