3D-2D估计
输入:3d空间点 2d像素点，输出:相机的位姿
此时相机位姿估计已知一张图片含有3D信息，另一个只有2D信息，此问题采取特征点法估计若干图像中的相机运动。思路为先从图像中提取若干特征，然后在3D-2D图像间进行特征匹配，这样就得到许多匹配好的点，再根据这些点进行相机位姿的求解，即求解相机相对于世界坐标系的姿态改变量R,t。其旋转矩阵设为R，平移向量设为t。
3D-2D估计本质是pespective-n-point（PnP）一个问题，即给定世界坐标系中n个3D点及其在图像中的相应2D投影的情况下，估计校准相机的姿势的问题。相机姿势由6个自由度（DOF）组成，其由旋转（滚动，俯仰和偏航）以及相机相对于世界的3D平移构成。该问题源于相机校准，并且在计算机视觉和其他领域中具有许多应用，包括3D姿态估计、机器人和增强现实。 对于n = 3，存在一个常用的问题解决方案，称为P3P，并且许多解决方案适用于n≥3的一般情况。即PNP问题
求解方法：
(1)直接线性变换DLT：
最少通过6对匹配点实现矩阵TT的线性求解，当匹配点多于6对时，可以使用SVDSVD对超定方程求最小二乘解。
(2)3对点估计位姿的P3P:
3D−2D匹配点，3D点是A,B,C(世界坐标系中坐标)，2D点是a,b,c(成像平面像素坐标)
P3P只利用3个点的信息，当给定匹配点多余3组时，难以利用更多信息。
如果3D点或者2D点受噪声影响，或者存在误匹配，算法失效。
在SLAM中，通常先使用 P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)。
(3)使用BundleAdjustment优化求解位姿
PNP问题
PNP问题的描述以及定义是相对简单的，他的目的就是求解3D-2D点对运动的方法。简单来说，就是在已知n个三维空间点坐标（相对于某个指定的坐标系A）及其二维投影位置的情况下，如何估计相机的位姿（即相机在坐标系A下的姿态）。举个例子，我们在一幅图像中，知道其中至少四个图像中确定的点在3D空间下的相对坐标位置，我们就可以估计出相机相对于这些点的姿态，或者说估计出这些3D点在相机坐标系下姿态。（上述说的姿态或者位姿，包括位置以及方向，即一个6自由度的状态）即给定世界参考系中的一组n个3D点及其对应的2D图像投影以及校准的固有相机参数，以相对于世界的旋转和平移的形式确定相机的6 DOF位姿。
即PNP方法是为了解决在当前两帧图像中，已知前一帧图像上的3d特征点和当前帧的2d特征点，求取当前帧的pose
PNP问题的可解性
1. 当PNP中N=1时，即只知道一对3D-2D点对的情况下：
当只有一个特征点P1，我们假设它就在图像的正中央，那么显然向量OcP1就是相机坐标系中的Z轴，此时相机永远是面对P1，于是相机可能的位置就是在以P1为球心的球面上，再一个就是球的半径也无法确定，于是有无数个解。
如上图（2D示意图）的示例，Oc有可能在O1球上的任何一点，或者O2或O3球上的任何一点。
2. 当PNP中N=2时，即只知道两对3D-2D点对的情况下：
现在多了一个约束条件，显然OcP1P2形成一个三角形，由于P1、P2两点位置确定，三角形的边P1P2确定，再加上向量OcP1，OcP2，根据小孔模型，从Oc点射线特征点的方向角也能确定，于是能够计算出OcP1的长度=r1，OcP2的长度=r2。于是这种情况下得到两个球：以P1为球心，半径为r1的球A；以P2为球心，半径为r2的球B。显然，相机位于球A，球B的相交处,应该是一个圆圈，依旧是无数个解。
3. 当PNP中N=3时，即只知道三对3D-2D点对的情况下：
与上述相似，这次又多了一个以P3为球心的球C，相机这次位于ABC三个球面的相交处，终于不再是无数个解了，但是这次应该会有4个解，即获得四个旋转矩阵、平移矩阵，其中一个就是我们需要的真解了。
4. 当PNP中N>3时，即知道三对以上3D-2D点对的情况下：
解法：而是先用3个点计算出4组解获得四个旋转矩阵、平移矩阵。根据公式：
其中(x,y,1)为像素平面上的坐标，（X,Y,Z,1)为世界坐标系在的坐标
将第四个点的世界坐标代入公式，获得其在图像中的四个投影（一个解对应一个投影），取出其中投影误差(与已知的图像上的2D点坐标的误差）最小的那个解，就是我们所需要的正解。
PNP问题求解方法
这里以P3P问题进行解析
假设空间中有A,B,C三点，投影到成像平面中有a,b,c三点，在PnP问题中，A,B,C在世界坐标系下的坐标是已知的，但是在相机坐标系下的坐标是未知的。a,b,c的坐标是已知的。PnP的目的就是要求解A,B,C在相机坐标系下的坐标值。如下图所示。需要注意的是三角形abc和三角形ABC不一定是平行的。
根据余弦定理有：
记x=OA/OC,y=OB/OC，因为A,B,C在相机坐标系中的坐标未知，因此x，y是未知的。
另记u=BC2/AB2,w=AC/AB, 根据A,B,C的世界坐标，u,w是可以求出的。
通过一系列的转化可以得到两个等式：
关于P3P的具体求解过程可参考以下笔记：
相机位姿求解——P3P问题 有实现代码
P3P公式推导
相机位姿估计入门
学习笔记之——P3P与ICP位姿估计算法及实验 有最小化重投影误差