EPnP算法学习随笔

这几天刚好在看epnp算法，写篇随笔简要回顾一下。
PnP问题是个很经典的问题，就是根据图像上的关键点来估计相机的位姿，这里主要研究的是单目摄像机，但是值的注意的是单目的精度一般都比较差，因为单目摄像机是无法获取深度信息的，比如两只眼睛的话我们往往能轻松的估计距离但是遮住一只眼睛的话我们的空间感往往会差很多，PnP问题也是同理，主要思想就是通过已知的参照点去求出变换的过程，所以PnP问题一定是需要预先准备一组已知的参考点的。
这里主要学习的EPnP算法，首先我们需要一个能将二维坐标和三维坐标对应起来的关系式，这样方便我们能宏观的看待问题：
\(s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = P \cdot \begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix}\\\)
这里\(X_i,Y_i,Z_i\)是世界坐标系下的坐标，P是相机投影矩阵具体如下：
\(P=K\begin{bmatrix} R \ t \end{bmatrix}\)
这里K是相机内参，R是旋转矩阵，t是平移向量，\(s_i\)是和相机深度有关的因子，难以直接求出，但是也可以直接代入6~8个点直接解得，那就是dlt算法了但是这样精度很差且抗噪声性能不够优秀，因此就有了今天的EPnP算法。
EPnP算法很灵活的应用了数学中向量分解的思想，把空间中的每一点都用四个虚拟控制点来表示，即

\[\ \mathbf{p}_i^w = \sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^w \tag{1} \]

\[\sum_{j=1}^4 \alpha_{ij} = 1 \tag{2}\]

这列\(c_j^w\)是世界坐标系下的控制点坐标\(p_i^w\)是世界坐标系下其余点的坐标（以下所有上标w是世界坐标系，上标c是相机坐标系），接下来我们要对之前三维坐标转化成二维坐标的操作进行拆分，先将控制点的坐标转化到三相机坐标系，在从相机坐标系转化到像素坐标系，但是这里就有一个疑惑了，如果转化到相机坐标系那么权重\(\alpha_{ij}\)是否会发生变化呢，因为如果变化了，那么用相机坐标系下的控制点去表示相机坐标系下的其他点权重就需要重新再求一遍，那么就会变得很麻烦了，幸运的是这个问题的答案是不会，具体的证明如下：

\[\mathbf{p}_i^c = \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} \mathbf{p}_i^w \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} \sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^w \\ 1 \end{bmatrix} \ \]

\[= \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} \sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^w \\ \sum_{j=1}^4 \alpha_{ij} \end{bmatrix} = \sum_{j=1}^4 \alpha_{ij} \begin{bmatrix} \mathbf{R} & \mathbf{t} \end{bmatrix} \begin{bmatrix} \mathbf{c}_j^w \\ 1 \end{bmatrix} = \sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^c \tag{3} \]

这里其实也很好理解因为世界坐标系转化到相机坐标系是个线性变换，只涉及了旋转和平移没有涉及拉伸等变换，也没有涉及维度的变化，因此权重当然也就不变了。
好了有了以上的基础，那么我们就开始进入正题，首先是控制点的选取，其实控制点只要保证可逆就行了，其实就是不共面，但是论文给了一种方法，就是把质心当做第一个点，其余点在以质心为中心在特征向量方向上伸展：
先求出质心

\[\mathbf{c}_1^w = \frac{1}{n} \sum_{i=1}^n \mathbf{p}_i^w \tag{4} \]

把世界点去重心化构造矩阵A

\[\mathbf{A} = \begin{bmatrix} (\mathbf{p}_1^w)^T - (\mathbf{c}_1^w)^T \\ \cdots \\ (\mathbf{p}_n^w)^T - (\mathbf{c}_1^w)^T \end{bmatrix} \tag{5} \]

对\(\mathbf{A}^T \mathbf{A}\)进行特征值分解

\[\begin{align*} \mathbf{c}_2^w &= \mathbf{c}_1^w + \sqrt{\lambda_1} \mathbf{v}_1 \\ \mathbf{c}_3^w &= \mathbf{c}_1^w + \sqrt{\lambda_2} \mathbf{v}_2 \\ \mathbf{c}_4^w &= \mathbf{c}_1^w + \sqrt{\lambda_3} \mathbf{v}_3 \end{align*} \tag{6} \]

\(\lambda_1\),\(\lambda_2\),\(\lambda_1\)是特征值，\(v_1,v_2,v_3\)是特征向量，是不是看起来很简单？但是接下来的步骤开始麻烦了，需要求的是旋转矩阵和平移向量这两个未知，那要怎么把世界坐标转化成相机坐标呢？别急我们不是有相机吗，相机可以知道控制点的像素坐标，因此只要能从控制点的像素坐标求出控制点的相机坐标，那么就可以结合已知的控制点世界坐标直接求出旋转矩阵和平移向量了，那就可以得到相机的位姿了，到这里EPnP算法的主要思想已经全部出来了：

\[w_i \begin{bmatrix} \mathbf{u}_i \\ 1 \end{bmatrix} = \mathbf{K} \mathbf{p}_i^c = \mathbf{K} \sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^c \tag{7} \]

\[w_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \sum_{j=1}^4 \alpha_{ij} \begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix} \tag{8} \]

这里\(\mathbf{p}_i^c\)是世界坐标系下的控制点，\(\mathbf{u}_i\)是像素坐标（这里i表示的是第几个控制点，j表示的是每个控制点第几个权重），\(c_x\)，\(c_y\)是主点，\(f_x\)，\(f_y\)是等效焦距对这个式子展开并且整理就可以得到一个方程如下：

\[\begin{cases} \sum_{j=1}^4 \left( \alpha_{ij} f_x x_j^c + \alpha_{ij} (c_x - u_i) z_j^c \right) = 0 \\ \sum_{j=1}^4 \left( \alpha_{ij} f_y y_j^c + \alpha_{ij} (c_y - v_i) z_j^c \right) = 0 \end{cases} \tag{9} \]

将这里面的变量写成如下的形式

\[\begin{bmatrix} 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{bmatrix}\tag{10} \]

那么方程就可以写成\(Mx=0\)的形式，
这个方程的解可以写成：

\[\mathbf{x} = \sum_{k=1}^N \beta_k \mathbf{v}_k \tag{11} \]

\(\mathbf{v}_k\)是\({M^TM}\)矩阵分解后的特征向量，但是这个解有什么实际的物理含义呢，
实际上写成如下形式更直观：

\[\begin{bmatrix} 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{bmatrix} = \beta_1 \begin{bmatrix} v_x^1 \\ v_y^1 \\ v_z^1 \\ v_x^2 \\ v_y^2 \\ v_z^2 \\ v_x^3 \\ v_y^3 \\ v_z^3 \\ v_x^4 \\ v_y^4 \\ v_z^4 \end{bmatrix}_1 + \beta_2 \begin{bmatrix} v_x^1 \\ v_y^1 \\ v_z^1 \\ v_x^2 \\ v_y^2 \\ v_z^2 \\ v_x^3 \\ v_y^3 \\ v_z^3 \\ v_x^4 \\ v_y^4 \\ v_z^4 \end{bmatrix}_2 + \cdots + \beta_N \begin{bmatrix} v_x^1 \\ v_y^1 \\ v_z^1 \\ v_x^2 \\ v_y^2 \\ v_z^2 \\ v_x^3 \\ v_y^3 \\ v_z^3 \\ v_x^4 \\ v_y^4 \\ v_z^4 \end{bmatrix}_N\tag{12}\ \]

特征向量以三行为界划分为四组，每组分别对应一个控制点的三个维度的坐标，为什么这样呢，不清楚的话回头看看（10）你就会明白了，现在只需要求出\(\beta_k\)就大工告成了，麻烦也就此产生了，\(\beta_k\)要怎么去呢？其实我们还漏掉了一个约束条件，还记的我们前面说的吗？世界坐标系变换到相机坐标系是一个线性变换，不涉及拉伸等操作的，那么控制点直接只是相对位置发生了变化，点与点之间的距离应该是保持不变的，由于我们总共有四个控制点两两组对我们就又有了6个约束条件，如下：

\[\|\mathbf{c}_i^c - \mathbf{c}_j^c\|^2 = \|\mathbf{c}_i^w - \mathbf{c}_j^w\|^2 \tag{13} \]

\[\left\|\sum_{k=1}^N \beta_k \mathbf{v}_k^{[i]} - \sum_{k=1}^N \beta_k \mathbf{v}_k^{[j]}\right\|^2 = \|\mathbf{c}_i^w - \mathbf{c}_j^w\|^2 \tag{14} \]

但是还有一个问题，特征向量一共多少个，即\(N\)为多少，一般建议考虑\(N=1,2,3,4\)四种情况，实际使用时可以四种都考虑，然后根据反投影变换求精度最优的那个，这四种情况我只介绍相对较难得\(N=4\)的情况，其他的可以参考这篇文章：https://zhuanlan.zhihu.com/p/361791835，这篇写的非常的详细还有代码资源就是公式太多看的有点烦就是。
首先对（13）、（14）式进行展开：

\[\|\beta_1 \mathbf{S}_1 + \beta_2 \mathbf{S}_2 + \beta_3 \mathbf{S}_3 + \beta_4 \mathbf{S}_4\|^2 = c \tag{15} \]

\[\begin{aligned} & \beta_1^2 \mathbf{S}_1^T \mathbf{S}_1 + 2\beta_1\beta_2 \mathbf{S}_1^T \mathbf{S}_2 + 2\beta_1\beta_3 \mathbf{S}_1^T \mathbf{S}_3 + 2\beta_1\beta_4 \mathbf{S}_1^T \mathbf{S}_4 + \beta_2^2 \mathbf{S}_2^T \mathbf{S}_2 + \\ &2\beta_2\beta_3 \mathbf{S}_2^T \mathbf{S}_3 + 2\beta_2\beta_4 \mathbf{S}_2^T \mathbf{S}_4 \ + \beta_3^2 \mathbf{S}_3^T \mathbf{S}_3 + 2\beta_3\beta_4 \mathbf{S}_3^T \mathbf{S}_4 + \beta_4^2 \mathbf{S}_4^T \mathbf{S}_4 = c \end{aligned} \tag{16} \]

\[[ \begin{array}{cccccccc} \mathbf{S}_1^T\mathbf{S}_1 & 2\mathbf{S}_1^T\mathbf{S}_2 & 2\mathbf{S}_1^T\mathbf{S}_3 & 2\mathbf{S}_1^T\mathbf{S}_4 & \mathbf{S}_2^T\mathbf{S}_2 & 2\mathbf{S}_2^T\mathbf{S}_3 & 2\mathbf{S}_2^T\mathbf{S}_4 & \mathbf{S}_3^T\mathbf{S}_3 & 2\mathbf{S}_3^T\mathbf{S}_4 & \mathbf{S}_4^T\mathbf{S}_4 \end{array} ]\begin{bmatrix} \beta_{11} \\ \beta_{12} \\ \beta_{13} \\ \beta_{14} \\ \beta_{22} \\ \beta_{23} \\ \beta_{24} \\ \beta_{33} \\ \beta_{34} \\ \beta_{44} \end{bmatrix} = c \tag{17} \]

整理成：

\[\mathbf{L}\beta = \rho \tag{18} \]

这里，\(\mathbf{S}_1,\mathbf{S}_2,\mathbf{S}_3,\mathbf{S}_4\)是\((\mathbf{v}_1^{[i]} - \mathbf{v}_1^{[j]}),(\mathbf{v}_2^{[i]} - \mathbf{v}_2^{[j]}),(\mathbf{v}_3^{[i]} - \mathbf{v}_3^{[j]}),(\mathbf{v}_4^{[i]} - \mathbf{v}_4^{[j]})\)，你可以看到这是6个方程但是有四个未知数，然后接下来的解法就比较魔幻了，有与和\(\beta\)有关的未知数是相关的因此只要求出四个就可以得出其他，一般先用

\[\left[ \begin{array}{cccc} \mathbf{S}_1^T\mathbf{S}_1 & 2\mathbf{S}_1^T\mathbf{S}_2 & 2\mathbf{S}_1^T\mathbf{S}_3 & 2\mathbf{S}_1^T\mathbf{S}_4 \end{array} \right] \begin{bmatrix} \beta_{11} \\ \beta_{12} \\ \beta_{13} \\ \beta_{14} \end{bmatrix} = c \tag{19} \]

这四个式子先求个大概得粗略值，然后使用高斯牛顿法或者其他方法近似，最后求得结果，相机坐标系下的控制点坐标知道后那么就临近尾声了，接下来就是个ICP问题：
先求出质心：

\[\mathbf{p}_c^c = \frac{1}{n} \sum_{i=1}^n \mathbf{p}_i^c \tag{20} \]

\[\mathbf{p}_c^w = \frac{1}{n} \sum_{i=1}^n \mathbf{p}_i^w \tag{21} \]

然后对点云去质心：

\[\bar{\mathbf{P}}^c = \begin{bmatrix} (\mathbf{p}_1^c)^T - (\mathbf{p}_c^c)^T \\ \vdots \\ (\mathbf{p}_n^c)^T - (\mathbf{p}_c^c)^T \end{bmatrix} \tag{22} \]

\[\bar{\mathbf{P}}^w = \begin{bmatrix} (\mathbf{p}_1^w)^T - (\mathbf{p}_c^w)^T \\ \vdots \\ (\mathbf{p}_n^w)^T - (\mathbf{p}_c^w)^T \end{bmatrix} \tag{23} \]

求出旋转矩阵：

\[\mathbf{W} = (\bar{\mathbf{P}}^c)^T \bar{\mathbf{P}}^w \tag{24} \]

\[[\mathbf{U}\mathbf{\Sigma}\mathbf{V}] = SVD\left( \mathbf{W} \right) \tag{25} \]

\[\mathbf{R} = \mathbf{U}\mathbf{V}^T \tag{26} \]

用质心差求出平移向量：

\[\mathbf{t} = \mathbf{p}_c^c - \mathbf{R}\mathbf{p}_c^w \tag{27} \]

于是就OK啦