ORB EPNP

Pnp

《摄像机位姿的加权线性算法》学报中有介绍

• 给定摄像机内参数，从空间点与它的图像点对应估计摄像机的旋转 R 和位置 c，通常称为摄像机位姿估计问题，也称为 PnP 问题

• 解决 PnP 问题所需要的最少点对应数为 3 (P3P)，它最多有 4 个解，后又提出 P3P 问题。Quan 和 Lan 对于 P4P 和 P5P 问题给出了线性 SVD 算法 [2] ，他们把 PnP 问题转化为多个 P3P 问题，根据 P3P 问题之间的关系线性求解 PnP，其算法时间复杂度较高。

• 在 n≥6 的情况下，经典的线性算法是 DLT(direct linear transformation) 算法 [1] ，它忽略了旋转矩阵的正交约束,将投影矩阵 12 个变量看作独立变量而得到线性约束。

• 最近，Moreno 和 Lepetit 提出了一种高效、高精度线性算法 EPnP [4] ，其基本思想是把空间点都通过 4 个虚拟的控制点所建立的坐标系来表示，其中 4 个控制点的选取分别为空间点集的中心以及 3 个主方向上的单位点。然后根据图像点的信息求解 4 个控制点的摄像机坐标系下的坐标，进而得到旋转矩阵和摄像机坐。EPnP 被认为是一种高效的线性算法，其时间复杂度为 O(n)，但实验结果表明，当深度幅度变化较为剧烈时(即深度比较小时)，EPnP 算法难以获得较好的估计结果。

• MLPnp 提出了PnP问题的极大似然(ML)解的一个新公式。为了避免奇异协方差矩阵，提出了一种从图像观测到方位向量的方差传播方法。 此外，对实时方法进行了基准测试，并在地面真实跟踪数据集上展示了如何使用我们的统计框架，以获得给定关于不确定观测的知识的未知数的准确性。

ORB-SLAM2

这里的pnp求解用的是 EPnP 的算法。

• 参考论文：EPnP:An Accurate O(n) Solution to the PnP problem
• https://en.wikipedia.org/wiki/Perspective-n-Point
• http://docs.ros.org/fuerte/api/re_vision/html/classepnp.html

Epnp 的思想：

• 将世界坐标系所有的3D点用四个虚拟的控制点来表示，将图像上对应的特征点转化为相机坐标系下的四个控制点
• 根据世界坐标系下的四个控制点与相机坐标系下对应的四个控制点（与世界坐标系下四个控制点有相同尺度）即可恢复出(R t)

步骤：

• //                                   |x|
  //   |u|   |fx r  u0||r11 r12 r13 t1||y|
  // s |v| = |0  fy v0||r21 r22 r23 t2||z|
  //   |1|   |0  0  1 ||r32 r32 r33 t3||1|// step1:用四个控制点来表达所有的3D点
  // p_w = sigma(alphas_j * pctrl_w_j), j从0到4
  // p_c = sigma(alphas_j * pctrl_c_j), j从0到4
  // sigma(alphas_j) = 1,  j从0到4// step2:根据针孔投影模型
  // s * u = K * sigma(alphas_j * pctrl_c_j), j从0到4// step3:将step2的式子展开, 消去s
  // sigma(alphas_j * fx * Xctrl_c_j) + alphas_j * (u0-u)*Zctrl_c_j = 0
  // sigma(alphas_j * fy * Xctrl_c_j) + alphas_j * (v0-u)*Zctrl_c_j = 0// step4:将step3中的12未知参数（4个控制点*3维参考点坐标）提成列向量
  // Mx = 0,计算得到初始的解x后可以用Gauss-Newton来提纯得到四个相机坐标系的控制点// step5:根据得到的p_w和对应的p_c，最小化重投影误差即可求解出R t
  

PnPsolver

• 算法原理：

  • 粗求位姿：${}_m{T}_{cw}$
    • 在3D-2D匹配点中随机选取4个匹配点对（4个点是因为EPnP算法需要至少4个点），根据这4个匹配点对使用EPnP算法求得一个粗略的位姿
  • 求局内点
  • 根据局内点再求位姿mRefineTcw

构造函数

// pcs表示3D点在camera坐标系下的坐标
// pws表示3D点在世界坐标系下的坐标
// us表示图像坐标系下的2D点坐标
// alphas为真实3D点用4个虚拟控制点表达时的系数
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;for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++){MapPoint* pMP = vpMapPointMatches[i];//依次获取一个MapPointif(pMP){	// 依次取每一个匹配的地图点，且地图点非空、非坏if(!pMP->isBad()){const cv::KeyPoint &kp = F.mvKeysUn[i];//得到2维特征点, 将KeyPoint类型变为Point2fmvP2D.push_back(kp.pt);//存放到mvP2D容器mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);//记录特征点是在哪一层提取出来的cv::Mat Pos = pMP->GetWorldPos();//世界坐标系下的3D点mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));//记录被使用特征点在原始特征点容器中的索引, mvKeyPointIndices是跳跃的，因mvKeyPointIndices.push_back(i);mvAllIndices.push_back(idx);//记录被使用特征点的索引, mvAllIndices是连续的idx++;}}}// Set camera calibration parametersfu = F.fx;fv = F.fy;uc = F.cx;vc = F.cy;SetRansacParameters();
}

参数设置：

// 设置RANSAC迭代的参数
void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4, float th2 = 5.991)
{ mRansacProb = probability; // Ransac概率mRansacMinInliers = minInliers;  // 最小迭代次数mRansacMaxIts = maxIterations; //最大迭代次数mRansacEpsilon = epsilon;mRansacMinSet = minSet;N = mvP2D.size(); // number of correspondences, 所有二维特征点个数mvbInliersi.resize(N);// inlier index, mvbInliersi记录每次迭代inlier的点// Adjust Parameters according to number of correspondencesint nMinInliers = N*mRansacEpsilon;// RANSAC的残差if(nMinInliers<mRansacMinInliers)nMinInliers=mRansacMinInliers;if(nMinInliers<minSet)nMinInliers=minSet;mRansacMinInliers = nMinInliers;if(mRansacEpsilon<(float)mRansacMinInliers/N)mRansacEpsilon=(float)mRansacMinInliers/N;// Set RANSAC iterations according to probability, epsilon, and max iterationsint nIterations;if(mRansacMinInliers==N)//根据期望的残差大小来计算RANSAC需要迭代的次数，最大迭代次数比该值大且小于最大值nIterations=1;elsenIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3)));mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));mvMaxError.resize(mvSigma2.size());// 图像提取特征的时候尺度层数for(size_t i=0; i<mvSigma2.size(); i++)// 不同的尺度，设置不同的最大偏差mvMaxError[i] = mvSigma2[i]*th2;
}

迭代求解

cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
{bNoMore = false;vbInliers.clear();nInliers=0;  // 根据ransac概率值计算出来的迭代次数// mRansacMinSet为每次RANSAC需要的特征点数，默认为4组3D-2D对应点set_maximum_number_of_correspondences(mRansacMinSet);// N为所有2D点的个数, mRansacMinInliers为RANSAC迭代过程中最少的inlier数,-// 若匹配点对N小于Racscac所需最小特征点时，returnif(N<mRansacMinInliers){bNoMore = true;return cv::Mat();}// mvAllIndices为所有参与PnP的2D点的索引// vAvailableIndices为每次从mvAllIndices中随机挑选mRansacMinSet组3D-2D对应点进行一次RANSACvector<size_t> vAvailableIndices;int nCurrentIterations = 0;while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations){nCurrentIterations++;  // 记录每调用一次iterate函数，内部会有多次迭代mnIterations++;  // 记录iterate调用次数 * 内部迭代次数总迭代次数reset_correspondences();vAvailableIndices = mvAllIndices;  // 匹配点的连续下标赋值给`vAvailableIndices`// Get min set of points/// 随机获取四个点，并删之，下次不能再使用,mRansacMinSet默认为4for(short i = 0; i < mRansacMinSet; ++i){int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);int idx = vAvailableIndices[randi];// 将对应的3D-2D压入到pws和us 11111add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}// Compute camera pose  计算相机的位姿compute_pose(mRi, mti);// Check inliersCheckInliers();/// 遍历每一个点，通过之前求解的(R t)检查哪些3D-2D点对属于inliers/// 将3D点由世界坐标系旋转到相机坐标系 。。 。此处用到了之前求解的(R t)/// 将相机坐标系下的3D进行针孔投影   -内参(orb3中使用了相机模型)/// 计算残差大小，2D-2D的欧氏距离 /// 根据残差大小及阈值设定 是否为外点if(mnInliersi>=mRansacMinInliers)  // 若迭代次数已经不小于最小迭代次数{// If it is the best solution so far, save it// 内点数大于最优内点数，即这是最好的解决方案，请保存 mBestTcwif(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));}/// 根据局内点再次使用EPnP计算位姿<font color="red">Refine</font>/// 将最优迭代的内点都放入计算位姿匹配对，不是上述的4对/// 计算相机位姿 <font color="red">compute_pose</font>/// 检测内点  <font color="red">CheckInliers</font> // 若内点个数大于 Ransac 最小迭代个数时，返回true，否则返回false///  若返回true，则计算成功返回 计算值，否则继续迭代if(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();}}}/// 若迭代次数完成后，即迭代次数大于最大迭代次数if(mnIterations>=mRansacMaxIts){bNoMore=true;// 若最优内点个数 大于 Ransac 最小迭代个数时，返回最优次数if(mnBestInliers>=mRansacMinInliers) {nInliers=mnBestInliers;vbInliers = vector<bool>(mvpMapPointMatches.size(),false);for(int i=0; i<N; i++){if(mvbBestInliers[i])vbInliers[mvKeyPointIndices[i]] = true;}return mBestTcw.clone();}}return cv::Mat();
}

EPnp 求解

• 步骤1：选取四个控制点 $C^w$

  • 用世界坐标系下特征点的质心坐标，和主成分分析法(PCA)得到的三个主方向向量，得到四个控制点坐标。
  • 第一个点，质心坐标：$C_1^w=\frac{1}{n}{\sum }_{i=1}^n{P}_i^w,C_1^w=(x_1^w,y_1^w,z_1^w{)}^T$
  • 构建去质心矩阵A，计算另外3个点的坐标：$A_{n\times 3}=\left[\begin{array}{c}(P_1^w{)}^T-(C_1^w{)}^T\\ ⋮\\ (P_n^w{)}^T-(C_n^w{)}^T\end{array}\right]$
  • SVD(奇异值分解)求解 $A^{T}A$（3X3矩阵）。由于参考点的坐标基本上都是不相同的，所以$A^{T}A$肯定是非奇异的，就有3个线性无关的特征向量。另外3个坐标点可以表示为：$\{\begin{array}{c}{C}_2^w=C_1^w+\sqrt{\frac{{\lambda }_1}{n}}\cdot \overrightarrow{v_1}\\ C_3^w=C_1^w+\sqrt{\frac{{\lambda }_2}{n}}\cdot \overrightarrow{v_2}\\ C_4^w=C_1^w+\sqrt{\frac{{\lambda }_3}{n}}\cdot \overrightarrow{v_3}\end{array}$
  • 求解 $A^{T}A$的原因：主成分分析法要求主成分矩阵各成分之间的协方差为0，所以主成分矩阵的协方差矩阵是对角矩阵，对角线上是方差，非对角线上是协方差，并且经过推导，主成分矩阵的协方差矩阵是可以通过数据集的协方差矩阵 $\frac{1}{n}{A}^{T}A$矩阵得到，所以通过SVD求解$A^{T}A$可以得到主成分矩阵，进而得到3个线性无关的向量

• 步骤2：求解4个控制点系数 $\alpha$

  • 世界坐标系下的每个三维点可以由4个控制点表达为：
    • $P_i^w={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^w$
    • ${\sum }_{j=1}^4{\alpha }_{ij}=1$
  • 由于$P_i^w=\left[\begin{array}{c}{x}_i^w\\ y_i^w\\ z_i^w\end{array}\right]=\left[\begin{array}{cccc}{C}_1^w& C_2^w& C_3^w& C_4^w\end{array}\right]\cdot \left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]$，且 ${\sum }_{i=1}^4{\alpha }_{ij}=1$，则可得到：
  • $\left[\begin{array}{c}{x}_i^w-c_{1x}^w\\ y_i^w-c_{1y}^w\\ z_i^w-c_{1z}^w\end{array}\right]=\left[\begin{array}{ccc}{C}_2^w-C_1^w& C_3^w-C_1^w& C_4^w-C_1^w\end{array}\right]\cdot \left[\begin{array}{c}{\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=>P^w=C^w\vec{\alpha }$
  • 所以：$\vec{\alpha }=(C^w{)}^{-1}{P}^w,{\alpha }_{i1}=1-{\alpha }_{i2}-{\alpha }_{i3}-{\alpha }_{i4}$，由4个点不共面，所以$C^w$肯定是可逆的

• 步骤3：初步求解相机坐标系下的控制点坐标 $C^c$， (此坐标不带有尺度信息)

  • 该坐标是由3D-2D中的2D点求解，该2D点是像素坐标系下的坐标，单位是像素。

    把相机投影模型搬出来：

    $s\cdot \left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=K\overrightarrow{p_i}=K{\sum }_{j=1}^4{\alpha }_{ij}{C}^c=\left[\begin{array}{ccc}{f}_u& 0& c_x\\ 0& f_v& c_y\\ 0& 0& 1\end{array}\right]\cdot {\sum }_{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]$

  • 由上式可得：$s\cdot \left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]={\sum }_{j=1}^4{\alpha }_{ij}{z}_j^{c}s\cdot \left[\begin{array}{c}\frac{{\sum }_{j=1}^4{\alpha }_{ij}(f_u{x}_j^c+c_x{z}_j^c)}{{\sum }_{j=1}^4{\alpha }_{ij}{z}_j^c}\\ \frac{{\sum }_{j=1}^4{\alpha }_{ij}(f_v{y}_j^c+c_y{z}_j^c)}{{\sum }_{j=1}^4{\alpha }_{ij}{z}_j^c}\\ 1\end{array}\right]$

  • 消去上式左右系数，得到两个方程：

    $\{\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{f}_u{x}_j^c+{\alpha }_{ij}(c_x-u_i)z_j^c=0\\ {\sum }_{j=1}^4{\alpha }_{ij}{f}_v{y}_j^c+{\alpha }_{ij}(c_y-v_i)z_j^c=0\end{array}$

  • 仔细观察上式，${\alpha }_{ij}$已由步骤2所得，相机内参和2D点坐标均为已知量，只剩下4个控制点在相机坐标系下的坐标 $x_j^c,y_j^c,z_j^c,(j=1,2,3,4)$，共有12个位置参数，一个2D点得两个方程，$n$个点可得 $2n$ 个方程，

    • 有线性方程组：$M\vec{x}=0,\vec{x}={\left[\begin{array}{cccc}(C_1^c{)}^T& (C_2^c{)}^T& (C_3^c{)}^T& (C_4^c{)}^T\end{array}\right]}^T$

  • $M$是一个$2n\times 12$的矩阵，上式的解即为$M$的零空间，通过SVD求解方程，可解得最小二乘解 $\vec{x}$，即相机坐标系下4个控制点的坐标，其中 ${\beta }_i,\overrightarrow{v_i}(i=1,\mathrm{2...},n)$分别是$M^{T}M$的特征值和特征向量，$n$ 可取1,2,3,4，具体跟相机的焦距有关，此处只考虑n=4的情况。

    $\vec{x}={\sum }_{i=1}^n{\beta }_i\cdot \overrightarrow{v_i},\overrightarrow{v_i}={\left[\begin{array}{cccc}\overrightarrow{v_{i1}}& \overrightarrow{v_{i2}}& \overrightarrow{v_{i3}}& \overrightarrow{v_{i4}}\end{array}\right]}^T$

  • 其中$v_i$是$M$的右奇异向量，对应的奇异值为0。具体解算方法为：求解$M^{T}M$的特征值和特征向量，特征值为0的特征向量即为$v_i$。需要注意，不论有多少点对，$M^{T}M$永远是$12\times 12$，而计算$M^{T}M$的复杂度为$O(n)$。

  • 但此时的结果并不带有尺度，所以还需要求解尺度。

• 步骤4：求解尺度，确定相机坐标系下4个控制点坐标 $C^c$

  • A，根据位姿变换不改变两点之间的距离有以下方程：$||C_j^c-C_j^c|{|}^2=||C_i^w-C_j^w|{|}^2$

  • 只考虑 $n=4$ 的情况，4个控制点有$C_4^2=6$个距离，所以上述方程有6个。

  • $\begin{array}{rl}{D}_{ij}^c=||C_i^c-C_j^c|{|}^2& =||{\beta }_1\overrightarrow{v_{1i}}+{\beta }_2\overrightarrow{v_{2i}}+{\beta }_3\overrightarrow{v_{3i}}+{\beta }_4\overrightarrow{v_{4i}}-{\beta }_1\overrightarrow{v_{1j}}+{\beta }_2\overrightarrow{v_{2j}}+{\beta }_3\overrightarrow{v_{3j}}+{\beta }_4\overrightarrow{v_{4j}}|{|}^2\\ & =||{\beta }_1(\overrightarrow{v_{1i}}-\overrightarrow{v_{1j}})+{\beta }_2(\overrightarrow{v_{2i}}-\overrightarrow{v_{2j}})+{\beta }_3(\overrightarrow{v_{3i}}-\overrightarrow{v_{3j}})+{\beta }_4(\overrightarrow{v_{4i}}-\overrightarrow{v_{4j}})|{|}^2\\ & =||{\beta }_1\overrightarrow{d{v}_{1k}}+{\beta }_2\overrightarrow{d{v}_{2k}}+{\beta }_3\overrightarrow{d{v}_{3k}}+{\beta }_4\overrightarrow{d{v}_{4k}}|{|}^2\\ & ={\beta }_1{\overrightarrow{d{v}_{1k}}}^2+2{\beta }_1{\beta }_2\overrightarrow{d{v}_{1k}}\overrightarrow{d{v}_{2k}}+2{\beta }_1{\beta }_3\overrightarrow{d{v}_{1k}}\overrightarrow{d{v}_{3k}}+2{\beta }_1{\beta }_4\overrightarrow{d{v}_{1k}}\overrightarrow{d{v}_{4k}}+\\ & +{\beta }_2{\overrightarrow{d{v}_{2k}}}^2+2{\beta }_2{\beta }_2\overrightarrow{d{v}_{2k}}\overrightarrow{d{v}_{2k}}+2{\beta }_2{\beta }_4\overrightarrow{d{v}_{2k}}\overrightarrow{d{v}_{4k}}+2{\beta }_1{\beta }_2\overrightarrow{d{v}_{1k}}\overrightarrow{d{v}_{2k}}+\\ & +{\beta }_3{\overrightarrow{d{v}_{3k}}}^2+2{\beta }_2{\beta }_3\overrightarrow{d{v}_{2k}}\overrightarrow{d{v}_{3k}}+{\beta }_4{\overrightarrow{d{v}_{4k}}}^2\end{array}$

  • 上式中 $k=1,2,3,4,5,6$，六种情况，代表6个距离，其中：

    $\{\begin{array}{c}{\overrightarrow{d{v}_{ik}}}^2=(d{x}_{ik}{)}^2+(d{y}_{ik}{)}^2+(d{z}_{ik}{)}^2\\ \overrightarrow{d{v}_{ik}}\overrightarrow{d{v}_{jk}}=d{x}_{ik}d{x}_{jk}+d{y}_{ik}d{y}_{jk}+d{z}_{ik}d{z}_{jk}\end{array}$

  • 上上式写成矩阵，即：$L_{6\times 10}\cdot {\beta }_{10\times 1}={\rho }_{6\times 1}$，$\rho$是控制点在世界坐标系下对应的6个距离

    $L_{6\times 10}=[d{v}_1^2,2d{v}_{1}d{v}_2,2d{v}_{1}d{v}_3,2d{v}_{1}d{v}_4,d{v}_2^2,2d{v}_{2}d{v}_3,2d{v}_{2}d{v}_4,d{v}_3^2,2d{v}_{3}d{v}_4,d{v}_4^2{]}_k$

    ${\beta }_{10\times 1}=[{\beta }_1^2,{\beta }_1{\beta }_2,{\beta }_1{\beta }_3,{\beta }_1{\beta }_4,{\beta }_2^2,{\beta }_2{\beta }_3,{\beta }_2{\beta }_4,{\beta }_3^2,{\beta }_3{\beta }_4,{\beta }_4^2{]}^T$

  • 上述式子，$L_{6\times 10}$已知，$\rho$已知，$\beta$未知的，有10个未知量，而我们只有6个方程，约束不够，但EPnP说不用怕，先另 $\beta$ 中的几项为0，把未知量控制住，使用SVD求解法，求出有一个初始值，然后使用高斯牛顿法来优化，最后得到一个优化值，ORB-SLAM2中将 $\beta$ 某几项设为0时，使用3种不同的设置方法，对应的函数为分别为：

    void PnPsolver::find_betas_approx_1();//[B11 B12 B13 B14]，其余为0
    void PnPsolver::find_betas_approx_2();//[B11 B12 B22]，其余为0
    void PnPsolver::find_betas_approx_3();//[B11 B12 B22 B13 B23]，其余为0
    

  • B，高斯-牛顿法优化，求解尺度 $\beta$ gauss_newton函数

  • 高斯-牛顿法具体原理可点击此处，简单来说，高斯-牛顿法是将两个雅克比矩阵相乘近似为海森矩阵，然后根据牛顿法求解，即 $H=J^{T}J$

  • 求$L_{6\times 10}$的雅克比矩阵，分布对 ${\beta }_1,{\beta }_2,{\beta }_3,{\beta }_4$求导，得到雅克比矩阵$J_{6\times 4}$，其中的 ${\beta }_1,{\beta }_2,{\beta }_3,{\beta }_4$ 先使用该步骤中的值计算

    $J_{6\times 4}={\left[\begin{array}{c}(2{\beta }_{1}d{v}_1^2+2{\beta }_{2}d{v}_{1}d{v}_2+2{\beta }_{3}d{v}_{1}d{v}_3+2{\beta }_{4}d{v}_{1}d{v}_4)\\ (2{\beta }_{2}d{v}_2^2+2{\beta }_{1}d{v}_{1}d{v}_2+2{\beta }_{3}d{v}_{2}d{v}_3+2{\beta }_{4}d{v}_{2}d{v}_4)\\ (2{\beta }_{3}d{v}_3^2+2{\beta }_{2}d{v}_{2}d{v}_3+2{\beta }_{1}d{v}_{1}d{v}_3+2{\beta }_{4}d{v}_{3}d{v}_4)\\ (2{\beta }_{4}d{v}_3^4+2{\beta }_{1}d{v}_{1}d{v}_4+2{\beta }_{2}d{v}_{2}d{v}_4+2{\beta }_{3}d{v}_{3}d{v}_4)\end{array}\right]}^T$

  • 求残差矩阵 $b$

    $b=\left[\begin{array}{c}{\rho }_1-\ast \ast \\ ⋮\\ {\rho }_6-\ast \ast \end{array}\right]$

  • 求解方程，$J_{6\times 4}\cdot \Delta x_{4\times 1}=b$，其中$\Delta x$是步长，使用QR方法求解，得到$\Delta x$，进而得到新的${\beta }_{4\times 1}$，${\beta }_{4\times 1}={\beta }_{4\times 1}+\Delta x_{4\times 1}$。

    • 使用新$\beta$更新步骤1，再次计算雅克比矩阵和残差矩阵，如此循环迭代（ORB-SLAM2中设置迭代次数为5次），求得尺度矩阵$\beta$，进而求解得到4个控制点在相机坐标系下的坐标$C^c$。

• 步骤5：求解R，t和误差(单位为像素) compute_R_and_t

  • 由步骤2得到的alphas，和步骤4得到$C^c$，可以表示出相机坐标系下的所有3D点 $P^c$。

    $P_i^c={\alpha }_{1i}{C}_1^c+{\alpha }_{2i}{C}_2^c+{\alpha }_{3i}{C}_3^c+{\alpha }_{4i}{C}_4^c,i=1,2,...,n$

  • 由于$P_i^c$的z分量必须为正，所以还需要对$P_i^c$进行符号修正，当$z<0$时，对$x,y,z$分量同时取相反数，保证z分量是正数。

  • 根据已知的世界坐标系下3D点$P_i^w$和此处$P_i^c$，问题转换为3D-3D点求解$R,t$，ORB-SLAM2使用SVD求解ICP问题，具体可参考高翔的《SLAM十四讲》，此处便不再做详解。求解完R, t之后还要求重投影误差rep_error。

  • ORB-SLAM2在步骤4求解尺度时，对 ${\beta }_{10\times 1}$ 设置了3种不同的假设方法，得到3种$P_i^c$，所以上一步需要要求得到3组不同的$R,t$ 和 rep_error，比较3组中的rep_error，选择误差最小的一组$R,t$作为EPnP的最终结果。

计算Pose

double PnPsolver::compute_pose(double R[3][3], double t[3])
{// 步骤1：获得EPnP算法中的四个控制点 22222choose_control_points();// 步骤2：计算世界坐标系下每个3D点用4个控制点线性表达时的系数alphas，公式1compute_barycentric_coordinates();// 步骤3：构造M矩阵，公式(3)(4)-->(5)(6)(7)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]);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);// 步骤3：求解Mx = 0// SVD分解M'McvMulTransposed(M, &MtM, 1);cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);//得到向量utcvReleaseMat(&M);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，并求解部分betas（如果全求解出来会有冲突）// 通过优化得到剩下的betas// 最后计算R t// EPnP论文公式10 15find_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]);// EPnP论文公式11 15find_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]);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];
}

1、选择控制点

void PnPsolver::choose_control_points(void)
{// Take C0 as the reference points centroid:// 步骤1：第一个控制点：参与PnP计算的参考3D点的几何中心cws[0][0] = cws[0][1] = cws[0][2] = 0;for(int i = 0; i < number_of_correspondences; i++)for(int j = 0; j < 3; j++)cws[0][j] += pws[3 * i + j];for(int j = 0; j < 3; j++)cws[0][j] /= number_of_correspondences;// Take C1, C2, and C3 from PCA on the reference points:// 步骤2：计算其它三个控制点，C1, C2, C3通过PCA分解得到// 将所有的3D参考点写成矩阵，(number_of_correspondences *　３)的矩阵CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);double pw0tpw0[3 * 3], dc[3], uct[3 * 3];CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);CvMat DC      = cvMat(3, 1, CV_64F, dc);CvMat UCt     = cvMat(3, 3, CV_64F, uct);// 步骤2.1：将存在pws中的参考3D点减去第一个控制点的坐标（相当于把第一个控制点作为原点）, 并存入PW0for(int i = 0; i < number_of_correspondences; i++)for(int j = 0; j < 3; j++)PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j]; // P^w_i - C^w_1// 步骤2.2：利用SVD分解P'P可以获得P的主分量// 类似于齐次线性最小二乘求解的过程，cvMulTransposed(PW0, &PW0tPW0, 1);   // PW0tPW0 = PW0的转置乘以PW0cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);cvReleaseMat(&PW0);	// 释放内存// 步骤2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量for(int i = 1; i < 4; i++) {double k = sqrt(dc[i - 1] / number_of_correspondences); // k = \sqrt(\lambta_i \ n)for(int j = 0; j < 3; j++)cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];}
}

2、求解控制点系数

// 求解四个控制点的系数alphas
// (a2 a3 a4)' = inverse(cws2-cws1 cws3-cws1 cws4-cws1)*(pws-cws1)，a1 = 1-a2-a3-a4
// 每一个3D控制点，都有一组alphas与之对应
// cws1 cws2 cws3 cws4为四个控制点的坐标
// pws为3D参考点的坐标
void PnPsolver::compute_barycentric_coordinates(void)
{double cc[3 * 3], cc_inv[3 * 3];CvMat CC     = cvMat(3, 3, CV_64F, cc);CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);// 第一个控制点在质心的位置，后面三个控制点减去第一个控制点的坐标（以第一个控制点为原点）// 步骤1：减去质心后得到x y z轴// // cws的排列 |cws1_x cws1_y cws1_z|  ---> |cws1|//          |cws2_x cws2_y cws2_z|       |cws2|//          |cws3_x cws3_y cws3_z|       |cws3|//          |cws4_x cws4_y cws4_z|       |cws4|//          // cc的排列  |cc2_x cc3_x cc4_x|  --->|cc2 cc3 cc4|//          |cc2_y cc3_y cc4_y|//          |cc2_z cc3_z cc4_z|for(int i = 0; i < 3; i++)for(int j = 1; j < 4; j++)cc[3 * i + j - 1] = cws[j][i] - cws[0][i];cvInvert(&CC, &CC_inv, CV_SVD);double * ci = cc_inv;for(int i = 0; i < number_of_correspondences; i++) {double * pi = pws + 3 * i;// pi指向第i个3D点的首地址double * a = alphas + 4 * i;// a指向第i个控制点系数alphas的首地址// pi[]-cws[0][]表示将pi和步骤1进行相同的平移for(int j = 0; j < 3; j++)a[1 + j] = ci[3 * j    ] * (pi[0] - cws[0][0]) +ci[3 * j + 1] * (pi[1] - cws[0][1]) +ci[3 * j + 2] * (pi[2] - cws[0][2]);a[0] = 1.0f - a[1] - a[2] - a[3];}
}

3、初步求解相机坐标系下的控制点坐标

fill_M 函数，即得到 每个点的位置方程

• 对每个参考点，$2\times 12$ 进行赋值，具体格式见下：
  • $\{\begin{array}{c}{\sum }_{j=1}^4{\alpha }_{ij}{f}_u{x}_j^c+{\alpha }_{ij}(c_x-u_i)z_j^c=0\\ {\sum }_{j=1}^4{\alpha }_{ij}{f}_v{y}_j^c+{\alpha }_{ij}(c_y-v_i)z_j^c=0\end{array}$

// 填充最小二乘的M矩阵
// 对每一个3D参考点：
// |ai1 0    -ai1*ui, ai2  0    -ai2*ui, ai3 0   -ai3*ui, ai4 0   -ai4*ui|
// |0   ai1  -ai1*vi, 0    ai2  -ai2*vi, 0   ai3 -ai3*vi, 0   ai4 -ai4*vi|
// 其中i从0到4
void PnPsolver::fill_M(CvMat * M,const int row, const double * as, const double u, const double v)
{double * M1 = M->data.db + row * 12;double * M2 = M1 + 12;for(int i = 0; i < 4; i++) {M1[3 * i    ] = as[i] * fu;M1[3 * i + 1] = 0.0;M1[3 * i + 2] = as[i] * (uc - u);M2[3 * i    ] = 0.0;M2[3 * i + 1] = as[i] * fv;M2[3 * i + 2] = as[i] * (vc - v);}
}

求解控制点坐标，先求出特征值和特征向量，由于此时的结果并不带有尺度，所以还需要求解尺度。

$\vec{x}={\sum }_{i=1}^n{\beta }_i\cdot \overrightarrow{v_i},\overrightarrow{v_i}={\left[\begin{array}{cccc}\overrightarrow{v_{i1}}& \overrightarrow{v_{i2}}& \overrightarrow{v_{i3}}& \overrightarrow{v_{i4}}\end{array}\right]}^T$。

  // 步骤3：构造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]);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);// 步骤3：求解Mx = 0// SVD分解M'McvMulTransposed(M, &MtM, 1);cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);//得到向量utcvReleaseMat(&M);

4、求解尺度

$||C_j^c-C_j^c|{|}^2=||C_i^w-C_j^w|{|}^2$ 整理后有：

$L_{6\times 10}\cdot {\beta }_{10\times 1}={\rho }_{6\times 1}$

求解尺度 $\beta$

令 $\beta$ 中某几项为0(orb 写了三种)，得到 解

  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);// 计算 d^c_{i,j} 和 d^w_{i,j}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，并求解部分betas（如果全求解出来会有冲突）// 通过优化得到剩下的betas,最后计算R t// EPnP论文公式10 15find_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]);// EPnP论文公式11 15find_betas_approx_2(&L_6x10, &Rho, Betas[2]);gauss_newton(&L_6x10, &Rho, Betas[2]);

compute_L_6x10 $D_{ij}^c=||C_i^c-C_j^c|{|}^2$

// 计算并填充矩阵L
void PnPsolver::compute_L_6x10(const double * ut, double * l_6x10)
{const double * v[4];v[0] = ut + 12 * 11;v[1] = ut + 12 * 10;v[2] = ut + 12 *  9;v[3] = ut + 12 *  8;double dv[4][6][3];for(int i = 0; i < 4; i++) {int a = 0, b = 1;for(int j = 0; j < 6; j++) {dv[i][j][0] = v[i][3 * a    ] - v[i][3 * b];dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];b++;if (b > 3) {a++;b = a + 1;}}}for(int i = 0; i < 6; i++) {double * row = l_6x10 + 10 * i;row[0] =        dot(dv[0][i], dv[0][i]);row[1] = 2.0f * dot(dv[0][i], dv[1][i]);row[2] =        dot(dv[1][i], dv[1][i]);row[3] = 2.0f * dot(dv[0][i], dv[2][i]);row[4] = 2.0f * dot(dv[1][i], dv[2][i]);row[5] =        dot(dv[2][i], dv[2][i]);row[6] = 2.0f * dot(dv[0][i], dv[3][i]);row[7] = 2.0f * dot(dv[1][i], dv[3][i]);row[8] = 2.0f * dot(dv[2][i], dv[3][i]);row[9] =        dot(dv[3][i], dv[3][i]);}
}

compute_rho $||C_i^w-C_j^w|{|}^2$

// 计算四个控制点任意两点间的距离，总共6个距离
void PnPsolver::compute_rho(double * rho)
{rho[0] = dist2(cws[0], cws[1]);rho[1] = dist2(cws[0], cws[2]);rho[2] = dist2(cws[0], cws[3]);rho[3] = dist2(cws[1], cws[2]);rho[4] = dist2(cws[1], cws[3]);rho[5] = dist2(cws[2], cws[3]);
}double PnPsolver::dist2(const double * p1, const double * p2)
{return(p1[0] - p2[0]) * (p1[0] - p2[0]) +(p1[1] - p2[1]) * (p1[1] - p2[1]) +(p1[2] - p2[2]) * (p1[2] - p2[2]);
}

有10个未知量，而我们只有6个方程，约束不够，但EPnP说不用怕，先另 $\beta$ 中的几项为0，把未知量控制住，使用SVD求解法，求出有一个初始值，然后使用高斯牛顿法来优化，最后得到一个优化值，ORB-SLAM2中将 $\beta$ 某几项设为0时，使用3种不同的设置方法，对应的函数为分别为：

// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_1 = [B11 B12     B13         B14]void PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,double * betas)
{double l_6x4[6 * 4], b4[4];CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);CvMat B4    = cvMat(4, 1, CV_64F, b4);for(int i = 0; i < 6; i++) {cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));}cvSolve(&L_6x4, Rho, &B4, CV_SVD);if (b4[0] < 0) {betas[0] = sqrt(-b4[0]);betas[1] = -b4[1] / betas[0];betas[2] = -b4[2] / betas[0];betas[3] = -b4[3] / betas[0];} else {betas[0] = sqrt(b4[0]);betas[1] = b4[1] / betas[0];betas[2] = b4[2] / betas[0];betas[3] = b4[3] / betas[0];}
}// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_2 = [B11 B12 B22                            ]void PnPsolver::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,double * betas)
{double l_6x3[6 * 3], b3[3];CvMat L_6x3  = cvMat(6, 3, CV_64F, l_6x3);CvMat B3     = cvMat(3, 1, CV_64F, b3);for(int i = 0; i < 6; i++) {cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));}cvSolve(&L_6x3, Rho, &B3, CV_SVD);if (b3[0] < 0) {betas[0] = sqrt(-b3[0]);betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;} else {betas[0] = sqrt(b3[0]);betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;}if (b3[1] < 0) betas[0] = -betas[0];betas[2] = 0.0;betas[3] = 0.0;
}// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_3 = [B11 B12 B22 B13 B23                    ]void PnPsolver::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,double * betas)
{double l_6x5[6 * 5], b5[5];CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);CvMat B5    = cvMat(5, 1, CV_64F, b5);for(int i = 0; i < 6; i++) {cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));}cvSolve(&L_6x5, Rho, &B5, CV_SVD);if (b5[0] < 0) {betas[0] = sqrt(-b5[0]);betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;} else {betas[0] = sqrt(b5[0]);betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;}if (b5[1] < 0) betas[0] = -betas[0];betas[2] = b5[3] / betas[0];betas[3] = 0.0;
}

高斯-牛顿法优化，求解尺度 $\beta$ gauss_newton函数

$L_{6\times 10}\cdot {\beta }_{10\times 1}={\rho }_{6\times 1}$

void PnPsolver::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,double betas[4])
{const int iterations_number = 5;double a[6*4], b[6], x[4];CvMat A = cvMat(6, 4, CV_64F, a);CvMat B = cvMat(6, 1, CV_64F, b);CvMat X = cvMat(4, 1, CV_64F, x);for(int k = 0; k < iterations_number; k++) {// 构建 Ax = bcompute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db, betas, &A, &B);// 求解 xqr_solve(&A, &B, &X);for(int i = 0; i < 4; i++)betas[i] += x[i];}
}//
void PnPsolver::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,double betas[4], CvMat * A, CvMat * b)
{for(int i = 0; i < 6; i++) {const double * rowL = l_6x10 + i * 10;double * rowA = A->data.db + i * 4;rowA[0] = 2 * rowL[0] * betas[0] +     rowL[1] * betas[1] +     rowL[3] * betas[2] +     rowL[6] * betas[3];rowA[1] =     rowL[1] * betas[0] + 2 * rowL[2] * betas[1] +     rowL[4] * betas[2] +     rowL[7] * betas[3];rowA[2] =     rowL[3] * betas[0] +     rowL[4] * betas[1] + 2 * rowL[5] * betas[2] +     rowL[8] * betas[3];rowA[3] =     rowL[6] * betas[0] +     rowL[7] * betas[1] +     rowL[8] * betas[2] + 2 * rowL[9] * betas[3];cvmSet(b, i, 0, rho[i] -(rowL[0] * betas[0] * betas[0] +rowL[1] * betas[0] * betas[1] +rowL[2] * betas[1] * betas[1] +rowL[3] * betas[0] * betas[2] +rowL[4] * betas[1] * betas[2] +rowL[5] * betas[2] * betas[2] +rowL[6] * betas[0] * betas[3] +rowL[7] * betas[1] * betas[3] +rowL[8] * betas[2] * betas[3] +rowL[9] * betas[3] * betas[3]));}
}void PnPsolver::qr_solve(CvMat * A, CvMat * b, CvMat * X)
{static int max_nr = 0;static double * A1, * A2;const int nr = A->rows;const int nc = A->cols;if (max_nr != 0 && max_nr < nr) {delete [] A1;delete [] A2;}if (max_nr < nr) {max_nr = nr;A1 = new double[nr];A2 = new double[nr];}double * pA = A->data.db, * ppAkk = pA;for(int k = 0; k < nc; k++) {double * ppAik = ppAkk, eta = fabs(*ppAik);for(int i = k + 1; i < nr; i++) {double elt = fabs(*ppAik);if (eta < elt) eta = elt;ppAik += nc;}if (eta == 0) {A1[k] = A2[k] = 0.0;cerr << "God damnit, A is singular, this shouldn't happen." << endl;return;} else {double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;for(int i = k; i < nr; i++) {*ppAik *= inv_eta;sum += *ppAik * *ppAik;ppAik += nc;}double sigma = sqrt(sum);if (*ppAkk < 0)sigma = -sigma;*ppAkk += sigma;A1[k] = sigma * *ppAkk;A2[k] = -eta * sigma;for(int j = k + 1; j < nc; j++) {double * ppAik = ppAkk, sum = 0;for(int i = k; i < nr; i++) {sum += *ppAik * ppAik[j - k];ppAik += nc;}double tau = sum / A1[k];ppAik = ppAkk;for(int i = k; i < nr; i++) {ppAik[j - k] -= tau * *ppAik;ppAik += nc;}}}ppAkk += nc + 1;}// b <- Qt bdouble * ppAjj = pA, * pb = b->data.db;for(int j = 0; j < nc; j++) {double * ppAij = ppAjj, tau = 0;for(int i = j; i < nr; i++)	{tau += *ppAij * pb[i];ppAij += nc;}tau /= A1[j];ppAij = ppAjj;for(int i = j; i < nr; i++) {pb[i] -= tau * *ppAij;ppAij += nc;}ppAjj += nc + 1;}// X = R-1 bdouble * pX = X->data.db;pX[nc - 1] = pb[nc - 1] / A2[nc - 1];for(int i = nc - 2; i >= 0; i--) {double * ppAij = pA + i * nc + (i + 1), sum = 0;for(int j = i + 1; j < nc; j++) {sum += *ppAij * pX[j];ppAij++;}pX[i] = (pb[i] - sum) / A2[i];}
}

5、求解R，t和误差(单位为像素)

• 3D-3D点求解$R,t$，ORB-SLAM2使用SVD求解ICP问题

double PnPsolver::compute_R_and_t(const double * ut, const double * betas,double R[3][3], double t[3])
{compute_ccs(betas, ut);compute_pcs();solve_for_sign();estimate_R_and_t(R, t);return reprojection_error(R, t);
}// 每一个控制点在相机坐标系下都表示为特征向量乘以beta的形式，EPnP论文的公式16
void PnPsolver::compute_ccs(const double * betas, const double * ut)
{for(int i = 0; i < 4; i++)ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;for(int i = 0; i < 4; i++) {const double * v = ut + 12 * (11 - i);for(int j = 0; j < 4; j++)for(int k = 0; k < 3; k++)ccs[j][k] += betas[i] * v[3 * j + k];}
}// 用四个控制点作为单位向量表示下的世界坐标系下3D点的坐标
void PnPsolver::compute_pcs(void)
{for(int i = 0; i < number_of_correspondences; i++) {double * a = alphas + 4 * i;double * pc = pcs + 3 * i;// {P^c_i=\alpha_{1i}C^c_1+\alpha_{2i}C^c_2+\alpha_{3i}C^c_3+\alpha_{4i}C^c_4,i=1,2,...,n}for(int j = 0; j < 3; j++)pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];}
}void PnPsolver::solve_for_sign(void)
{if (pcs[2] < 0.0) {for(int i = 0; i < 4; i++)for(int j = 0; j < 3; j++)ccs[i][j] = -ccs[i][j];for(int i = 0; i < number_of_correspondences; i++) {pcs[3 * i    ] = -pcs[3 * i];pcs[3 * i + 1] = -pcs[3 * i + 1];pcs[3 * i + 2] = -pcs[3 * i + 2];}}
}// 根据世界坐标系下的四个控制点与机体坐标下对应的四个控制点（和世界坐标系下四个控制点相同尺度），求取R t
void PnPsolver::estimate_R_and_t(double R[3][3], double t[3])
{double pc0[3], pw0[3];pc0[0] = pc0[1] = pc0[2] = 0.0;pw0[0] = pw0[1] = pw0[2] = 0.0;for(int i = 0; i < number_of_correspondences; i++) {const double * pc = pcs + 3 * i;const double * pw = pws + 3 * i;for(int j = 0; j < 3; j++) {pc0[j] += pc[j];pw0[j] += pw[j];}}for(int j = 0; j < 3; j++) {pc0[j] /= number_of_correspondences;pw0[j] /= number_of_correspondences;}double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];CvMat ABt   = cvMat(3, 3, CV_64F, abt);CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);cvSetZero(&ABt);for(int i = 0; i < number_of_correspondences; i++) {double * pc = pcs + 3 * i;double * pw = pws + 3 * i;for(int j = 0; j < 3; j++) {abt[3 * j    ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);}}cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);for(int i = 0; i < 3; i++)for(int j = 0; j < 3; j++)R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);const double det =R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];if (det < 0) {R[2][0] = -R[2][0];R[2][1] = -R[2][1];R[2][2] = -R[2][2];}t[0] = pc0[0] - dot(R[0], pw0);t[1] = pc0[1] - dot(R[1], pw0);t[2] = pc0[2] - dot(R[2], pw0);
}

ORB-SLAM3

• PnPsolver 函数也有，但最后调用的为：MLPnPsolver
• 在重定位时计算当前帧和候选帧的位姿变换，利用了3D-2D匹配。与ORB-SLAM2中的EPnP算法不同，MLPnP不依赖于相机模型，主要根据相机的反投影函数计算bearing vector。

MLPnPsolver

构造 与 EPnP相比，增加了反投影点

MLPnPsolver::MLPnPsolver(const Frame &F, const vector<MapPoint *> &vpMapPointMatches)://3D(地图点)-2D(Frame中匹配的关键点)匹配mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){mvpMapPointMatches = vpMapPointMatches;//匹配的地图点mvBearingVecs.reserve(F.mvpMapPoints.size());//bearing vector，即3D点在相机坐标系下的方向向量mvP2D.reserve(F.mvpMapPoints.size());//匹配的2D点mvSigma2.reserve(F.mvpMapPoints.size());//卡方检验中的sigma值，根据每个关键点所在层数不同而有所不同mvP3Dw.reserve(F.mvpMapPoints.size());//3D点在世界坐标系下的坐标mvKeyPointIndices.reserve(F.mvpMapPoints.size());//匹配的关键点的索引，不连续(因为有的索引并没有匹配)mvAllIndices.reserve(F.mvpMapPoints.size());//连续的索引，从0到匹配数-1int idx = 0;//遍历所有匹配的地图点，计算MLPnP相关的量for(size_t i = 0, iend = mvpMapPointMatches.size(); i < iend; i++){MapPoint* pMP = vpMapPointMatches[i];//取出地图点if(pMP){if(!pMP -> isBad()){if(i >= F.mvKeysUn.size()) continue;const cv::KeyPoint &kp = F.mvKeysUn[i];//地图点对应的2D点mvP2D.push_back(kp.pt);//保存2D点坐标mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);//根据所在层保存2D点的sigma值//Bearing vector should be normalizedcv::Point3f cv_br = mpCamera->unproject(kp.pt);//反投影，得到相机坐标系下3D点坐标(即bearing vector)cv_br /= cv_br.z;//单位化bearingVector_t br(cv_br.x,cv_br.y,cv_br.z);mvBearingVecs.push_back(br);//保存bearing vector//3D coordinatescv::Mat cv_pos = pMP -> GetWorldPos();//地图点的3D坐标point_t pos(cv_pos.at<float>(0),cv_pos.at<float>(1),cv_pos.at<float>(2));mvP3Dw.push_back(pos);//保存3D点世界坐标系下的坐标mvKeyPointIndices.push_back(i);//关键点索引，有的i是不会被保存的(当pMP坏的时候)mvAllIndices.push_back(idx);//从0开始连续的索引，idx++;}}}SetRansacParameters();//设置Ransac参数，包括最小集、迭代次数、最小内点数、每个点的最大误差等}

CheckInliers 改为反投影 在算误差

/// 重投影误差检测
void MLPnPsolver::CheckInliers(){mnInliersi=0;for(int i=0; i<N; i++){point_t p = mvP3Dw[i];cv::Point3f P3Dw(p(0),p(1),p(2));cv::Point2f P2D = mvP2D[i];float xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];float yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];float zc = mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2];cv::Point3f P3Dc(xc,yc,zc);cv::Point2f uv = mpCamera->project(P3Dc);float distX = P2D.x-uv.x;float distY = P2D.y-uv.y;float error2 = distX*distX+distY*distY;if(error2<mvMaxError[i]){mvbInliersi[i]=true;mnInliersi++;}else{mvbInliersi[i]=false;}}
}

MLPnP 求解

void MLPnPsolver::computePose(const bearingVectors_t &f, const points_t &p, const cov3_mats_t &covMats,const std::vector<int> &indices, transformation_t &result) {size_t numberCorrespondences = indices.size();assert(numberCorrespondences > 5);bool planar = false;// compute the nullspace of all vectorsstd::vector<Eigen::MatrixXd> nullspaces(numberCorrespondences);Eigen::MatrixXd points3(3, numberCorrespondences);points_t points3v(numberCorrespondences);points4_t points4v(numberCorrespondences);for (size_t i = 0; i < numberCorrespondences; i++) {bearingVector_t f_current = f[indices[i]];points3.col(i) = p[indices[i]];// nullspace of right vectorEigen::JacobiSVD<Eigen::MatrixXd, Eigen::HouseholderQRPreconditioner>svd_f(f_current.transpose(), Eigen::ComputeFullV);nullspaces[i] = svd_f.matrixV().block(0, 1, 3, 2);points3v[i] = p[indices[i]];}//// 1. test if we have a planar scene//Eigen::Matrix3d planarTest = points3 * points3.transpose();Eigen::FullPivHouseholderQR<Eigen::Matrix3d> rankTest(planarTest);Eigen::Matrix3d eigenRot;eigenRot.setIdentity();// if yes -> transform points to new eigen frame//rankTest.setThreshold(1e-10);if (rankTest.rank() == 2) {planar = true;// self adjoint is faster and more accurate than general eigen solvers// also has closed form solution for 3x3 self-adjoint matrices// in addition this solver sorts the eigenvalues in increasing orderEigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(planarTest);eigenRot = eigen_solver.eigenvectors().real();eigenRot.transposeInPlace();for (size_t i = 0; i < numberCorrespondences; i++)points3.col(i) = eigenRot * points3.col(i);}//// 2. stochastic model//Eigen::SparseMatrix<double> P(2 * numberCorrespondences,2 * numberCorrespondences);bool use_cov = false;P.setIdentity(); // standard// if we do have covariance information// -> fill covariance matrixif (covMats.size() == numberCorrespondences) {use_cov = true;int l = 0;for (size_t i = 0; i < numberCorrespondences; ++i) {// invert matrixcov2_mat_t temp = nullspaces[i].transpose() * covMats[i] * nullspaces[i];temp = temp.inverse().eval();P.coeffRef(l, l) = temp(0, 0);P.coeffRef(l, l + 1) = temp(0, 1);P.coeffRef(l + 1, l) = temp(1, 0);P.coeffRef(l + 1, l + 1) = temp(1, 1);l += 2;}}//// 3. fill the design matrix A//const int rowsA = 2 * numberCorrespondences;int colsA = 12;Eigen::MatrixXd A;if (planar) {colsA = 9;A = Eigen::MatrixXd(rowsA, 9);} elseA = Eigen::MatrixXd(rowsA, 12);A.setZero();// fill design matrixif (planar) {for (size_t i = 0; i < numberCorrespondences; ++i) {point_t pt3_current = points3.col(i);// r12A(2 * i, 0) = nullspaces[i](0, 0) * pt3_current[1];A(2 * i + 1, 0) = nullspaces[i](0, 1) * pt3_current[1];// r13A(2 * i, 1) = nullspaces[i](0, 0) * pt3_current[2];A(2 * i + 1, 1) = nullspaces[i](0, 1) * pt3_current[2];// r22A(2 * i, 2) = nullspaces[i](1, 0) * pt3_current[1];A(2 * i + 1, 2) = nullspaces[i](1, 1) * pt3_current[1];// r23A(2 * i, 3) = nullspaces[i](1, 0) * pt3_current[2];A(2 * i + 1, 3) = nullspaces[i](1, 1) * pt3_current[2];// r32A(2 * i, 4) = nullspaces[i](2, 0) * pt3_current[1];A(2 * i + 1, 4) = nullspaces[i](2, 1) * pt3_current[1];// r33A(2 * i, 5) = nullspaces[i](2, 0) * pt3_current[2];A(2 * i + 1, 5) = nullspaces[i](2, 1) * pt3_current[2];// t1A(2 * i, 6) = nullspaces[i](0, 0);A(2 * i + 1, 6) = nullspaces[i](0, 1);// t2A(2 * i, 7) = nullspaces[i](1, 0);A(2 * i + 1, 7) = nullspaces[i](1, 1);// t3A(2 * i, 8) = nullspaces[i](2, 0);A(2 * i + 1, 8) = nullspaces[i](2, 1);}} else {for (size_t i = 0; i < numberCorrespondences; ++i) {point_t pt3_current = points3.col(i);// r11A(2 * i, 0) = nullspaces[i](0, 0) * pt3_current[0];A(2 * i + 1, 0) = nullspaces[i](0, 1) * pt3_current[0];// r12A(2 * i, 1) = nullspaces[i](0, 0) * pt3_current[1];A(2 * i + 1, 1) = nullspaces[i](0, 1) * pt3_current[1];// r13A(2 * i, 2) = nullspaces[i](0, 0) * pt3_current[2];A(2 * i + 1, 2) = nullspaces[i](0, 1) * pt3_current[2];// r21A(2 * i, 3) = nullspaces[i](1, 0) * pt3_current[0];A(2 * i + 1, 3) = nullspaces[i](1, 1) * pt3_current[0];// r22A(2 * i, 4) = nullspaces[i](1, 0) * pt3_current[1];A(2 * i + 1, 4) = nullspaces[i](1, 1) * pt3_current[1];// r23A(2 * i, 5) = nullspaces[i](1, 0) * pt3_current[2];A(2 * i + 1, 5) = nullspaces[i](1, 1) * pt3_current[2];// r31A(2 * i, 6) = nullspaces[i](2, 0) * pt3_current[0];A(2 * i + 1, 6) = nullspaces[i](2, 1) * pt3_current[0];// r32A(2 * i, 7) = nullspaces[i](2, 0) * pt3_current[1];A(2 * i + 1, 7) = nullspaces[i](2, 1) * pt3_current[1];// r33A(2 * i, 8) = nullspaces[i](2, 0) * pt3_current[2];A(2 * i + 1, 8) = nullspaces[i](2, 1) * pt3_current[2];// t1A(2 * i, 9) = nullspaces[i](0, 0);A(2 * i + 1, 9) = nullspaces[i](0, 1);// t2A(2 * i, 10) = nullspaces[i](1, 0);A(2 * i + 1, 10) = nullspaces[i](1, 1);// t3A(2 * i, 11) = nullspaces[i](2, 0);A(2 * i + 1, 11) = nullspaces[i](2, 1);}}//// 4. solve least squares//Eigen::MatrixXd AtPA;if (use_cov)AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstableelseAtPA = A.transpose() * A;Eigen::JacobiSVD<Eigen::MatrixXd> svd_A(AtPA, Eigen::ComputeFullV);Eigen::MatrixXd result1 = svd_A.matrixV().col(colsA - 1);// now we treat the results differently,// depending on the scene (planar or not)//transformation_t T_final;rotation_t Rout;translation_t tout;if (planar) // planar case{rotation_t tmp;// until now, we only estimated// row one and two of the transposed rotation matrixtmp << 0.0, result1(0, 0), result1(1, 0),0.0, result1(2, 0), result1(3, 0),0.0, result1(4, 0), result1(5, 0);// row 3tmp.col(0) = tmp.col(1).cross(tmp.col(2));tmp.transposeInPlace();double scale = 1.0 / std::sqrt(std::abs(tmp.col(1).norm() * tmp.col(2).norm()));// find best rotation matrix in frobenius senseEigen::JacobiSVD<Eigen::MatrixXd> svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);rotation_t Rout1 = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose();// test if we found a good rotation matrixif (Rout1.determinant() < 0)Rout1 *= -1.0;// rotate this matrix back using the eigen frameRout1 = eigenRot.transpose() * Rout1;translation_t t = scale * translation_t(result1(6, 0), result1(7, 0), result1(8, 0));Rout1.transposeInPlace();Rout1 *= -1;if (Rout1.determinant() < 0.0)Rout1.col(2) *= -1;// now we have to find the best out of 4 combinationsrotation_t R1, R2;R1.col(0) = Rout1.col(0);R1.col(1) = Rout1.col(1);R1.col(2) = Rout1.col(2);R2.col(0) = -Rout1.col(0);R2.col(1) = -Rout1.col(1);R2.col(2) = Rout1.col(2);std::vector<transformation_t, Eigen::aligned_allocator<transformation_t>> Ts(4);Ts[0].block<3, 3>(0, 0) = R1;Ts[0].block<3, 1>(0, 3) = t;Ts[1].block<3, 3>(0, 0) = R1;Ts[1].block<3, 1>(0, 3) = -t;Ts[2].block<3, 3>(0, 0) = R2;Ts[2].block<3, 1>(0, 3) = t;Ts[3].block<3, 3>(0, 0) = R2;Ts[3].block<3, 1>(0, 3) = -t;std::vector<double> normVal(4);for (int i = 0; i < 4; ++i) {point_t reproPt;double norms = 0.0;for (int p = 0; p < 6; ++p) {reproPt = Ts[i].block<3, 3>(0, 0) * points3v[p] + Ts[i].block<3, 1>(0, 3);reproPt = reproPt / reproPt.norm();norms += (1.0 - reproPt.transpose() * f[indices[p]]);}normVal[i] = norms;}std::vector<double>::iteratorfindMinRepro = std::min_element(std::begin(normVal), std::end(normVal));int idx = std::distance(std::begin(normVal), findMinRepro);Rout = Ts[idx].block<3, 3>(0, 0);tout = Ts[idx].block<3, 1>(0, 3);} else // non-planar{rotation_t tmp;tmp << result1(0, 0), result1(3, 0), result1(6, 0),result1(1, 0), result1(4, 0), result1(7, 0),result1(2, 0), result1(5, 0), result1(8, 0);// get the scaledouble scale = 1.0 /std::pow(std::abs(tmp.col(0).norm() * tmp.col(1).norm() * tmp.col(2).norm()), 1.0 / 3.0);//double scale = 1.0 / std::sqrt(std::abs(tmp.col(0).norm() * tmp.col(1).norm()));// find best rotation matrix in frobenius senseEigen::JacobiSVD<Eigen::MatrixXd> svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);Rout = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose();// test if we found a good rotation matrixif (Rout.determinant() < 0)Rout *= -1.0;// scale translationtout = Rout * (scale * translation_t(result1(9, 0), result1(10, 0), result1(11, 0)));// find correct direction in terms of reprojection error, just take the first 6 correspondencesstd::vector<double> error(2);std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> Ts(2);for (int s = 0; s < 2; ++s) {error[s] = 0.0;Ts[s] = Eigen::Matrix4d::Identity();Ts[s].block<3, 3>(0, 0) = Rout;if (s == 0)Ts[s].block<3, 1>(0, 3) = tout;elseTs[s].block<3, 1>(0, 3) = -tout;Ts[s] = Ts[s].inverse().eval();for (int p = 0; p < 6; ++p) {bearingVector_t v = Ts[s].block<3, 3>(0, 0) * points3v[p] + Ts[s].block<3, 1>(0, 3);v = v / v.norm();error[s] += (1.0 - v.transpose() * f[indices[p]]);}}if (error[0] < error[1])tout = Ts[0].block<3, 1>(0, 3);elsetout = Ts[1].block<3, 1>(0, 3);Rout = Ts[0].block<3, 3>(0, 0);}//// 5. gauss newton//rodrigues_t omega = rot2rodrigues(Rout);Eigen::VectorXd minx(6);minx[0] = omega[0];minx[1] = omega[1];minx[2] = omega[2];minx[3] = tout[0];minx[4] = tout[1];minx[5] = tout[2];mlpnp_gn(minx, points3v, nullspaces, P, use_cov);Rout = rodrigues2rot(rodrigues_t(minx[0], minx[1], minx[2]));tout = translation_t(minx[3], minx[4], minx[5]);// result inverse as opengv uses this conventionresult.block<3, 3>(0, 0) = Rout;//Rout.transpose();result.block<3, 1>(0, 3) = tout;//-result.block<3, 3>(0, 0) * tout;
}

随机抽样一致性算法 Ransca

https://en.wikipedia.org/wiki/Random_sample_consensus

随机样本一致性 (RANSAC)

• 随机抽样一致性 (RANSAC) 是一种迭代方法，用于从一组观察数据中包含异常值的数学估计模型，异常值不会对估计值产生影响。因此，它也可以解释为一种异常值检测方法。 从某种意义上说，它是一种非确定性算法，它仅以一定的概率产生合理的结果，随着允许更多的迭代，这种概率会增加。
• RANSAC也做了以下假设：给定一组（通常很小的）局内点，存在一个可以估计模型参数的过程；而该模型能够解释或者适用于局内点。

RANSAC的基本假设是：

• 数据由“局内点”组成，即其分布可以通过某些模型参数集解释的数据
• “局外点”是不能适应该模型的数据，例如：噪声的极值；错误的测量方法；对数据的错误假设
• 除此之外的数据属于噪声。

示例

• 一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

• 

概述

• RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。
• RANSAC 算法本质上由迭代重复的两个步骤组成：
  • 从数据集中随机选择包含最小数据项的样本子集，仅使用该样本子集的元素计算拟合模型和相应的模型参数，样本子集的基数最小，足以确定模型参数。
  • 算法检测整个数据集中那些元素与第一步获得的估计模型参数实例化模型一致，如果数据元素在某一误差范围之外则该元素被视为“局外点”，该误差阈值定义了可归因于噪声影响的最大偏差。
• RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
  • 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出
  • 用上述中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点
  • 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理
  • 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过
  • 最后，通过估计局内点与模型的错误率来评估模型
• 这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用

算法

• 输入：

  • data – 一组观测数据
  • model – 适应于数据的模型
  • n – 适用于模型的最小数据个数
  • k – 算法的迭代次数
  • t – 用于决定数据是否适应于模型的阈值
  • d – 判定模型是否适用于数据集的数据数据

• 输出：

  • best_model – 跟数据最匹配的模型参数（如果没有找到好的模型，返回null）
  • best_consensus_set —— 估计出模型的数据点
  • best_error —— 跟数据相关的估计出的模型错误

• 伪代码：

  • iterations = 0
    best_model = null
    best_consensus_set = null
    best_error = 无穷大

  • while ( iterations < k )

    • maybe_inliers = 从数据集中随机选择n个点
      maybe_model = 适合于maybe_inliers的模型参数
      consensus_set = maybe_inliers

    • for ( 每个数据集中不属于maybe_inliers的点 ）
      if ( 如果点适合于maybe_model，且错误小于t ）
      将点添加到consensus_set

    • if （ consensus_set中的元素数目大于d ）
      已经找到了好的模型，现在测试该模型到底有多好
      better_model = 适合于consensus_set中所有点的模型参数
      this_error = better_model究竟如何适合这些点的度量

      ​ if ( this_error < best_error )

      ​ 我们发现了比以前好的模型，保存该模型直到更好的模型出现
      ​ best_model = better_model
      ​ best_consensus_set = consensus_set
      ​ best_error = this_error

    • 增加迭代次数

  • 返回 best_model, best_consensus_set, best_error

• RANSAC算法的可能变化包括以下几种：

  • 如果发现了一种足够好的模型（该模型有足够小的错误率），则跳出主循环。这样可能会节约计算额外参数的时间
  • 直接从maybe_model计算this_error，而不从consensus_set重新估计模型。这样可能会节约比较两种模型错误的时间，但可能会对噪声更敏感。

参数

• 我们不得不根据特定的问题和数据集通过实验来确定参数t和d。然而参数 $k$（迭代次数）可以从理论结果推断。当我们从估计模型参数时，用 $p$ 表示一些迭代过程中从数据集内随机选取出的点均为局内点的概率；此时，结果模型很可能有用，因此 $p$ 也表征了算法产生有用结果的概率。用w表示每次从数据集中选取一个局内点的概率，如下式所示：$w=\frac{局内点数量}{数据集数目}$

• 通常情况下，我们事先并不知道 $w$ 的值，但是可以给出一些鲁棒的值。假设估计模型需要选定 $n$ 个点，$w^n$ 是所有 $n$ 个点均为局内点的概率；$1-w^n$ 是 $n$ 个点中至少有一个点为局外点的概率，此时表明我们从数据集中估计出了一个不好的模型。 $(1-w^n{)}^k$ 表示算法永远都不会选择到 $n$ 个点均为局内点的概率，它和 $1-p$ 相同。因此：$1-p=(1-w^n{)}^k$

• 我们对上式的两边取对数，得出：$k=\frac{1-p}{1-w^n}$

• 值得注意的是，这个结果假设 $n$ 个点都是独立选择的；也就是说，某个点被选定之后，它可能会被后续的迭代过程重复选定到。这种方法通常都不合理，由此推导出的 $k$ 值被看作是选取不重复点的上限。

  例如，要从上图中的数据集寻找适合的直线，RANSAC算法通常在每次迭代时选取2个点，计算通过这两点的直线maybe_model，要求这两点必须唯一。

• 为了得到更可信的参数，标准偏差或它的乘积可以被加到 $k$ 上。$k$ 的标准偏差定义为：$SD(K)=\frac{\sqrt{(1-w^n)}}{w^n}$

缺点与优点：

• RANSAC的优点是它能鲁棒的估计模型参数。例如，它能从包含大量局外点的数据集中估计出高精度的参数。RANSAC的缺点是它计算参数的迭代次数没有上限；如果设置迭代次数的上限，得到的结果可能不是最优的结果，甚至可能得到错误的结果。RANSAC只有一定的概率得到可信的模型，概率与迭代次数成正比。RANSAC的另一个缺点是它要求设置跟问题相关的阀值。
• RANSAC只能从特定的数据集中估计出一个模型，如果存在两个（或多个）模型，RANSAC不能找到别的模型。