从ORBSlam里学知识——EPnP

如果EPnP论文不太好找的话可以从我的百度网盘下：链接：https://pan.baidu.com/s/1pY18HkLMsjfW3Zxe9Sis3A 
提取码：mzek

看了一些讲EPnP的博客，感觉大家都是上来就推公式，也没有个前戏啥的，干巴巴，我觉得对那些之前没接触过但是想深入了解的童鞋来说不太友好，我想换个思路来讲这个O(n)的方法，注重理解。另外提前说明一下，我希望我的博客大多都是白话，就像在聊天一样，最终把它讲清楚，所以可能有的地方会“废话”多一些，比如开个车啥的，不影响我们的共同目的就好！

总述：

EPnP说白了就是在解决一个问题，如何将世界坐标系下的3D点和各自投影到相机平面的2D点结合起来，计算得到世界坐标系到相机坐标系的R和t。EPnP的思路也比较简单，就是想先把将2D恢复到相机坐标系下的3D，然后再和世界坐标系的3D进行ICP，求出来R和t就完了，是不是挺简单，但是这有一个问题，2D怎么回到3D，如果简简单单的用这个图片就能回去那还搞什么前端啊对吧，所以整个问题的核心就是这里，我们需要给2D加一些别的约束，来求出3D。那么EPnP是这样想的：Rt是一个刚体变换，并没有失去结构信息，比如一个杯子，杯子的底座在世界坐标系下相对于杯子中心的位置在变到相机坐标系之后并不变，因为刚体变换不改变物体本身的样子，所以我们就从这个点开始说起！

我们重新梳理一下：

EPnP的输入：

• 世界坐标系下的3D点
• 对应投影到相机屏幕的2D点
• 相机内参

EPnP输出：

• 世界坐标系到相机坐标系的转换关系R和t

EPnP思路：

• 利用结构信息不变性将2D恢复到3D坐标（相机坐标系下）
• 与世界坐标系的3D坐标进行ICP，求得R和t

现在我们开始：

第一步：找出世界坐标系下的控制点wC

上面这张图我把两个坐标系下的点列出来了，如果要我们找结构信息，那我们首先会想到欧氏距离，也就是他们的坐标，或者说是相对位置，好像我们可以明显感觉到同一个点在两个坐标系下的坐标有一种强相关，但是直接用坐标来计算距离真的是太麻烦了，而且那么多点，把每个点都计算一遍明显不现实。这时候EPnP站出来了，他说既然都是算相对距离，那我干脆找几个标志物，让每个点都相对于这些个标志物有一个相对位置，那么不管坐标系怎么变，相对位置肯定不会变的对吧，这样就不用处理那么多点了，好，说干就干，怎么选标志物呢，中心肯定算一个，那剩下的呢？我既然在三维上，那我就找点集相对比较集中的3个方向好了，交给PCA来做好了，现在我就得到了世界坐标系下的4个标志点c1,c2,c3,c4，我们把它叫做控制点。

第二步：找出世界坐标系下每个点关于控制点的相对坐标α

有了控制点，我们需要找每个点相对于控制点的位置，也就是需要4个权重来表示这每个点，点坐标我知道，控制点坐标我也知道，通过上图这么一算我就知道了每个点的相对位置，用α来表示就是α1,α2,α3,α4，也就相当于知道了他们的结构信息，🆗，由总述我们知道相对位置是不变的，也就是相机坐标系下相对位置也是α；上图中wXi表示世界坐标系下的3D坐标，c0~c3是世界坐标系下的控制点（上一步已经求出来的），α0~α3是相对坐标，wc0是中心坐标，通过减去中心坐标，可以利用公式求出来α1~α3，然后我不能修改3D点距离中心点的长度，所以权重肯定得为1，所以α0就用1减去他们三个就好了(这里被视作一个α的约束, 即和为1)；

第三步：由两个约束构建等式MX=0

上上图中红笔写的代表还未求出来的，蓝笔代表已求出来的，可以看到相机坐标系下的3D点的结构约束已经被我们找出来了，再利用投影约束就就可以写出等式了，如下图所示。构造这样的等式可以将相机坐标系下的3D点消掉（下图的红框），从而只留下相机坐标系下的控制点作为未知量，而且是12维的。

每对3D-2D匹配点能够提供2个约束，而我们的未知量是一个12维的向量，所以看样子我们就可以用ICP求出来R和t，大功告成了！

第四步：其实还没完,我们还需要求解尺度

我们求出来这有一个向量(12x1)之后，还缺少尺度因子，因为我们求出来的向量大多会归一化操作，之后就不知道该向量真实的模长．所以这里还需要再求一下各个向量的系数，首先可以解决向量模长的问题，其次可以解出各个向量的权重问题，所以这样的话就得去找M的零空间了（因为是求Mx=0，M是矩阵，x是向量，把构成这种等式的向量张成的空间叫做矩阵M的零空间，所以向量x应该处于M的零空间内才可以出现等于0），对M求SVD和对MTM求特征值是一样的，而且整个EPnP的复杂度体现在这里，所以是O(n)。而且我现在要求的x是MTM的零空间，所以我要找MTM的最小特征值对应的特征向量，才是x，那么问题来了，难道这里只取这么一个特征值很小的特征向量吗？

第五步：处理特征向量的问题

EPnP说：非也！当我采用6对匹配点的时候，没有自由度,  直接就可以求出来(一对点提供2个约束)，所以这种情况下用一个特征向量就行了; 但是如果点数量很少, 如只有4对时(为什么只有4对, 因为EPnP要求控制点选4个, 且是不共面的4个点, 因为如果控制点是3个的话, 加上α约束是4个方程, 但是只有三个未知数, 所以只是得到一个最小二乘解, 无法得到精确解, 所以最少的匹配点对数为4, 这也就是为什么后面的N最大是4就可以了. 另外4个控制点相当于一个齐次解, 本质上: 3D参考点的齐次坐标是控制点齐次坐标的线性组合), 或者焦距变得很大、或者有噪声的时候，最多可以求出来4个为0的特征值（当噪声很大的时候可能特征值不是准确的为0，但是已经很小很小了），像上图所示，焦距f=100时只有第12个特征值是0，所以只要最后一个特征向量就可以了，但是当焦距f=10000时，第9，10，11，12特征值都是0了，也就是说我只用一个特征向量是没有办法表示的，那我就只能把这4个加权来表示下。然后很自然的给特征向量的权重起名为β，且最多有4个，即N最大为4。

第六步：已知特征向量，求取权重β

我们再来梳理一下：现在我已经把这些特征向量求出来了，接下来要求一个权重因子，把特征向量加权来表示未知量x，再次声明：x是12x1的向量，表示相机坐标系下的4个控制点坐标，所以这一步我们要用β来表示这4个控制点坐标，然后再找约束把β求出来。上一步知道N可以取1，2，3，4，取不同的N得到的β也是不一样的，所以EPnP分4种情况来讨论这个，但是约束从哪里来呢，我们知道结构信息是不变的，所以控制点之间的距离也是不变的，所以C42有6个距离，那就是世界坐标系下的某两个点距离与相机坐标系下同样点之间的距离是一样的，所以一共有6个约束。

N=1的时候就一个β，直接就可以求出来，N=2的时候我们把距离表示出来发现有平方项呢，所以是3个未知量，也能求，N=3的时候6个未知数也可以hold住，N=4的时候就不行了，约束不够了. (ORBSlam中直接求的N=4时候的情况). EPnP说不怕我们用线性化方法，先直接令β中的某几个项为0, 能够求出一个粗略的初始值, 再通过GN优化求解精确值，比如我直接令上图β向量中的后几个都为0，把未知量数量控制住，当然也可以把其他的某几项设为0，反正能够用6个约束求出来一些未知量就行，然后开始万金油方法：优化。

第七步：GN优化得到最后的β

我们把上一步求出来的不太准的β放到GN优化器里面去当一个比较靠谱的初始值，依旧利用控制点之间的距离平方差作为残差，然后采用朴素的GN方法实现最后的β的求解，最后得到相机坐标系的控制点坐标. 为了防止出现误差, 可以多试几次, 每次让别的β等于0. 最后找出最好的β就行. 再通过ICP和世界坐标系下的控制点坐标求出来R和t，就算大功告成了。

总结：

看了挺多次论文，之前总是不懂为什么这样做，看别的博主觉得公式讲的很清楚，但是不知道为什么这样做，所以看完跟没看一样，直到最近听大佬将，对EPnP的思想有了自己的理解，再深入去想一想，就能明白很多东西。俗话说能讲出来才是真正懂了，我就试着用博客来讲一讲，如果有讲的比较乱的地方希望大家多多指正！