(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   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

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

输入 : \color{Green}{输入}: 输入: 向量 x = ( x 1 , x 2 , ⋯   , x n ) T ≠ 0 \mathbf{x}=\left(\mathrm{x}_{1}, \mathrm{x}_{2}, \cdots, \mathrm{x}_{\mathrm{n}}\right)^{\mathrm{T}} \neq \mathbf 0 x=(x1​,x2​,⋯,xn​)T=0 输出 : \color{Green}{输出}: 输出: 豪斯霍尔德Householder 矩阵 H \mathbf H H，以及 σ \sigma σ，满足 H x = − σ ⋅ e 1 \mathrm{Hx}=-\sigma \cdot {\mathbf {e}}_{1} Hx=−σ⋅e1​ 算法 : \color{Green}{算法}: 算法: ①  η = max ⁡ ( x 1 , x 2 , ⋯   , x n ) ②  x = ( x 1 η , x 2 η , ⋯   , x i η ) = ( x 1 , x 2 , ⋯   , x n ) ③  σ = sign ⁡ ( x 1 ) ④  v = ( x 1 + σ , x 2 , ⋯   , x n ) ⑤  ρ = ∣ ∣ v ∣ ∣ 2 2 / 2 ⑥  σ ← η σ (23) \color{Green} \tag{23} \begin{array}{l} ①~\eta=\max\left(\mathrm{x}_{1}, \mathrm{x}_{2}, \cdots, \mathrm{x}_{\mathrm{n}}\right) \\ ②~\mathbf{x}=(\frac{\mathrm{x}_{\mathrm{1}}}{\eta},\frac{\mathrm{x}_{\mathrm{2}}}{\eta},\cdots,\frac{\mathrm{x}_{\mathrm{i}}}{\eta})=\left(\mathrm{x}_{1}, \mathrm{x}_{2}, \cdots, \mathrm{x}_{\mathrm{n}}\right) \\ ③~\sigma=\operatorname{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} ① η=max(x1​,x2​,⋯,xn​)② x=(ηx1​​,ηx2​​,⋯,ηxi​​)=(x1​,x2​,⋯,xn​)③ σ=sign(x1​)④ v=(x1​+σ,x2​,⋯,xn​)⑤ ρ=∣∣v∣∣22​/2⑥ σ←ησ​(23)根据 ρ \rho ρ 值和向量 v \mathbf{v} v 可以计算出豪氏矩阵 H = E − 2 ⋅ v v T ∥ v ∥ 2 2 = E − ρ − 1 v v T \mathbf{H}=\mathbf{E}-2 \cdot \frac{\mathbf{vv}^{T}}{\|\mathbf{v}\|_{2}^{2}}=\mathbf{E}-\rho^{-1} \mathbf{vv}^{T} H=E−2⋅∥v∥22​vvT​=E−ρ−1vvT，另外算法输出的 σ \sigma σ 是乘上了规范化因子的值，需要注意的是，上述过程需要循环执行，因为其仅仅是对矩阵一列进行了豪斯霍尔德Householder 变换而已。变换公式如下： H = E − 2 ⋅ v v T ∥ v ∥ 2 2 = E − ρ − 1 v v T (15) \color{Green} \tag{15} \mathbf{H}=\mathbf{E}-2 \cdot \frac{\mathbf{vv}^{T}}{\|\mathbf{v}\|_{2}^{2}}=\mathbf{E}-\rho^{-1} \mathbf{vv}^{T} H=E−2⋅∥v∥22​vvT​=E−ρ−1vvT(15)其中 ρ = ∥ v ∥ 2 2 2 = 1 2 [ ( x 1 + σ ) 2 + x 2 2 + ⋯ + x n 2 ] = 1 2 ( ∑ i = 1 n x i 2 + σ 2 + 2 σ ⋅ x 1 ) = σ ( σ + x 1 ) (18) \color{Green} \tag{18} \rho=\frac{\|\mathbf{v}\|_{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) ρ=2∥v∥22​​=21​[(x1​+σ)2+x22​+⋯+xn2​]=21​(i=1∑n​xi2​+σ2+2σ⋅x1​)=σ(σ+x1​)(18) 另外 A Δ β = Δ B \mathbf{A} \Delta \boldsymbol{\beta}= \Delta \mathbf{B} AΔβ=ΔB使用QR分解之后可以表示为(注意 Q 为正交矩阵，即 Q − 1 = Q T \mathbf Q 为正交矩阵，即 \mathbf Q^{-1}=\mathbf Q^{T} Q为正交矩阵，即Q−1=QT) Q R Δ β = Δ B Δ β = ( Q R ) − 1 Δ B (02) \color{Green} \tag{02} \mathbf{QR} \Delta \boldsymbol{\beta}= \Delta \mathbf{B} \\\Delta \boldsymbol{\beta}= \mathbf{(QR)}^{-1} \Delta \mathbf{B} QRΔβ=ΔBΔβ=(QR)−1ΔB(02)

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

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

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

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

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

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

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

( 7 ) ： \color{blue}(7)： (7)： 进行真正的QR分，主要分两部执行 Δ β = ( Q R ) − 1 Δ B = R − 1 ( Q − 1 Δ B ) \Delta \boldsymbol{\beta}= \mathbf{(QR)}^{-1} \Delta \mathbf{B} = \mathbf R^{-1}(Q^{-1} \Delta \mathbf{B}) Δβ=(QR)−1ΔB=R−1(Q−1ΔB)，即先计算 ( Q − 1 Δ B ) (Q^{-1} \Delta \mathbf{B}) (Q−1ΔB) 再计算 R − 1 ( Q − 1 Δ B ) \mathbf R^{-1}(Q^{-1} \Delta \mathbf{B}) R−1(Q−1ΔB)。因为 Q \mathbf{Q} Q 为正交矩阵，所以存在 Q − 1 = Q T \mathbf{Q}^{-1}=\mathbf{Q}^T Q−1=QT。源码中计算完之后是直接存储于变量 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