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

一、前言

从上一篇博客中，以及了解了类 PnPsolver 总体流程与思路，并且分析其核心函数 PnPsolver::iterate()，再进行详细分析之前，把前面博客 (01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束 推导出来的公式粘贴一下(标号没有改变)
c 1 w = 1 n ∑ i = 1 n p i w (01) \color{Green} \tag{01} \mathbf{c}_{1}^{w}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w} c1w​=n1​i=1∑n​piw​(01) c 2 w = c 1 w + λ 1 w n v 1 w c 3 w = c 1 w + λ 2 w n v 2 w c 4 w = c 1 w + λ 3 w n v 3 w (03) \color{Green} \tag{03} \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} c2w​=c1w​+nλ1w​​ ​v1w​c3w​=c1w​+nλ2w​​ ​v2w​c4w​=c1w​+nλ3w​​ ​v3w​​(03)
[ α i 2 α i 3 α i 4 ] = [ c 2 w − c 1 w c 3 w − c 1 w c 4 w − c 1 w ] − 1 ( p i w − c 1 w ) α i 1 = 1 − α i 2 − α i 3 − α i 4 (07) \color{Green} \tag{07} \begin{array}{c} {\left[\begin{array}{l} \alpha_{i 2} \\ \alpha_{i 3} \\ \alpha_{i 4} \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_{i 1}=1-\alpha_{i 2}-\alpha_{i 3}-\alpha_{i 4} \end{array} ⎣ ⎡​αi2​αi3​αi4​​⎦ ⎤​=[c2w​−c1w​​c3w​−c1w​​c4w​−c1w​​]−1(piw​−c1w​)αi1​=1−αi2​−αi3​−αi4​​(07) M x = 0 (14) \color{Green} \tag{14} \mathbf M \mathbf x=0 Mx=0(14)其符号字母的含义这里也不再重述了，不太明白的朋友直接通过上面给出的博客链接可以查看具体推导过程。
 

二、代码流程

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

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