(01)ORB-SLAM2源码无死角解析-(44) EPnP 源代码分析(4)→PnPsolver::qr_solve():QR分解

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

一、前言

通过上一篇博客建立了高斯牛顿增量方程 $$\mathbf{A}\Delta \mathbf{\beta }=\Delta \mathbf{B}\text{\hspace{1em}(01)}$$那么接下来就是对该方程的求解，源码中使用的方法是 QR分解(豪斯霍尔德Householder变换)，具体的推导过程可以参考博客: (01)ORB-SLAM2源码无死角解析-(40) EPnP 算法原理详解→理论基础四:QR分解(豪斯霍尔德Householder变换)，其中的(23)式如下所示：

$输入:$ 向量 $\mathbf{x}={\left({\mathrm{x}}_1,{\mathrm{x}}_2,\cdots ,{\mathrm{x}}_{\mathrm{n}}\right)}^{\mathrm{T}}\ne 0$
$输出:$ 豪斯霍尔德Householder 矩阵 $\mathbf{H}$，以及 $\sigma$，满足 $\mathrm{Hx}=-\sigma \cdot {\mathbf{e}}_1$
$算法:$
$$\begin{array}{l}①\eta =\max \left({\mathrm{x}}_1,{\mathrm{x}}_2,\cdots ,{\mathrm{x}}_{\mathrm{n}}\right)\\ ②\mathbf{x}=(\frac{{\mathrm{x}}_1}{\eta },\frac{{\mathrm{x}}_2}{\eta },\cdots ,\frac{{\mathrm{x}}_{\mathrm{i}}}{\eta })=\left({\mathrm{x}}_1,{\mathrm{x}}_2,\cdots ,{\mathrm{x}}_{\mathrm{n}}\right)\\ ③\sigma =\mathrm{sign}\left({\mathrm{x}}_1\right)\\ ④\mathbf{v}=\left({\mathrm{x}}_1+\sigma ,{\mathrm{x}}_2,\cdots ,{\mathrm{x}}_{\mathrm{n}}\right)\\ ⑤\rho =||\mathbf{v}|{|}_2^2/2\\ ⑥\sigma \leftarrow \eta \sigma \end{array}\text{\hspace{1em}(23)}$$根据 $\rho$ 值和向量 $\mathbf{v}$ 可以计算出豪氏矩阵 $\mathbf{H}=\mathbf{E}-2\cdot \frac{{\mathbf{vv}}^T}{\Vert \mathbf{v}{\Vert }_2^2}=\mathbf{E}-{\rho }^{-1}{\mathbf{vv}}^T$，另外算法输出的 $\sigma$ 是乘上了规范化因子的值，需要注意的是，上述过程需要循环执行，因为其仅仅是对矩阵一列进行了豪斯霍尔德Householder 变换而已。变换公式如下：
$$\mathbf{H}=\mathbf{E}-2\cdot \frac{{\mathbf{vv}}^T}{\Vert \mathbf{v}{\Vert }_2^2}=\mathbf{E}-{\rho }^{-1}{\mathbf{vv}}^T\text{\hspace{1em}(15)}$$其中
$$\rho =\frac{\Vert \mathbf{v}{\Vert }_2^2}{2}=\frac{1}{2}\left[{\left({\mathrm{x}}_1+\sigma \right)}^2+{\mathrm{x}}_2^2+\cdots +{\mathrm{x}}_{\mathrm{n}}^2\right]=\frac{1}{2}\left(\sum _{\mathrm{i}=1}^{\mathrm{n}}{\mathrm{x}}_{\mathrm{i}}^2+{\sigma }^2+2\sigma \cdot {\mathrm{x}}_1\right)=\sigma \left(\sigma +{\mathrm{x}}_1\right)\text{\hspace{1em}(18)}$$
另外$\mathbf{A}\Delta \mathbf{\beta }=\Delta \mathbf{B}$使用QR分解之后可以表示为(注意$\mathbf{Q}为正交矩阵，即{\mathbf{Q}}^{-1}={\mathbf{Q}}^T$)
$$\begin{gathered} \mathbf{QR}\Delta \mathbf{\beta }=\Delta \mathbf{B} \\ \Delta \mathbf{\beta }={(\mathbf{QR})}^{-1}\Delta \mathbf{B}\text{\hspace{1em}(02)} \end{gathered}$$

为了方便大家理解这里先为大家讲解一些代码，且变量与上面的公式对应起来：

$(1)：$ 首先展开一个 nc 次的循环，nc 为需要分解矩阵的 $\mathbf{A}$ 的列数。首先注意一个变量，那就是 ppAkk，其每次循环都是指向目前所在 k 列的对角线元素地址。

$(2)：$ 对当前遍历的第k列，对角线以下的所有元素进行统计，找到最大的元素，保存在 elt 变量中，对应于上面公式的 $\eta$。

$(3)：$ 以当前列对角线的元素地址 ppAik 为起点进行遍历，遍历对角线以下的所有元素。先利用 elt 变量，对元素进行归一化.即完成公式的第二步②。

$(4)：$ 代码中的 sigma 为公式③的 $\sigma$, 根据其执行第四步，对应源码为 *ppAkk += sigma，就是对角线上的元素 +$\sigma$ 操作。

$(5)：$ 再次对前列对角线以下的所有元素，再进行一次遍历，注意，这次的遍历已经执行了 *ppAkk += sigma，也就是说目前遍历的列可以理解为对公式中的 $\mathbf{v}$进行遍历，遍历求得所有元素的平方和存储于变量sum。

$(6)：$ 其上的所有操作，都是为了获得两个变量，即代码中的
A1→存储每列向量(对角线元素以下)进行豪斯霍尔德Householder变换需要的$\rho =\sigma \left(\sigma +{\mathrm{x}}_1\right)$；
A2→公式(23)中的⑥，即 $\sigma =\eta \sigma$

$(7)：$ 进行真正的QR分，主要分两部执行 $\Delta \mathbf{\beta }={(\mathbf{QR})}^{-1}\Delta \mathbf{B}={\mathbf{R}}^{-1}(Q^{-1}\Delta \mathbf{B})$，即先计算 $(Q^{-1}\Delta \mathbf{B})$ 再计算 ${\mathbf{R}}^{-1}(Q^{-1}\Delta \mathbf{B})$。因为 $\mathbf{Q}$ 为正交矩阵，所以存在 ${\mathbf{Q}}^{-1}={\mathbf{Q}}^T$。源码中计算完之后是直接存储于变量 pb 之中的，也就是传入的参数 CvMat * b。
 

二、代码注释

代码位于 src/PnPsolver.cc 文件中:

/**
 * @brief 使用QR分解来求解增量方程 
 * @param[in]  A   系数矩阵
 * @param[in]  b   非齐次项
 * @param[out] X   增量
 */
void PnPsolver::qr_solve(CvMat * A, CvMat * b, CvMat * X)
{
  static int max_nr = 0;        
  static double * A1, * A2;     


  const int nr = A->rows;       // 系数矩阵A的行数
  const int nc = A->cols;       // 系数矩阵A的列数

  // 判断是否需要重新分配A1 A2的内存区域
  if (max_nr != 0 && max_nr < nr) 
  {
    // 如果 max_nr != 0 说明之前已经创建了一个 last_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,     // 指向系数矩阵A的数据区
         * ppAkk = pA;          // 一直都会指向对角线上的元素
  // 对系数矩阵的列展开遍历
  for(int k = 0; k < nc; k++) 
  {
    double * ppAik = ppAkk,           // 只是辅助下面的for循环中,遍历对角线元素下的当前列的所有元素
             eta = fabs(*ppAik);      // 存储当前列对角线元素下面的所有元素绝对值的最大值

    // 遍历当前对角线约束下,当前列的所有元素,并且找到它们中的最大的绝对值
    for(int i = k + 1; i < nr; i++) 
    {
      double elt = fabs(*ppAik);
      if (eta < elt) eta = elt;
        ppAik += nc;                  // 指向下一列
    }

    //? 判断靠谱不? 由于系数矩阵是雅克比,并且代价函数中的L元素都是二次项的形式,所以原则上都应该是大于0的
    if (eta == 0) 
    {
      A1[k] = A2[k] = 0.0;
      cerr << "God damnit, A is singular, this shouldn't happen." << endl;
      return;
    } 
    else
    {

      // 开始正儿八经地进行QR分解了
      // 感觉这里面使用的ription provided.是数值分析中的计算方法,和矩阵论中的定义的算法还是不一样的
      // 注意在这里面,ppAik被重ription provided.定义了,在这个结构中以这里定义的这个为准
      double * ppAik = ppAkk, 
              sum = 0.0,
              inv_eta = 1. / eta; // 卧槽还能直接+.表示浮点数啊,长见识了
      // 对当前列下面的每一行的元素展开遍历（包含位于矩阵主对角线上的元素）
      for(int i = k; i < nr; i++) 
      {
        *ppAik *= inv_eta;          // NOTICE 注意这个操作是永久的，当前指向的元素都会被“归一化”
        sum += *ppAik * *ppAik;     // 平方和
        ppAik += nc;                // 指针移动到下一行的这个元素
      }

      // 计算 sigma ,同时根据对角线元素的符号保持其为正数
      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++) 
      {
        // 首先这一遍循环是为了计算tau
        // 又重新定义了
        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 b
  double * ppAjj = pA, * pb = b->data.db;
  // 对于每一列展开计算
  for(int j = 0; j < nc; j++) 
  {
    // 这个部分倒的确是在计算Q^T*b
    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 b
  // backward method
  double * 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];    //pX[j] 就是上一步中刚刚计算出来的那个
      ppAij++;
    }
    pX[i] = (pb[i] - sum) / A2[i];  // 比较像了
  }
}

 

三、结语

计算 $\Delta \mathbf{\beta }={(\mathbf{QR})}^{-1}\Delta \mathbf{B}$ 的时候要考虑到 $\mathbf{QR}$ 的特殊性，$\mathbf{Q}$ 为正交矩阵，$\mathbf{R}$ 为上三角矩阵。 通过 PnPsolver::qr_solve() 函数，就能够获得高斯牛顿迭代需要的增量。进而对 $\mathbf{\beta }$ 进行优化。求得 $\mathbf{\beta }$ 之后，那么进一步就可以求得相机在世界坐标系下的姿态。在接下来的博客中会进行详细的讲解