(01)ORB-SLAM2源码无死角解析-(42) EPnP 源代码分析(2)→compute_pose():控制点选取,系数计算

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

一、前言

从上一篇博客中，以及了解了类 PnPsolver 总体流程与思路，并且分析其核心函数 PnPsolver::iterate()，再进行详细分析之前，把前面博客 (01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束 推导出来的公式粘贴一下(标号没有改变)
$${\mathbf{c}}_1^w=\frac{1}{n}\sum _{i=1}^n{\mathbf{p}}_i^w\text{\hspace{1em}(01)}$$$$\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)}$$
$$\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)}$$$$\mathbf{Mx}=0\text{\hspace{1em}(14)}$$其符号字母的含义这里也不再重述了，不太明白的朋友直接通过上面给出的博客链接可以查看具体推导过程。
 

二、代码流程

经过前面的分析，知道 PnPsolver::iterate() 的核心代码集中在 PnPsolver::compute_pose() 中，从函数名也可明显得知，主要功能为求解相机位姿。步骤如下(以及代码注释)：

       $(1):$ 获得EPnP算法中的四个控制点→choose_control_points()
       $(2):$ 计算世界坐标系下每个3D点用4个控制点线性表达时的系数alphas→compute_barycentric_coordinates();
       $(3):$ 构造M矩阵，EPnP原始论文中公式(3)(4)–>(5)(6)(7); 矩阵的大小为 2n*12 ,n 为使用的匹配点的对数。
       $(4):$ 求解$\mathbf{Mx}=0$, ①对${\mathbf{M}}^T\mathbf{M}$进行特征值分解； ②分情况对$\mathbf{L}\mathbf{\beta }=\mathbf{\rho }$进行讨论
       $(5):$ 选择重投影误差最小，也就是效果最好$\mathbf{R},\mathbf{t}$。

/**
 * @brief 使用EPnP算法计算相机的位姿.其中匹配点的信息由类的成员函数给定 
 * @param[out] R    求解位姿里的旋转矩阵
 * @param[out] T    求解位姿里的平移向量
 * @return double   使用这对旋转和平移的时候, 匹配点对的平均重投影误差
 */
double PnPsolver::compute_pose(double R[3][3], double t[3])
{
  // Step 1：获得EPnP算法中的四个控制点
  choose_control_points();

  // Step 2：计算世界坐标系下每个3D点用4个控制点线性表达时的系数alphas
  compute_barycentric_coordinates();

  // Step 3：构造M矩阵，EPnP原始论文中公式(3)(4)-->(5)(6)(7); 矩阵的大小为 2n*12 ,n 为使用的匹配点的对数
  CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);

  // 根据每一对匹配点的数据来填充矩阵M中的数据
  // alphas:  世界坐标系下3D点用4个虚拟控制点表达时的系数
  // us:      图像坐标系下的2D点坐标
  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);    // 这里实际是特征向量

  // Step 4：求解Mx = 0

  // Step 4.1 先计算其中的特征向量vi
  // 求M'M
  cvMulTransposed(M, &MtM, 1);
  // 该函数实际是特征值分解，得到特征值D，特征向量ut，对应EPnP论文式(8)中的vi
  cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); 
  cvReleaseMat(&M);

  // Step 4.2 计算分情况讨论的时候需要用到的矩阵L和\rho
  // EPnP论文中式13中的L和\rho
  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);

  // 计算这两个量,6x10是先准备按照EPnP论文中的N=4来计算的
  compute_L_6x10(ut, l_6x10);
  compute_rho(rho);


  // Step 4.3 分情况计算N=2,3,4时能够求解得到的相机位姿R,t并且得到平均重投影误差
  double Betas[4][4],         // 本质上就四个beta1~4,但是这里有四种情况(第一维度表示)
         rep_errors[4];       // 重投影误差
  double Rs[4][3][3],         //每一种情况迭代优化后得到的旋转矩阵
         ts[4][3];            //每一种情况迭代优化后得到的平移向量

  // 不管什么情况，都假设论文中N=4，并求解部分betas（如果全求解出来会有冲突）
  // 通过优化得到剩下的 betas
  // 最后计算R t


  // 求解近似解：N=4的情况
  find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
  // 高斯牛顿法迭代优化得到 beta
  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]);

  // Step 5 看看哪种情况得到的效果最好,然后就选哪个
  int N = 1;    // trick , 这样可以减少一种情况的计算
  if (rep_errors[2] < rep_errors[1]) N = 2;
  if (rep_errors[3] < rep_errors[N]) N = 3;

  // Step 6 将最佳计算结果保存到返回计算结果用的变量中
  copy_R_and_t(Rs[N], ts[N], R, t);

  // Step 7 并且返回平均匹配点对的重投影误差,作为对相机位姿估计的评价
  return rep_errors[N];
}

接下来，就是对其细节进行分析了。

 

三、选择控制点→choose_control_points

/**
 * @brief 从给定的匹配点中计算出四个控制点
 * 
 */
void PnPsolver::choose_control_points(void)
{
  // Take C0 as the reference points centroid:
  // Step 1：第一个控制点：参与PnP计算的参考3D点的质心（均值）
  // cws[4][3] 存储控制点在世界坐标系下的坐标，第一维表示是哪个控制点，第二维表示是哪个坐标(x,y,z)
  // 计算前先把第1个控制点坐标清零
  cws[0][0] = cws[0][1] = cws[0][2] = 0;

  // 遍历每个匹配点中世界坐标系3D点，然后对每个坐标轴加和
  // number_of_correspondences 默认是 4
  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:
  // Step 2：计算其它三个控制点，C1, C2, C3通过特征值分解得到
  // ref: https://www.zhihu.com/question/38417101
  // ref: https://yjk94.wordpress.com/2016/11/11/pca-to-layman/

  // 将所有的3D参考点写成矩阵，(number_of_correspondences * 3)的矩阵
  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);     // PW0^T * PW0，为了进行特征值分解
  CvMat DC      = cvMat(3, 1, CV_64F, dc);          // 特征值
  CvMat UCt     = cvMat(3, 3, CV_64F, uct);         // 特征向量

  // Step 2.1：将存在pws中的参考3D点减去第一个控制点(均值中心)的坐标（相当于把第一个控制点作为原点）, 并存入PW0
  for(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];

  // Step 2.2：利用特征值分解得到三个主方向
  // PW0^T * PW0
  // cvMulTransposed(A_src,Res_dst,order, delta=null,scale=1): 
  // Calculates Res=(A-delta)*(A-delta)^T (order=0) or (A-delta)^T*(A-delta) (order=1)
  cvMulTransposed(PW0, &PW0tPW0, 1);

  // 这里实际是特征值分解
  cvSVD(&PW0tPW0,                         // A
        &DC,                              // W，实际是特征值
        &UCt,                             // U，实际是特征向量
        0,                                // V
        CV_SVD_MODIFY_A | CV_SVD_U_T);    // flags

  cvReleaseMat(&PW0);

  // Step 2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量
  // ?这里的循环次数不应写成4,而应该是变量 number_of_correspondences
  for(int i = 1; i < 4; i++) {
    // 这里只需要遍历后面3个控制点
    double k = sqrt(dc[i - 1] / number_of_correspondences);
    for(int j = 0; j < 3; j++)
      cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
  }
}

 

四、系数计算→compute_barycentric_coordinates

/**
 * @brief 求解世界坐标系下四个控制点的系数alphas，在相机坐标系下系数不变
 * 
 */
void PnPsolver::compute_barycentric_coordinates(void)
{
  // pws为世界坐标系下3D参考点的坐标
  // cws1 cws2 cws3 cws4为世界坐标系下四个控制点的坐标
  // alphas 四个控制点的系数，每一个pws，都有一组alphas与之对应
  double cc[3 * 3], cc_inv[3 * 3];
  CvMat CC     = cvMat(3, 3, CV_64F, cc);       // 除第1个控制点外，另外3个控制点在控制点坐标系下的坐标
  CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);   // 上面这个矩阵的逆矩阵

  // Step 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|

  // 将后面3个控制点cws 去重心后 转化为 cc
  for(int i = 0; i < 3; i++)                      // x y z 轴
    for(int j = 1; j < 4; j++)                    // 哪个控制点
      cc[3 * i + j - 1] = cws[j][i] - cws[0][i];  // 坐标索引中的-1是考虑到跳过了第1个控制点0

  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][]表示去质心
    // a0,a1,a2,a3 对应的是四个控制点的齐次重心坐标
    for(int j = 0; j < 3; j++)
      // +1 是因为跳过了a0
      /*    这里的原理基本上是这个样子：(这里公式的下标和程序中的不一样，是从1开始的)
       *    cp=p_i-c1
       *    cp=a1(c1-c1)+a2(c2-c1)+a3(c3-c1)+a4(c4-c1)
       *      => a2*cc2+a3*cc3+a4*cc4
       *    [cc2 cc3 cc4] * [a2 a3 a4]^T = cp
       *  => [a2 a3 a4]^T = [cc2 cc3 cc4]^(-1) * cp
       */      
      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]);
    // 最后计算用于进行归一化的a0
    a[0] = 1.0f - a[1] - a[2] - a[3];
  } // 遍历每一个匹配点
}

 

五、求解相机坐标系下的控制点

计算出世界坐标系下的控制点以及对应的系数之后 ，那么接下来就可以构建矩阵方程 $\mathbf{Mx}=0$ ，然后求解出相机坐标系下的控制点，对应代码如下:

 // Step 3：构造M矩阵，EPnP原始论文中公式(3)(4)-->(5)(6)(7); 矩阵的大小为 2n*12 ,n 为使用的匹配点的对数
  CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);

  // 根据每一对匹配点的数据来填充矩阵M中的数据
  // alphas:  世界坐标系下3D点用4个虚拟控制点表达时的系数
  // us:      图像坐标系下的2D点坐标
  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);    // 这里实际是特征向量

  // Step 4：求解Mx = 0

  // Step 4.1 先计算其中的特征向量vi
  // 求M'M
  cvMulTransposed(M, &MtM, 1);
  // 该函数实际是特征值分解，得到特征值D，特征向量ut，对应EPnP论文式(8)中的vi
  cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); 
  cvReleaseMat(&M);

对 $\mathbf{M}$ 进行 SVD 奇异值分解，最小特征值对应的特征向量就是矩阵方程的最优解。其解就是相机坐标系下的控制点，对应于源码中的 $Ut$。另外最核心的函数 compute_L_6x10 对应：

/**
 * @brief 计算矩阵L,论文式13中的L矩阵,不过这里的是按照N=4的时候计算的
 * 
 * @param[in]  ut               特征值分解之后得到的12x12特征矩阵
 * @param[out] l_6x10           计算的L矩阵结果，维度6x10 
 */
void PnPsolver::compute_L_6x10(const double * ut, double * l_6x10)
{
  // Step 1 获取最后4个零特征值对应的4个12x1的特征向量
  const double * v[4];

  // 对应EPnP里N=4的情况。直接取特征向量的最后4行
  // 以这里的v[0]为例，它是12x1的向量，会拆成4个3x1的向量v[0]^[0]，v[0]^[1]，v[0]^[1]，v[0]^[3]，对应4个相机坐标系控制点
  v[0] = ut + 12 * 11;    // v[0] : v[0][0]~v[0][2]  => v[0]^[0]  , * \beta_0 = c0  (理论上)
                          //        v[0][3]~v[0][5]  => v[0]^[1]  , * \beta_0 = c1 
                          //        v[0][6]~v[0][8]  => v[0]^[2]  , * \beta_0 = c2
                          //        v[0][9]~v[0][11] => v[0]^[3]  , * \beta_0 = c3
  v[1] = ut + 12 * 10;
  v[2] = ut + 12 *  9;
  v[3] = ut + 12 *  8;

  // Step 2 提前计算中间变量dv 
  // dv表示中间变量，是difference-vector的缩写
  // 4 表示N=4时对应的4个12x1的向量v, 6 表示4对点一共有6种两两组合的方式，3 表示v^[i]是一个3维的列向量
  double dv[4][6][3];


  // N=4时候的情况. 控制第一个下标的就是a,第二个下标的就是b,不过下面的循环中下标都是从0开始的
  for(int i = 0; i < 4; i++) {
    // 每一个向量v[i]可以提供四个控制点的"雏形"v[i]^[0]~v[i]^[3]
    // 这四个"雏形"两两组合一共有六种组合方式: 
    // 下面的a变量就是前面的那个id,b就是后面的那个id
    int a = 0, b = 1;
    for(int j = 0; j < 6; j++) {
      // dv[i][j]=v[i]^[a]-v[i]^[b]
      // a,b的取值有6种组合 0-1 0-2 0-3 1-2 1-3 2-3
      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;
      }
    }
  }

  // Step 3 用前面计算的dv生成L矩阵
  // 这里的6代表前面每个12x1维向量v的4个3x1子向量v^[i]对应的6种组合
  for(int i = 0; i < 6; i++) {
    double * row = l_6x10 + 10 * i;
    // 计算每一行中的每一个元素,总共是10个元素      // 对应的\beta列向量
    row[0] =        dot(dv[0][i], dv[0][i]);  //*b11
    row[1] = 2.0f * dot(dv[0][i], dv[1][i]);  //*b12
    row[2] =        dot(dv[1][i], dv[1][i]);  //*b22
    row[3] = 2.0f * dot(dv[0][i], dv[2][i]);  //*b13
    row[4] = 2.0f * dot(dv[1][i], dv[2][i]);  //*b23
    row[5] =        dot(dv[2][i], dv[2][i]);  //*b33
    row[6] = 2.0f * dot(dv[0][i], dv[3][i]);  //*b14
    row[7] = 2.0f * dot(dv[1][i], dv[3][i]);  //*b24
    row[8] = 2.0f * dot(dv[2][i], dv[3][i]);  //*b34
    row[9] =        dot(dv[3][i], dv[3][i]);  //*b44
  }
}

 

六、结语

通过该篇博客，对 compute_pose() 源码进行了细致的分析，且与前面的理论推导对应了起来。当然，这里还没有讲解完成，下一篇博客会对剩余部分进行分析，也就是分情况对 $\mathbf{L}\mathbf{\beta }=\mathbf{\rho }$ 进行讨论。
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件