(01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下:
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、背景介绍

论文： Lepetit V , Fua M N . EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of
Computer Vision, 2009.

该算法功能如下：

输入：
	1.n个世界坐标的下的3D点，论文中称为3D参考点。
	2.这n个3D点投影在图像上的2D坐标
	3. 相机内参矩阵 ，包括焦距和主点

输出: 相机的位姿R,t

应用：特征点的图像跟踪，需要实时处理有噪声的特征点，对计算精度和效率要求比较高，只需4对匹配点即可求解。

 
算法的优点：

	1.只需要4对非共面点，对于平面只需要3对点
	2.闭式解，不需要迭代，不需要初始估计值。
	3.精度比较高。和迭代法里精度最高的方法LHM方法精度相当。
	4.比较鲁棒，可以处理带噪声的数据。迭代法受到初始估计的影响比较大，会不稳定
	5.线性计算复杂度为O(n)
	6.平面和非平面都适用

 

二、原理和步骤：

$目的:$ n个世界坐标系下的3D点及其在图像上的2D投影点，还有相机还有相机内参，目的是为了求世界坐标系到相机坐标系下的位姿变换 $\mathbf{R},\mathbf{t}$ 。

EPnP的思路就是先把2D图像点通过内参变换到相机坐标系下的3D点，然后用ICP来求解3D-3D的变换就得到了位姿。那么问题的核心就转化为如何通过2D信息，加上一些约束，来得到相机坐标系下的3D点。因为我们这里的位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系下是不变的。我们称之为刚体结构不变性。后面就是紧紧围绕这个特性来求解的

$(1):$首先我们对3D点的表达方式进行了新的定义。之前不管是世界坐标系还是相机坐标系下的3D点，它们都是相对于自己坐标系下的原点的。那么两个坐标系原点不同，坐标的量级可能差异非常大，比如相机坐标系下3D点坐标范围可能是10-100之间，世界坐标系下坐标可能是1000-10000之间，这对求解优化都是不利的。所以我们要统一一下量级。可以理解为归一化吧，这在求基础矩阵、单应矩阵时都是常规手段。
        具体来说，我们对每个坐标系定义4个控制点，其中一个是质心（也就是各个方向均值），其他3个 用PCA从三个主方向选取，这4个控制点可以认为是参考基准，类似于坐标系里的基。所有的3D点都表达为这4个参考点的线性组合。这些系数我们称之为权重，为了不改变数据的相对距离，权重和必须为1。这样，我们就可以用世界坐标系或相机坐标系下的4个控制点表示所有的世界坐标系或相近坐标系下的3D点。

$(2):$利用投影方程将图像2D点恢复相机坐标系下3D点（未知量）。经过整理后，一组点对可以得到2个方程。我们待求的相机坐标系下3D点对应的4个控制点，每个控制点3个分量，总共12个未知数组成的一个向量。

$(3):$用SVD分解可以求解上述向量，但是因为恢复的相机坐标系下3D点还有个尺度因子 $\beta$， 这里我们根据结构信息不变性作为约束，求解。

$(4):$最后用高斯牛顿法优化上述求解的$\beta$ 。

注：ORB-SLAM里使用的EPnP是直接拷贝OpenCV里的源码：modules → calib3d → src → epnp.cpp
 

三、统一变量格式：

为了方便理解后续的公式推导，首先统一一下变量的定义格式：
$(1):$ 用上标 ${}^w$ 和 ${}^c$ 分别表示世界坐标系和相机坐标系中的坐标。

$(2):$ n个3D参考点在世界坐标系中的坐标是 $已知输入$，记为 ${\mathbf{p}}_i^w,i=1,\cdots ,n$。

$(3):$ n个3D参考点在相机坐标系下的坐标是 $未知的$，记为${\mathbf{p}}_i^c,i=1,\cdots ,n$。

$(4):$ n个3D参考点在相机坐标系下对应的n个2D投影坐标是 $已知输入$，记为${\mathbf{u}}_i,i=1,\cdots ,n$。

$(5):$ 4个控制点在世界坐标系的坐标为 ${\mathbf{c}}_j^w,j=1,\cdots ,4$

$(6):$ 4个控制点在世界坐标系的是 $未知的$，记为 ${\mathbf{c}}_j^c,j=1,\cdots ,4$

$(7):$ 4个控制点系数 ${\alpha }_{ij},i=1,\cdots ,n$，$j=1,\cdots ,4$,也就是也就是论文中的homogeneous barycentriccoordinates，我们翻译为齐次重心坐标。

注意: 同一3D点在世界坐标系下和相机坐标系下的控制点系数相同。后面会给出证明。另外以上坐标都是非齐次坐标，后面也都是非齐次坐标。
 

四、控制点如何选取

理论上，控制点的坐标可以任意选取。但在实践中，作者发现了一种可以提高结果稳定性的控制点选择
方法。具体如下
$(1):$将参考点的质心（或者称为重心、均值中心）设置为其中一个控制点，表达式如下。这是有一定物理意义的，因为后续会使用质心对坐标点进行归一化。$${\mathbf{c}}_1^w=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^w\text{\hspace{1em}(01)}$$$(2):$剩下的3个控制点从数据的三个主方向上选取。我们对世界坐标系下3D点集合 ${\mathbf{p}}_i^w,i=1,\cdots ,n$，去质心后得到$$\mathbf{A}=\left[\begin{array}{c}{\left({\mathbf{p}}_1^w\right)}^T-{\left({\mathbf{c}}_1^w\right)}^T\\ ⋮\\ {\left({\mathbf{p}}_n^w\right)}^T-{\left({\mathbf{c}}_1^w\right)}^T\end{array}\right]\text{\hspace{1em}(02)}$$$\mathbf{A}$是一个 nx3 的矩阵，那么 ${\mathbf{A}}^T\mathbf{A}$ 就是 3x3 的方阵，通过对矩阵 ${\mathbf{A}}^T\mathbf{A}$ 进行特征值分解，得到三个特征值 ${\lambda }_1^w,{\lambda }_2^w,{\lambda }_3^w$, 他们对应的特征向量为 $v_1^w,v_2^w,v_3^w$。 将剩余的3个控制点表示为$$\begin{array}{l}{\mathbf{c}}_2^w={\mathbf{c}}_1^w+\sqrt{\frac{{\lambda }_1^w}{n}}{v}_1^w\\ {\mathbf{c}}_3^w={\mathbf{c}}_1^w+\sqrt{\frac{{\lambda }_2^w}{n}}{v}_2^w\\ {\mathbf{c}}_4^w={\mathbf{c}}_1^w+\sqrt{\frac{{\lambda }_3^w}{n}}{v}_3^w\end{array}\text{\hspace{1em}(03)}$$为什么要加上$c_1^w$, 因为前面去了质心，所以要重新加上。这样就可以获得4在世界坐标系的控制点 ${\mathbf{c}}_j^w,j=1,\cdots ,4$。
 

五、计算控制点系数

我们将世界坐标系下3D点的坐标表示为对应控制点坐标的线性组合：$${\mathbf{p}}_i^w=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w,\sum _{j=1}^4{\alpha }_{ij}=1\text{\hspace{1em}(04)}$$在论文中，${\alpha }_{ij}$ 称为homogeneous barycentric coordinates，我们翻译为齐次重心坐标，它实际上表达的是世界坐标系下3D点在控制点坐标系下的坐标系数。当控制点 $c_j^w$ 通过第一步的方法确定后，${\alpha }_{ij}$也是唯一确定的，推导如下：$${\mathbf{p}}_i^w={\alpha }_{i1}{\mathbf{c}}_1^w+{\alpha }_{i2}{\mathbf{c}}_2^w+{\alpha }_{i3}{\mathbf{c}}_3^w+{\alpha }_{i4}{\mathbf{c}}_4^w\text{\hspace{1em}(05)}$$3D点的重心为，也是围 $c_1^w$，也是第一个控制点，上式左右分别减去重心：$$\begin{array}{rl}{\mathbf{p}}_i^w-{\mathbf{c}}_1^w& ={\alpha }_{i1}{\mathbf{c}}_1^w+{\alpha }_{i2}{\mathbf{c}}_2^w+{\alpha }_{i3}{\mathbf{c}}_3^w+{\alpha }_{i4}{\mathbf{c}}_4^w-{\mathbf{c}}_1^w\\ & ={\alpha }_{i1}{\mathbf{c}}_1^w+{\alpha }_{i2}{\mathbf{c}}_2^w+{\alpha }_{i3}{\mathbf{c}}_3^w+{\alpha }_{i4}{\mathbf{c}}_4^w-\left({\alpha }_{i1}+{\alpha }_{i2}+{\alpha }_{i3}+{\alpha }_{i4}\right){\mathbf{c}}_1^w\\ & ={\alpha }_{i2}\left({\mathbf{c}}_2^w-{\mathbf{c}}_1^w\right)+{\alpha }_{i3}\left({\mathbf{c}}_3^w-{\mathbf{c}}_1^w\right)+{\alpha }_{i4}\left({\mathbf{c}}_4^w-{\mathbf{c}}_1^w\right)\\ & =\left[\begin{array}{lll}{\mathbf{c}}_2^w-{\mathbf{c}}_1^w& {\mathbf{c}}_3^w-{\mathbf{c}}_1^w& {\mathbf{c}}_4^w-{\mathbf{c}}_1^w\end{array}\right]\left[\begin{array}{l}{\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]\end{array}\text{\hspace{1em}(06)}$$那么,世界坐标系下控制点的系数就可以这样计算得到：$$\begin{array}{c}\left[\begin{array}{l}{\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]={\left[\begin{array}{ccc}{\mathbf{c}}_2^w-{\mathbf{c}}_1^w& {\mathbf{c}}_3^w-{\mathbf{c}}_1^w& {\mathbf{c}}_4^w-{\mathbf{c}}_1^w\end{array}\right]}^{-1}\left({\mathbf{p}}_i^w-{\mathbf{c}}_1^w\right)\\ {\alpha }_{i1}=1-{\alpha }_{i2}-{\alpha }_{i3}-{\alpha }_{i4}\end{array}\text{\hspace{1em}(07)}$$以上是世界坐标系下的推导，那么，在相机坐标系下，${\alpha }_{ij}$ 满足如下的对应关系：
$${\mathbf{p}}_i^c=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c,\sum _{j=1}^4{\alpha }_{ij}=1\text{\hspace{1em}(08)}$$前面已经提到世界坐标系满足(04)式的关系，那么相同的 ${\alpha }_{ij}$ 使得相机坐标系满足(08)式，这里我们给出一个 $结论$：同一个3D点在世界坐标系下对应控制点的系数 ${\alpha }_{ij}$ 和其在相机坐标系下对应控制点的系数相同。也就是说，我们可以预先在世界坐标系下求取控制点系数 ${\alpha }_{ij}$，然后将其作为已知量拿到相机坐标系下使用。下面是该结论的推导过程(${\mathbf{T}}_{cw}表示为需要求解的相机位姿$)：
$$\begin{array}{rl}\left[\begin{array}{c}{\mathbf{p}}_i^c\\ 1\end{array}\right]& ={\mathbf{T}}_{cw}\left[\begin{array}{c}{\mathbf{p}}_i^w\\ 1\end{array}\right]\\ & ={\mathbf{T}}_{cw}\left[\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^w\\ \\ {\sum }_{j=1}^4{\alpha }_{ij}\end{array}\right]\\ & =\sum _{j=1}^4{\alpha }_{ij}{\mathbf{T}}_{cw}\left[\begin{array}{c}{\mathbf{c}}_j^w\\ 1\end{array}\right]\\ & =\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{\mathbf{c}}_j^c\\ 1\end{array}\right]\end{array}\text{\hspace{1em}(09)}$$所以结论(08)成立。但是这里需要注意，以上推导建立在 ${\sum }_{j=1}^4{\alpha }_{ij}=1$ 这个重要的约束条件上，如果没有这个约束，那么上述结论不成立。

到目前为止，我们已经根据世界坐标系下3D点 ${\mathbf{P}}_i^w$ 求出了世界坐标系下的4个控制点 ${\mathbf{c}}_j^w,j=1,\cdots ,4$, 以及每个3D点对应的控制点系数 ${\alpha }_{ij}$, 前面证明过: 同一个3D点在世界坐标系下对应控制点的系数 ${\alpha }_{ij}$ 和其在相机坐标系下对应控制点的系数相同。所以如果我们能把4个控制点在相机坐标系下的坐标 ${\mathbf{c}}_j^c,j=1,\cdots ,4$ 求解出来，就可以得到世界坐标系下3D点在相机坐标系下的坐标 ${\mathbf{P}}_j^c$。这样就可以根据ICP求解位姿了。

 

六、透视投影关系构建约束

记 $w_i$为投影尺度系数，$\mathbf{K}$相机内参矩阵，$\mathbf{u}$为相机坐标系下3D参考点 对应的2D投影坐标，根据相机投影原理可得：$$w_i\left[\begin{array}{c}{\mathbf{u}}_i\\ \\ 1\end{array}\right]=\mathbf{K}{\mathbf{p}}_i^c=\mathbf{K}\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_j^c\text{\hspace{1em}(10)}$$记控制点 $c_j^c$ 坐标为 $[x_j^c,y_j^c,z_j^c{]}^T$, $f_u,f_v$是焦距，$u_c,v_c$是主点坐标，上式可以化为：
$$w_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_u& 0& u_c\\ 0& f_v& v_c\\ 0& 0& 1\end{array}\right]\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]\text{\hspace{1em}(11)}$$根据最后一行可以推出$$w_i=\sum _{j=1}^4({\alpha }_{ij}{z}_j^c),i=1,\cdots ,n\text{\hspace{1em}(12)}$$消去最后一行，我们把上面矩阵展开写成等式右边为0的表达式，所以实际上每个点对可以得到2个方程:$$\begin{array}{l}{\sum }_{j=1}^4({\alpha }_{ij}{f}_u{x}_j^c+{\alpha }_{ij}\left(u_c-u_i\right)z_j^c)=0\\ \\ {\sum }_{j=1}^4({\alpha }_{ij}{f}_v{y}_j^c+{\alpha }_{ij}\left(v_c-v_i\right)z_j^c)=0\end{array}\text{\hspace{1em}(12)}$$这里的待求的未知数是12个相机坐标系下控制点坐标$\left\{\left(x_j^c,y_j^c,z_j^c\right)\right\},j=1,\cdots ,4$，我们把 个匹配点对全部展开，再写成矩阵的形式：$$\left[\begin{array}{ccccccc}{\alpha }_{11}{f}_u& 0& {\alpha }_{11}\left(u_c-u_1\right)& \cdots & {\alpha }_{14}{f}_u& 0& {\alpha }_{14}\left(u_c-u_1\right)\\ 0& {\alpha }_{11}{f}_v& {\alpha }_{11}\left(v_c-v_1\right)& \cdots & 0& {\alpha }_{14}{f}_v& {\alpha }_{14}\left(v_c-v_1\right)\\ ⋮& & & & & & \\ {\alpha }_{i1}{f}_u& 0& {\alpha }_{i1}\left(u_c-u_i\right)& \cdots & {\alpha }_{i4}{f}_u& 0& {\alpha }_{i4}\left(u_c-u_i\right)\\ 0& {\alpha }_{i1}{f}_v& {\alpha }_{i1}\left(v_c-v_i\right)& \cdots & 0& {\alpha }_{i4}{f}_v& {\alpha }_{i4}\left(v_c-v_i\right)\\ ⋮& & & & & & \\ {\alpha }_{n1}{f}_u& 0& {\alpha }_{n1}\left(u_c-u_n\right)& \cdots & {\alpha }_{n4}{f}_u& 0& {\alpha }_{n4}\left(u_c-u_n\right)\\ 0& {\alpha }_{n1}{f}_v& {\alpha }_{n1}\left(v_c-v_n\right)& \cdots & 0& {\alpha }_{n4}{f}_v& {\alpha }_{n4}\left(v_c-v_n\right)\end{array}\right]\left[\begin{array}{l}{x}_1^c\\ y_1^c\\ z_1^c\\ x_2^c\\ y_2^c\\ z_2^c\\ x_3^c\\ y_3^c\\ z_3^c\\ x_4^c\\ y_4^c\\ z_4^c\end{array}\right]=0\text{\hspace{1em}(13)}$$其中，$i=1,\cdots ,n$ 表示点对的数目，记左边第一个矩阵为 $\mathbf{M}$，它的大小为 2n x12，第二个矩阵x是带求未知量组成的矩阵，大小为 12 x 1，则上式可以写成：
$$\mathbf{Mx}=0\text{\hspace{1em}(14)}$$

七、结语

通过前面的推导，最终获得矩阵方程 $\mathbf{Mx}=0$，其中 $\mathbf{x}$ 就是由 $c_j^c=\left\{\left(x_j^c,y_j^c,z_j^c\right)\right\},j=1,\cdots ,4$ 组合而成的列向量，也就是说其为我们需要求解的世界坐标系下3D点在相机坐标系下的坐标。