求解 PnP 问题

在上一节中，我们提到 ORB-SLAM2 可以根据匹配的3D地图点和当前帧特征点的2D像素坐标估计相机位姿。这种根据3D-2D映射关系求解相机位姿的问题，就是所谓的 PnP 问题(Perspective-n-Point Problem)。在机器视觉领域中，这是一个非常常见的问题， OpenCV 库还专门提供了函数 solvePnP进行求解。 PnP 问题有很多种解法，ORB-SLAM2 采用了一种称为 EPnP 的算法，求解效率还不错。通常，EPnP 的解都会拿来作为高斯牛顿之类迭代优化算法的初值，经过迭代之后得到一个更优的解。

本文中，我们先形式化的描述 PnP 问题，并以最简单的 DLT(直接线性变换, Direct Linear Transformation)的思想尝试求解它。然后查看 ORB-SLAM2 中的 PnPSolver 的求解总体过程，再结合论文和代码，分析 EPnP 的原理和求解过程。

1. PnP 问题

如右侧从OpenCV的文档中找到的图所示， PnP 问题(Perspective-n-Point Problem)是，已知相机内参矩阵$K$和 $n$ 个 3D 空间点$\{ \boldsymbol{c_1, c_2, \cdots, c_n} \}$及其到图像上 2D 的投影点$\{ \boldsymbol{\mu_1, \mu_2, \cdots, \mu_n} \}$，求解相机的位置和姿态。记第 $i$ 个 3D 空间点的齐次坐标为 $\boldsymbol{c_i} = \begin{bmatrix} x_i & y_i & z_i & 1\end{bmatrix}^T$，其在图像上投影的 2D 像素坐标为 $\boldsymbol{\mu_i} = \begin{bmatrix} u_i & v_i & 1 \end{bmatrix}^T$。其投影过程，可以分解为两步：

根据相机的位姿，将空间点从世界坐标系下变换到相机坐标系下。
将相机坐标系下的点，根据相机内参矩阵，投影到图像上。

其整个过程相当于连续乘了两个矩阵：

$$ s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \boldsymbol{K} \left[ \boldsymbol{R} \big | \boldsymbol{t} \right] \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} t_1 & t_2 & t_3 & t_4 \\ t_5 & t_6 & t_7 & t_8 \\ t_9 & t_{10} & t_{11} & t_{12} \end{bmatrix} \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} $$

其中，$s$是一个尺度系数，在计算时通常通过叉乘或者归一化将之消除掉。$\boldsymbol{K, R, t}$分别是相机的内参矩阵、姿态矩阵和位置向量。参照单应矩阵和基础矩阵的求解过程，我们用矩阵$\boldsymbol{A} = \boldsymbol{K} \left[ \boldsymbol{R} \big | \boldsymbol{t} \right]$将上式改写为:

$$ s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} a_1 & a_2 & a_3 & a_4 \\ a_5 & a_6 & a_7 & a_8 \\ a_9 & a_{10} & a_{11} & a_{12} \end{bmatrix}}_{\boldsymbol{A}} \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} \Rightarrow \begin{cases} u_i = \frac{ a_1 x_i + a_2 y_i + a_3 z_i + a_4 }{ a_9 x_i + a_{10} y_i + a_{11}z_i + a_{12} } \\ v_i = \frac{ a_5 x_i + a_6 y_i + a_7 z_i + a_8 }{ a_9 x_i + a_{10} y_i + a_{11}z_i + a_{12} } \end{cases} \Rightarrow \begin{bmatrix} x_i & y_i & z_i & 1 & 0 & 0 & 0 & 0 & -x_i & -y_i & -z_i & -1 \\ 0 & 0 & 0 & 0 & x_i & y_i & z_i & 1 & -x_i & -y_i & -z_i & -1 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_{11} \\ a_{12} \end{bmatrix} = \boldsymbol{0} $$

如此，对于 $n$ 个匹配点对，就可以得到下面形式的线性方程组。SVD分解，解零空间，就可以解得矩阵$\boldsymbol{A}$。最少6个匹配点对，就可以完成求解。这就是一个DLT(Direct Linear Transformation)的方法。

$$ \begin{bmatrix} x_0 & y_0 & z_0 & 1 & 0 & 0 & 0 & 0 & -x_0 & -y_0 & -z_0 & -1 \\ 0 & 0 & 0 & 0 & x_0 & y_0 & z_0 & 1 & -x_0 & -y_0 & -z_0 & -1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_{n-1} & y_{n-1} & z_{n-1} & 1 & 0 & 0 & 0 & 0 & -x_{n-1} & -y_{n-1} & -z_{n-1} & -1 \\ 0 & 0 & 0 & 0 & x_{n-1} & y_{n-1} & z_{n-1} & 1 & -x_{n-1} & -y_{n-1} & -z_{n-1} & -1 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_{11} \\ a_{12} \end{bmatrix} = \boldsymbol{0} $$

当然上述DLT算法解得的是矩阵$\boldsymbol{A}$，它包含了相机内参$\boldsymbol{K}$、姿态矩阵$\boldsymbol{R}$和平移向量$\boldsymbol{t}$。进一步的，通过QR分解，可以从矩阵$\boldsymbol{A}$中把这三个都给分解出来。看起来这一过程还附带算出了相机的内参，这也正是相机的内参标定的求解过程。 DLT算法简单直接，但是它忽略了太多的约束，所以结果一般都不会很好。后来人们还研究出了很多求解 PnP 问题的算法，有只需要3个点就可以求解的P3P算法。 ORB-SLAM2 用的就是EPnP算法，效率高而且稳定，据说其算法复杂度是 $O(n)$ 的。下面我们先来看一下 ORB-SLAM2 中 PnPSolver 的总体过程。

2. 解析 PnPSolver

2.1 PnPSolver 构造函数

我们先简单看一下 PnPSolver 的构造函数，如下面的代码片段所示，它有两个输入参数 F 是将要求解位姿的当前帧，vpMapPointMatches 中则记录了与 F 匹配的地图点，既世界坐标系下的 3D 点坐标。在构造函数的成员变量初始化列表中，有4个指针变量 pws, us, alphas, pcs 分别表示匹配点的3D世界坐标、2D像素坐标、各点对应的4个控制点加权系数、相机坐标系下3D坐标。

        PnPsolver::PnPsolver(const Frame & F, const vector<MapPoint*> &vpMapPointMatches)
            : pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0),
              mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0)

        {
            mvpMapPointMatches = vpMapPointMatches;
            mvP2D.reserve(F.mvpMapPoints.size());
            mvSigma2.reserve(F.mvpMapPoints.size());
            mvP3Dw.reserve(F.mvpMapPoints.size());
            mvKeyPointIndices.reserve(F.mvpMapPoints.size());
            mvAllIndices.reserve(F.mvpMapPoints.size());
            int idx=0;

在构造函数一开始，先给一些容器申请了内存。mvpMapPointMatches 拷贝了一份输入的地图点匹配信息。mvP2D 记录特征点2D坐标的容器， mvSigma2 记录特征点的图像金字塔系数，mvP3Dw 记录地图点在世界坐标系下的坐标，mvKeyPointIndices 记录特征点在 mvpMapPointMatches 的索引， mvAllIndices 对所有有效的匹配点建立索引，用于 RANSAC 策略。

然后在下面的代码片段中遍历帧 F 中的所有特征点，刨除那些没有地图点与之匹配的，将剩下的点坐标信息和索引记录到刚刚分配的那些容器中。

            for (size_t i=0, iend=vpMapPointMatches.size(); i < iend; i++) {
                MapPoint* pMP = vpMapPointMatches[i];
                if (pMP && !pMP->isBad()) {

展开记录3D-2D点坐标和索引 >>>

                    const cv::KeyPoint &kp = F.mvKeysUn[i];
                    mvP2D.push_back(kp.pt);
                    mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);

                    cv::Mat Pos = pMP->GetWorldPos();
                    mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2)));

                    mvKeyPointIndices.push_back(i);
                    mvAllIndices.push_back(idx);               
                    idx++;

            }   }
            fu = F.fx; fv = F.fy; uc = F.cx; vc = F.cy;    
            SetRansacParameters();
        }

最后用成员变量 fu, fv 记录下相机的焦距，uc, vc 记录相机的光心坐标。然后调用成员函数 SetRansacParameters 设置RANSAC随机策略。我们已经在地图初始化总体过程中接触过 RANSAC 算法了，其精髓在于，以最少的样本求解问题，多次随机采样，选取最优的那组解。函数 SetRansacParameters 本来有6个参数需要输入，只是因为都有默认值，所以这里都省略了。这 6 个参数分别是，用于理论计算RANSAC迭代次数的概率参数、最小的内点数量、最大迭代次数、求解问题的最小样本数量、内点比例、用于判定内点的距离阈值。该函数的具体内容这里就不再详细分析了。

2.2 总体迭代过程 iterate

成功构建了 PnPSolver 之后，我们就可以通过接口函数 iterate 来迭代 EPnP 算法求解相机的位姿。下面是该函数的代码片段，它输入了四个参数，最后将相机的位姿估计以 cv::Mat 的形式输出。其中 nIterations 表示迭代的次数；bNoMore 是一个输出的参数，用于说明是否达到了最大的迭代次数；容器 vbInliers 对应着当前帧的各个特征点，标识着结束迭代之后，它们是否判定为内点，nInliers 则记录了最后的内点数量。函数的一开始先对这些输出参数赋予了初值。

        cv::Mat PnPsolver::iterate(int nIterations, bool & bNoMore, vector<bool> & vbInliers, int & nInliers) {
            bNoMore = false; vbInliers.clear(); nInliers = 0;
            set_maximum_number_of_correspondences(mRansacMinSet);

上面代码片段中的第3行，调用了一个函数用于根据 RANSAC 的最小样本数量申请内存。其输入参数 mRansacMinSet 在函数 SetRansacParameters 中赋值为 4，这也是 EPnP 算法能够求解的最少点数。在该函数，还会给成员指针变量 pws, us, alphas, pcs 根据最小样本数量通过 new 申请内存。

            if (N < mRansacMinInliers) { bNoMore = true; return cv::Mat(); }  
            vector<size_t> vAvailableIndices;
            int nCurrentIterations = 0;

在左侧的代码片段中，检查匹配点对数量 N，如果太少就放弃求解。成员变量 N 在函数 SetRansacParameters 中通过语句N = mvP2D.size();赋予初值。

接着定义一个临时的容器 vAvailableIndices 用于拷贝成员容器 mvAllIndices，构建 RANSAC 策略随机抽取特征点的样本集合。变量 nCurrentIterations 是当前迭代次数的计数器。在下面的代码片段中，开始进行迭代。迭代有两个终止条件，其一，总的迭代计数 mnIterations 应当小于在函数 SetRansacParameters 中指定的最大迭代次数；其二，当前迭代计数 nCurrentIterations 应当小于调用 iterate 时的输入参数 nIterations。

            while (mnIterations < mRansacMaxIts || nCurrentIterations < nIterations) {
                nCurrentIterations++; mnIterations++; reset_correspondences();

在迭代一开始，先对 nCurrentIterations 和 mnIterations 两个变量计数，再调用函数 reset_correspondences 清空匹配点计数。其实这个函数里面只有一句话number_of_correspondences = 0;。然后在下面的 for 循环中从所有的匹配点对，不放回的进行 mRansacMinSet 次采样(4 次)。在采样之前先将 mvAllIndices 拷贝一份到 vAvailableIndices 中，采样时生成一个 [0, vAvailableIndices.size()-1] 的随机数，同时通过函数 add_correspondence 记录下匹配点对的 3D 世界坐标和 2D 像素坐标，保存在指针变量 pws 和 us 所指的内存中。 14, 15 行负责将已经被选中的样本从集合中移除。

                vAvailableIndices = mvAllIndices;
                for(short i = 0; i < mRansacMinSet; ++i) {
                    int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
                    int idx = vAvailableIndices[randi];

                    add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);

                    vAvailableIndices[randi] = vAvailableIndices.back();
                    vAvailableIndices.pop_back();
                }

                compute_pose(mRi, mti);    
                CheckInliers();

接着先后调用 EPnP 原生的函数 compute_pose 估计相机位姿，再通过函数 CheckInliers 完成内点的筛选工作。函数 compute_pose 完成了 EPnP 的估计工作，我们将在下一小节中分析。 CheckInliers 是在根据刚才估计的相机位姿信息，遍历所有的特征点，计算重投影误差，剔除掉那些误差较大的匹配点对。筛选之后的匹配信息直接更新到成员变量 mvbInliersi 中了。同时会用成员变量 mnInliersi 统计内点数量。

当内点的数量超过了在函数 SetRansacParameters 中指定的最少内点数量 mRansacMinInliers 时，ORB-SLAM2 就会进一步的优化位姿。首先，判定当前的结果是否是历次迭代过程中内点数量最多的那个，若是将之记录下来，用 mBestTcw 保存当前的位姿估计 mRi 和 mti。然后在函数 Refine 中用所有的内点再调用一次 compute_pose 和 CheckInliers，进一步优化位姿并筛选内点。如果内点数量仍然足够多，就会返回 true，此时认为找到了一个还算可以的位姿直接在第41行中返回了。否则 Refine 将返回 false，此时需要继续迭代。

                if (mnInliersi >= mRansacMinInliers) {
                    if (mnInliersi > mnBestInliers) {

展开记录下最优的一次估计 >>>

                        mvbBestInliers = mvbInliersi;
                        mnBestInliers = mnInliersi;

                        cv::Mat Rcw(3,3,CV_64F,mRi);
                        cv::Mat tcw(3,1,CV_64F,mti);
                        Rcw.convertTo(Rcw,CV_32F);
                        tcw.convertTo(tcw,CV_32F);
                        mBestTcw = cv::Mat::eye(4,4,CV_32F);
                        Rcw.copyTo(mBestTcw.rowRange(0,3).colRange(0,3));
                        tcw.copyTo(mBestTcw.rowRange(0,3).col(3));

                    }
                    if (Refine()) {

展开 Refine后处理 >>>

                        nInliers = mnRefinedInliers;
                        vbInliers = vector<bool>(mvpMapPointMatches.size(), false);
                        for (int i = 0; i < N; i++) {
                            if (mvbRefinedInliers[i])
                                vbInliers[mvKeyPointIndices[i]] = true;
                        }

                        return mRefinedTcw.clone();
                    }
                }

函数 compute_pose 及其调用的子函数是 EPnP 算法的核心。下面我们结合论文中关于算法原理的介绍，详细分析函数 compute_pose 的实现过程。

3. EPnP 算法

根据论文中的表述，EPnP 算法的时间复杂度是 $O(n)$ 的，相比于其它$O(n^5), O(n^8)$的算法而言，它要高效很多了。其核心思想体现在三个方面：

将世界坐标系下的 $n$ 个 3D 点，用 4 个虚拟的控制点通过加权和的形式表达。
这种虚拟控制点的加权和关系，经过相机位姿 $\left[ \boldsymbol{R} \big | \boldsymbol{t} \right]$ 变换到相机坐标系下仍然成立，既对应点的加权系数不变。
根据2D像素坐标估计出 4 个虚拟的控制点在相机坐标系的坐标，可以将 3D-2D 的映射问题转换为 3D-3D 点映射问题。

总体上，EPnP 算法可以分为四步：1. 计算世界坐标系下的虚拟控制点, 2. 计算表达各个3D点的控制点权重，3. 求解相机坐标系下的控制点坐标, 4. ICP 求解相机位姿。下面是其具体实现的函数 compute_pose 的代码片段。它有两个参数 R, t 分别输出相机的旋转矩阵和平移向量。用于求解相机位姿的匹配点信息已经保存在成员变量 pws 和 us 中。

        double PnPsolver::compute_pose(double R[3][3], double t[3]) {  
            choose_control_points();
            compute_barycentric_coordinates();

该函数一开始，先调用函数 choose_control_points 计算了世界坐标系下的四个虚拟控制点。理论上控制点的选取可以是任意的，但论文中说，以3D点的质心以及PCA分解后的三个主轴上选取的三个点，所构成的控制点有助于提高算法的稳定性。

In theory the control points can be chosen arbitrarily. However, in practice, we have found that the stability of our method is increased by taking the centroid of the reference points as one, and to select the rest in such a way that they form a basis aligned with the principal directions of the data.

我们把计算得到的 4 个控制点的世界坐标记为 $\boldsymbol{c_j^w}, j = 1, \cdots, 4$，那么世界坐标系下的第 $i$ 个 3D 点坐标都可以写成这四个控制点的加权和形式。

$$ \boldsymbol{p_i^w} = \sum_{j=1}^4 \alpha_{ij} \boldsymbol{c_j^w}, \text{with} \sum_{j=1}^4 \alpha_{ij} = 1 $$

其中，$\alpha_{ij}$被称为homogeneous barycentric coordinate。我们可以将上式写成齐次矩阵的形式，求解得到 $\alpha_{ij}$。 $$ \begin{bmatrix} \boldsymbol{p_i^w} \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} \boldsymbol{c_1^w} & \boldsymbol{c_2^w} & \boldsymbol{c_3^w} & \boldsymbol{c_4^w} \\ 1 & 1 & 1 & 1 \end{bmatrix}}_{C} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} \Rightarrow \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} = C^{-1} \begin{bmatrix} \boldsymbol{p_i^w} \\ 1 \end{bmatrix} $$ 它们都在上面的代码片段中，通过函数 compute_barycentric_coordinates 计算得到，结果将被保存在成员变量 a 中。该函数并没有直接构建矩阵$C$然后对其求逆，而是将 3D 坐标和四个控制点都减去质心，再进行求解的。其实现细节这里就不再展开了。

世界坐标系下的点$\boldsymbol{p_i^w}$经过变换$\left[ R \big| t \right]$之后得到相机坐标系下的坐标 $\boldsymbol{p_i^c}$。对控制点$\boldsymbol{c_j^w}$做相同的变换得到$\boldsymbol{c_j^c}$，仍然满足相同的加权关系，即：

$$ \boldsymbol{p_i^c} = \sum_{j=1}^4 \alpha_{ij} \boldsymbol{c_j^c}, \text{with} \sum_{j=1}^4 \alpha_{ij} = 1 $$

再考虑到针孔相机模型，对于每一个 3D-2D 匹配点关系，都有下式成立:

$$ \forall i, w_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_u & 0 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix}\left( \sum_{j=1}^4 \alpha_{ij} \begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix}\right) $$

其中，$\boldsymbol{{c_j^c}}^T = \begin{bmatrix} x_j^c & y_j^c & z_j^c \end{bmatrix} $是第j个控制点在相机坐标系下的坐标。$u_i, v_i$是2D像素坐标， $f_u, f_v, u_c, v_c$分别是相机的焦距和光心，$w_i$是一个尺度因子，将在后续计算中约去。参照 DLT 算法的套路，每个点都可以写出两个约束，n个点可以构造如下的 $\boldsymbol{Mx} = \boldsymbol{0}$形式的线性方程组:

$$ \begin{bmatrix} \alpha_{11} f_x & 0 & \alpha_{11}(c_x - u_1) & \cdots & \alpha_{14} f_x & 0 & \alpha_{14}(c_x - u_1) \\ 0 & \alpha_{11} f_y & \alpha_{11}(c_y - v_1) & \cdots & 0 & \alpha_{14} f_y & \alpha_{14}(c_y - v_1) \\ \vdots & \vdots & \vdots & & \vdots & \vdots & \vdots \\ \alpha_{n1} f_x & 0 & \alpha_{n1}(c_x - u_1) & \cdots & \alpha_{n4} f_x & 0 & \alpha_{n4}(c_x - u_1) \\ 0 & \alpha_{n1} f_y & \alpha_{n1}(c_y - v_1) & \cdots & 0 & \alpha_{n4} f_y & \alpha_{n4}(c_y - v_1) \\ \end{bmatrix} \begin{bmatrix} \boldsymbol{c_1^c} \\ \vdots \\ \boldsymbol{c_4^c} \end{bmatrix} = \boldsymbol{0} $$

其中，$\boldsymbol{x} = \begin{bmatrix} \boldsymbol{{c_1^c}}^T & \boldsymbol{{c_2^c}}^T & \boldsymbol{{c_3^c}}^T & \boldsymbol{{c_4^c}}^T \end{bmatrix}^T$。求解出 $\boldsymbol{x}$ 就得到了相机坐标系下的控制点坐标。在下面的代码片段中，先通过 OpenCV 的接口创建了一个$2n \times 12$的矩阵，再通过函数 fill_M 完成上式中矩阵 $\boldsymbol{M}$ 的构建工作。

            CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
            for(int i = 0; i < number_of_correspondences; i++)
                fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]);

根据线性代数的说法，这个方程组的解一定属于 $\boldsymbol{M}$ 的零空间，既，$\boldsymbol{x}$ 可以写成 $\boldsymbol{M}$ 的零空间正交基的线性组合。

$$ \boldsymbol{x} = \sum_{i = 1}^N { \beta_i \boldsymbol{v}_i } $$

其中，$\boldsymbol{v}_i, i \in [1, N]$ 是 $\boldsymbol{M}$ 的零空间正交基，既该矩阵的 $N$ 个为 0 的奇异值所对应的列向量。 $\beta_i$ 是线性组合的系数。通过对 $12 \times 12$ 的矩阵 $\boldsymbol{M}^T \boldsymbol{M}$进行奇异值分解可以得到$\boldsymbol{v}_i, i \in [1, N]$。下面的代码中，先构建了局部变量 MtM, D, Ut 分别用来记录矩阵 $\boldsymbol{M}^T \boldsymbol{M}$，以及它分解的特征值和特征向量。 OpenCV 的接口 cvMulTransposed 根据 $\boldsymbol{M}$ 生成 $\boldsymbol{M}^T \boldsymbol{M}$，cvSVD 完成实际的SVD分解操作。

            double mtm[12 * 12], d[12], ut[12 * 12];
            CvMat MtM = cvMat(12, 12, CV_64F, mtm);
            CvMat D   = cvMat(12,  1, CV_64F, d);
            CvMat Ut  = cvMat(12, 12, CV_64F, ut);
            cvMulTransposed(M, &MtM, 1);
            cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
            cvReleaseMat(&M);

现在，我们还需要确定零空间的秩 $N$ 和线性组合系数$\beta_i$。论文中通过一些实验发现， $N$ 跟相机的焦距之间呈现一种正相关的关系。下面是从原文中抠出来的一张图，图中横坐标是$\boldsymbol{M}^T \boldsymbol{M}$的12个奇异值，纵轴是奇异值的大小。不同的曲线对应着不同的焦距，随着焦距的增大，曲线不断下移。从右侧放大的图中可以看到，$f = 10000$时也就只有4个奇异值接近为0。于是乎，作者就针对 $N \in [1, 4]$ 的四种情况分别讨论了系数$\beta_i$的计算方法。

控制点坐标是经过旋转和平移，从世界坐标系转换到相机坐标系的。这两种变换都不会改变控制点之间的距离。因此存在如下的关系式:

$$ \begin{array}{rc} & \| \boldsymbol{c_i^c} - \boldsymbol{c_i^c} \| = \| \boldsymbol{c_i^w} - \boldsymbol{c_j^w} \| \\ \Rightarrow & \sum_{k = 1}^N \| { \beta_k \boldsymbol{v}_k^{[i]} } - { \beta_k \boldsymbol{v}_k^{[j]} } \| = \| \boldsymbol{c_i^w} - \boldsymbol{c_j^w} \| \\ \end{array} $$

其中，$\boldsymbol{v}_k^{[i]}$ 是零空间中特征向量$\boldsymbol{v}_k$ 与 $\boldsymbol{c_i^c}$ 对应的 $3 \times 1$ 的子向量。 EPnP 的论文中说是针对 $N = 1, \cdots 4$ 四种情况分别计算了部分 $\beta$ 的取值，然后通过高斯-牛顿的方法求解所有的 $\beta$值，构建相机坐标系下的控制点$\boldsymbol{c_i^c}$。接着，求解 ICP 问题计算相机的姿态矩阵和平移向量，选取使得重投影误差最小的那组解。实际看代码，好像只依次讨论了$N = 4, 2, 3$三种情况。

如下面的代码片段所示，在定义并计算了一些用于求解$\beta$的临时变量之后。compute_pose 函数先后调用了 find_betas_approx_?, gauss_newton, compute_R_and_t 完成了$\beta$和相机位姿的估计，数组 rep_erros 中保存了重投影误差。

展开准备求解$\beta$的临时变量的定义和计算 >>>

            double l_6x10[6 * 10], rho[6];
            CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
            CvMat Rho    = cvMat(6,  1, CV_64F, rho);

            compute_L_6x10(ut, l_6x10);
            compute_rho(rho);

            double Betas[4][4], rep_errors[4];
            double Rs[4][3][3], ts[4][3];

            // N = 4
            find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
            gauss_newton(&L_6x10, &Rho, Betas[1]);
            rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
            // N = 2
            find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
            gauss_newton(&L_6x10, &Rho, Betas[2]);
            rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
            // N = 3
            find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
            gauss_newton(&L_6x10, &Rho, Betas[3]);
            rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);

最后比较各个配置下的重投影误差，选取最小的那个作为最后的解，如下面的代码片段所示。

            int N = 1;
            if (rep_errors[2] < rep_errors[1]) N = 2;
            if (rep_errors[3] < rep_errors[N]) N = 3;
            copy_R_and_t(Rs[N], ts[N], R, t);
            return rep_errors[N];
        }

4. 完

本文中，我们介绍了根据 3D-2D 映射关系求解相机位姿的 PnP 问题。先介绍了最直接的 DLT 算法，然后详细分析了 ORB-SLAM2 中的 PnPsolver 的迭代过程。最后结合论文和代码，深入讲解了 EPnP 的原理和求解过程。该算法的核心思想体现在三个方面：

将世界坐标系下的 $n$ 个 3D 点，用 4 个虚拟的控制点通过加权和的形式表达。
这种虚拟控制点的加权和关系，经过相机位姿 $\left[ \boldsymbol{R} \big | \boldsymbol{t} \right]$ 变换到相机坐标系下仍然成立，既对应点的加权系数不变。
根据2D像素坐标估计出 4 个虚拟的控制点在相机坐标系的坐标，可以将 3D-2D 的映射问题转换为 3D-3D 点映射问题。