视觉SLAM十四讲学习5 位姿估计（4）PNP方法，DLT，P3P

前言

上篇记录了本质矩阵，基础矩阵，单应矩阵的求解。

本篇学习3D-2D点的匹配方法PnP(Perspective-n-Point)，DLT，P3P。

PnP

已知某相邻帧之间匹配点的2D和3D坐标（世界坐标），求当前的位姿，称为PnP。

在这里，已知匹配点的3D坐标往往是上一帧的点坐标，2D坐标是当前帧与上一帧匹配到的点坐标。

DLT

直接线性变换(DLT)是直接通过相机运动模型求解。假设P是3D点，P’是2D点对应在归一化坐标平面的3D点
$$\begin{gathered} P^{\prime }=K^{-1}p \\ s{P}^{\prime }=[R,t]P \\ s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{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{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right] \\ s=t_{9}X+t_{10}Y+t_{11}Z+t_{12} \\ u=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}} \\ v=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}} \\ {t}_{1}X+t_{2}Y+t_{3}Z+t_4-u(t_{9}X+t_{10}Y+t_{11}Z+t_{12})=0 \\ {t}_{5}X+t_{6}Y+t_{7}Z+t_8-v(t_{9}X+t_{10}Y+t_{11}Z+t_{12})=0 \end{gathered}$$
由上面的方程可知，一对匹配点能形成两个方程，而位姿矩阵有12个未知数，因此6对匹配点就能通过矩阵方程$Ax=0$求算位姿。

如果匹配点数多于6对，则方程编程超定方程组，可以使用线性最小二乘法求解（SVD），也就是把矩阵SVD分解$A=U\Sigma V^T$后，$V$的最后一个列向量是方程的解。

需要注意的是，$Ax=0,\lambda Ax=0$，因此求出的$R,t$也是没有尺度的，并且$R$不一定满足正交矩阵约束。具体处理方法《十四讲》并没有详细说明。OpenCV内置了这个方法，因此这里也不展开。

P3P

P3P通过三对匹配点求位姿，再通过另外一对点验证结果，因此需要四对点，且四点不能共面。

主要就是根据三角形Oab,OAB;Oac,OAC;Obc,OBC之间的相似，以及余弦定理，通过吴消元法计算出OA/OC,OB/OC，然后回代计算出OC。abc是归一化平面上的点。

余弦定理：
$$\begin{gathered} O{A}^2+O{B}^2-2OA\cdot OB\cos AOB=A{B}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cos BOC=B{C}^2 \\ O{A}^2+O{C}^2-2OA\cdot OC\cos AOC=A{C}^2 \end{gathered}$$
上面的式子中，OA,OB,OC未知，AB,BC,AC已知，cos值都可以通过相似获得：
$$\begin{gathered} \cos AOB=\cos aOb=\frac{O{a}^2+O{b}^2-a{b}^2}{2Oa\cdot Ob} \\ \cos BOC=\cos bOc=\frac{O{b}^2+O{c}^2-b{c}^2}{2Ob\cdot Oc} \\ \cos AOC=\cos aOc=\frac{O{a}^2+O{c}^2-a{c}^2}{2Oa\cdot Oc} \end{gathered}$$
然后就能通过消元求出OA,OB,OC，也就得到了3D点的相机坐标，于是变成了3D-3D的问题，通过ICP解决。

P3P只能通过4对点计算位姿，误差比较大。OpenCV也内置了这个方法

后记

下篇记录BA，重头戏。