EPnP

https://www.cnblogs.com/shang-slam/p/6481344.html

EPnP在ORB-SLAM中主要用于Tracking线程中的重定位Relocalization模块，需要通过当前关键帧Bow与候选帧匹配上的3D地图点，迅速建立当前相机的初始姿态。

PnP问题解决了已知世界参考系下地图点以及相机参考系下投影点位置时3D-2D相机位姿估计问题，不需要使用对极约束（存在初始化，纯旋转和尺度问题，且一般需要8对点），可以在较少的匹配点（最少3对点，P3P方法）中获得较好的运动估计，是最重要的一种姿态估计方法。最后，如果知道世界参考系下的地图点，同时知道相机参考系下的地图点，可以通过ICP的方法去求解姿态。

这样就构成了立体视觉中最重要的对极几何，PnP和ICP三种最常用的姿态估计方法。

---

 

求解PnP问题目前主要有直接线性变换（DLT），P3P，EPnP，UPnP以及非线性优化方法。

DLT

直接构建一个12个未知数的[R|t]增广矩阵（先不考虑旋转矩阵的自由度只有3），取六个点对，去求解12个未知数（每一个3D点到归一化平面的映射给出两个约束），最后将[R|t]左侧3*3矩阵块进行QR分解，用一个旋转矩阵去近似（将3*3矩阵空间投影到SE(3)流形上）。

P3P

P3P方法是通过3对3D/2D匹配点，求解出四种可能的姿态，在OpenCV calib3d模块中有实现，但是对于相机远离3D平面（low parallax）或者视角垂直于3D平面的情况下效果不佳，不知道最近有没有更新过。论文Complete Solution Classification for the Perspective-Three-Point Problem中提到了一种改进的方法，可以消除这种退化的情况。

将世界坐标系下的ABC三点和图像坐标系下的abc三点匹配，其中AB，BC，AC的长度已知，<a,b>，<b,c>，<a,c>也是已知，通过余弦定理可以求出A，B，C在相机参考系中的坐标，然后使用类似ICP的坐标系对齐，就可以求得当前相机薇姿。

通过余弦定理构建二元二次方程组【2】可以求解出OA，OB，OC之间的长度比例，从而确定世界坐标系下的相机位姿。可以想象一下为什么会出现四个解，在空间中的位置是什么样的，以及为什么在远离3D点平面或者视角垂直3D点平面时，会出现退化情况。

EPnP

需要4对不共面的（对于共面的情况只需要3对）3D-2D匹配点，是目前最有效的PnP求解方法。

The aim of the Perspective-n-Point problem—PnP in short—is to determine the position and orientation of a camera given its intrinsic parameters and a set of n correspondences between 3D points and their 2D projections. It has many applications in Computer Vision, Robotics, Augmented Re- ality and has receivedmuch attention in both the Photogrammetry and Computer Vision communities. In particular, applications such as feature point-based camera tracking require dealing with hundreds of noisy feature points in real-time, which requires computationally efficient methods.

空间中任意3D点的坐标可以用4个不共面的3D点坐标的权重表示

通常选取世界坐标下的四个控制点坐标为C^w=[0,0,0,1]^T, [1,0,0,1]^T,[0,1,0,1]^T,[0,0,1,1]^T；通过n个3D点在相机平面的投影关系，以及与这四个控制点的权重关系，构建一个12*12方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出相机位姿。

通常在用EPnP求得四对点下的封闭解后，可以将该解作为非线性优化的初值，优化提高精度。

EPnP算法

EPnP算法

相机坐标系用Fc

，世界坐标系用Fw表示，任何一点可以用四个控制点pwi表示

pwi=∑j=14αijcwj,with∑j=14αij=1(1)

对于相机坐标系同样成立

pci=∑j=14αijccj,with∑j=14αij=1(2)

对于上面的公式，首先需要说明的是αij确实存在。因为cwj或ccj

构成的方程组是欠定的，所以一定存在解。
理论上来说，控制点可以随便选择，这里选择控制点为参考点的中心，其他的点在PCA得到的主轴上单位长度处，从而提高算法的稳定性。

控制点在相机坐标系的坐标

根据投影方程得到世界坐标系中参考点坐标和相机坐标系中参考点的约束关系：

∀i,ωi[ui 1]=Apci=A∑j=14αijccj(3)

写成矩阵的形式为：

∀i,ωi[ui vi 1]=[fu0uc 0fvvc 001]∑j=14αij[xcj ycj zcj](4)

将等式的第三列代入第一二列，得到

∑j=14αijfuxcj+αij(uc−ui)zcj=0(5)

∑j=14αijfvycj+αij(vc−vi)zcj=0(6)

因此，可以得到下面的线性方程组：

Mx=0,withx=[ccT1,ccT2,ccT3,ccT4]T(7)

上面的方程中，四个控制点总共12个未知变量，M为2n×12的矩阵。因此，x属于M的右零空间，vi为矩阵M的右奇异向量，可以通过求解MTM的零空间特征值得到。

x=∑i=1Nβivi(8)

[说明]使用MTM比使用M计算量更少，因为MTM是求解是常数复杂度，而M是O(n3)的复杂度，但是计算MTM的复杂度是O(n)

的。

选择合适的线性组合

上面求解的x

中，需要确定βi，也就是确定合适的线性组合。根据参考点的位置不同，矩阵MTM的零空间维数可能为N=1→4维。求解β的策略是控制点在坐标系w和c中，两两之间的距离是相同，而x的3k+1−3k分量表示分别表示不同的控制点在相机坐标系中的坐标，总共有C24=6个约束。
如果N=1，则根据约束有

‖βv[i]−βv[j]‖2=‖cwi−cwj‖2(9)

所以

β=∑[i,j]∈[1;4]‖v[i]−v[j]‖⋅‖cwi−cwj‖∑[i,j]∈[1;4]‖v[i]−v[j]‖2

如果N=2，

‖β1v[i]1+β2v[i]2−(β1v[j]1+β2v[j]2)‖2=‖cwi−cwj‖2(10)

由于β1和β2只以二次项出现在方程中，记β=[β21,β1β2,β22]T， ρ的每一项为‖cwi−cwj‖2，得到相面的方程

Lβ=ρ(11)

其中L是由v1和v2构成的6×3的矩阵。
上面的方程可以通过β=(LTL)−1LTρ得到，然后通过选择合适的符号从β21,β1β2,β22使得所有的pci有正的z

坐标。

如果N=3

则和N=2差不多，唯一的区别在于使用的是L的逆，而不是伪逆，此时的L为6×6

的矩阵。

G-N优化

前面的步骤可以得到目标点在相机坐标系中的闭式解，作为G-N优化的初始值，优化的变量为β=[β1,⋯,βN]T

，目标函数为

Error(β)=∑(i,j)s.t.i<j(‖cci−ccj‖2−‖cwi−cwj‖2)(12)

该优化过程和参考点的数目无关，优化步骤和时间是常数。

计算R，t

前面的两步计算不同维数的零空间的误差，选择误差最小维数对应的β

，从而得到x，恢复出控制点在相机坐标系中的坐标并根据质心坐标系数得到参考点在相机坐标系的坐标。剩下的工作就是已知一组点云在两个坐标系中的坐标，求两个坐标系的位姿变换。
步骤如下：
（1）求中心点，pcc=∑picN，pcw=∑piwN；
（2）去中心，qic=pic−pcc,qiw=piw−pcw；
（3）计算H矩阵，H=∑Ni=1qicqiTw
（4）对H进行SVD，H=UΛVT；
（5）计算X=VUT，如果det(x)=1，则R=X，t=Pcc−RPcw。否则R(2,⋅)=−R(2,⋅)

https://www.cnblogs.com/jian-li/p/5689122.html

https://www.cnblogs.com/jian-li/p/5689122.html