(01)ORB-SLAM2源码无死角解析-(43) EPnP 源代码分析(3)→find_betas_approx(),gauss_newton()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life 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→官方认证  

一、前言

上一篇博客对 src/PnPsolver.cc 文件中的 compute_pose() 函数分析，并且对其中调用的 choose_control_points()，compute_barycentric_coordinates() 进行了讲解，知道了代码中是如何求得 四个控制点，以及它们对应的系数，并且进一步求得了 相机坐标系下的控制点。有了这些信息，根据前面博客 (01)ORB-SLAM2源码无死角解析-(38) EPnP 算法原理详解→理论基础二:分情况求得beta初始解 可以建立矩阵方程： N = 4 的情况 : \color{blue}{N=4的情况}: N=4的情况: L 6 × 4 ⋅ β 4 × 1 = ρ 6 × 1 (16) \color{Green} \tag{16} \mathbf{L}_{6 \times 4} \cdot \boldsymbol{\beta}_{4 \times 1}=\boldsymbol{\rho}_{6 \times 1} L6×4​⋅β4×1​=ρ6×1​(16) N = 3 的情况 : \color{blue}{N=3的情况}: N=3的情况: L 6 × 5 ⋅ β 5 × 1 = ρ 6 × 1 (19) \color{Green} \tag{19} \mathbf{L}_{6 \times 5} \cdot \boldsymbol{\beta}_{5 \times 1}=\boldsymbol{\rho}_{6 \times 1} L6×5​⋅β5×1​=ρ6×1​(19) N = 2 的情况 : \color{blue}{N=2的情况}: N=2的情况: L 6 × 3 ⋅ β 3 × 1 = ρ 6 × 1 (22) \color{Green} \tag{22} \mathbf{L}_{6 \times 3} \cdot \boldsymbol{\beta}_{3 \times 1}=\boldsymbol{\rho}_{6 \times 1} L6×3​⋅β3×1​=ρ6×1​(22) N = 1 的情况 ( 存在闭解 , 直接求解即可 ) : \color{blue}{N=1的情况(存在闭解,直接求解即可)}: N=1的情况(存在闭解,直接求解即可): β = ∑ { i , j } ∈ [ 1 : 4 ] ∥ v [ i ] − v [ j ] ∥ ⋅ ∥ c i w − c j w ∥ ∑ { i , j } ∈ [ 1 : 4 ] ∥ v [ i ] − v [ j ] ∥ 2 (06) \color{Green} \tag{06} \beta=\frac{\sum_{\{i, j\} \in[1: 4]}\left\|\mathbf{v}^{[i]}-\mathbf{v}^{[j]}\right\| \cdot\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{w}\right\|}{\sum_{\{i, j\} \in[1: 4]}\left\|\mathbf{v}^{[i]}-\mathbf{v}^{[j]}\right\|^{2}} β=∑{i,j}∈[1:4]​∥ ∥​v[i]−v[j]∥ ∥​2∑{i,j}∈[1:4]​∥ ∥​v[i]−v[j]∥ ∥​⋅∥ ∥​ciw​−cjw​∥ ∥​​(06) 通过如上方程即可求得 β \boldsymbol{\beta} β 的初始解，进一步通过高斯牛顿迭代对其进行优化。下面我们就来看看源码中的实现，其代码都位于src/PnPsolver.cc 文件中，在 compute_pose() 函数中可以看到如下代码:

  // 求解近似解：N=4的情况
  find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
  // 高斯牛顿法迭代优化得到 beta
  gauss_newton(&L_6x10, &Rho, Betas[1]);
  
  // 求解近似解：N=2的情况
  find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
  gauss_newton(&L_6x10, &Rho, Betas[2]);

  // 求解近似解：N=3的情况
  find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
  gauss_newton(&L_6x10, &Rho, Betas[3]);

 

二、find_betas_approx_1

/**
 * @brief 计算N=4时候的粗糙近似解，暴力将其他量置为0
 * 
 * @param[in]  L_6x10  矩阵L
 * @param[in]  Rho     非齐次项 \rho, 列向量
 * @param[out] betas   计算得到的beta
 */
void PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
			       double * betas)
{
  // 计算N=4时候的粗糙近似解，暴力将其他量置为0
  // betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]  -- L_6x10中每一行的内容
  // betas_approx_1 = [B11 B12     B13         B14            ]  -- L_6x4 中一行提取出来的内容

  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);

  // 提取L_6x10矩阵中每行的第0,1,3,6个元素，得到L_6x4
  for(int i = 0; i