• ORB-SLAM2 ---- Initializer::ReconstructH函数

    目录

    1.函数作用

    2.函数解析 

    2.1 调用函数解析

    2.2 Initializer::ReconstructH函数总体思路

    2.2.1 代码

    2.2.2 总体思路解析 

    3.Initializer::CheckRT

    3.1 函数作用

    3.2 构造函数 

    3.3 代码 

    3.4 流程解析 

    3.4.0 初始化参数

    3.4.1 计算初始化两帧的投影矩阵 

    3.4.2  三角化恢复三维点Initializer::Triangulate

    3.4.3  遍历所有的特征点对检查三维点是否合适

    3.4.4 最后处理 

    1.函数作用

            用H矩阵恢复R, t和三维点。

    2.函数解析 

    2.1 调用函数解析

    1. ​return ReconstructH(vbMatchesInliersH,	//输入，匹配成功的特征点对Inliers标记
    2. 							H,					//输入，前面RANSAC计算后的单应矩阵
    3. 							mK,					//输入，相机的内参数矩阵
    4. 							R21,t21,			//输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
    5. 							vP3D,				//特征点对经过三角测量之后的空间坐标，也就是地图点
    6. 							vbTriangulated,		//特征点对是否成功三角化的标记
    7. 							1.0,				//这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时
    8. 												//需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
    9. 							50);				//为了进行运动恢复，所需要的最少的三角化测量成功的点个数
    10.  
    11. ​

            该函数的调用函数为Initializer::Initialize，该函数的目的是初始化SLAM系统，即用单目初始化器的第一帧作为SLAM系统的基点并计算出第一帧和第二帧的变换矩阵并初始化地图点。此函数是在计算出H矩阵的前提下，我们想通过H矩阵来恢复单目初始化器两帧间的位姿。

            输入参数为匹配成功的特征点对Inliers标记、RANSAC计算出的单应矩阵H、相机的内参、认为某对特征点的三角化测量中有效时需要满足的最小视差角、为了进行运动恢复，所需要的最少的三角化测量成功的点个数（如果恢复的3D点小于这个则认为初始化失败）

            输出参数为计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换、特征点对经过三角测量之后的空间坐标，也就是地图点。

    2.2 Initializer::ReconstructH函数总体思路

    2.2.1 代码

    1. /**
    2.  * @brief 用H矩阵恢复R, t和三维点
    3.  * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
    4.  * 代码使用了Faugeras SVD-based decomposition算法，参考文献
    5.  * Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988 
    6.  * 
    7.  * @param[in] vbMatchesInliers          匹配点对的内点标记
    8.  * @param[in] H21                       从参考帧到当前帧的单应矩阵
    9.  * @param[in] K                         相机的内参数矩阵
    10.  * @param[in & out] R21                 计算出来的相机旋转
    11.  * @param[in & out] t21                 计算出来的相机平移
    12.  * @param[in & out] vP3D                世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标
    13.  * @param[in & out] vbTriangulated      特征点是否成功三角化的标记
    14.  * @param[in] minParallax               对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
    15.  * @param[in] minTriangulated           为了进行运动恢复，所需要的最少的三角化测量成功的点个数
    16.  * @return true                         单应矩阵成功计算出位姿和三维点
    17.  * @return false                        初始化失败
    18.  */
    19. bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
    20.                       cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
    21. {
    22.  
    23.     // 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T
    24.     // 参考 ：Motion and structure from motion in a piecewise plannar environment.
    25.     //        International Journal of Pattern Recognition and Artificial Intelligence, 1988
    26.     // https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment
    27.     
    28.     // 流程:
    29.     //      1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解
    30.     //        1.1 讨论 d' > 0 时的 4 组解
    31.     //        1.2 讨论 d' < 0 时的 4 组解
    32.     //      2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
    33.  
    34.     // 统计匹配的特征点对中属于内点(Inlier)或有效点个数
    35.     int N=0;
    36.     for(size_t i=0, iend = vbMatchesInliers.size() ; i
    37.         if(vbMatchesInliers[i])
    38.             N++;
    39.  
    40.     // We recover 8 motion hypotheses using the method of Faugeras et al.
    41.     // Motion and structure from motion in a piecewise planar environment.
    42.     // International Journal of Pattern Recognition and Artificial Intelligence, 1988
    43.  
    44.     // 参考SLAM十四讲第二版p170-p171
    45.     // H = K * (R - t * n / d) * K_inv
    46.     // 其中: K表示内参数矩阵
    47.     //       K_inv 表示内参数矩阵的逆
    48.     //       R 和 t 表示旋转和平移向量
    49.     //       n 表示平面法向量
    50.     // 令 H = K * A * K_inv
    51.     // 则 A = k_inv * H * k
    52.  
    53.     cv::Mat invK = K.inv();
    54.     cv::Mat A = invK*H21*K;
    55.  
    56.     // 对矩阵A进行SVD分解
    57.     // A 等待被进行奇异值分解的矩阵
    58.     // w 奇异值矩阵
    59.     // U 奇异值分解左矩阵
    60.     // Vt 奇异值分解右矩阵，注意函数返回的是转置
    61.     // cv::SVD::FULL_UV 全部分解
    62.     // A = U * w * Vt
    63.     cv::Mat U,w,Vt,V;
    64.     cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);
    65.  
    66.     // 根据文献eq(8)，计算关联变量
    67.     V=Vt.t();
    68.  
    69.     // 计算变量s = det(U) * det(V)
    70.     // 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt)
    71.     float s = cv::determinant(U)*cv::determinant(Vt);
    72.     
    73.     // 取得矩阵的各个奇异值
    74.     float d1 = w.at<float>(0);
    75.     float d2 = w.at<float>(1);
    76.     float d3 = w.at<float>(2);
    77.  
    78.     // SVD分解正常情况下特征值di应该是正的，且满足d1>=d2>=d3
    79.     if(d1/d2<1.00001 || d2/d3<1.00001) {
    80.         return false;
    81.     }
    82.  
    83.  
    84.     // 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算
    85.     // 定义8中情况下的旋转矩阵、平移向量和空间向量
    86.     vector vR, vt, vn;
    87.     vR.reserve(8);
    88.     vt.reserve(8);
    89.     vn.reserve(8);
    90.  
    91.     // Step 1.1 讨论 d' > 0 时的 4 组解
    92.     // 根据论文eq.(12)有
    93.     // x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
    94.     // x2 = 0
    95.     // x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))
    96.     // 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))
    97.     //    aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))
    98.     // 则
    99.     // x1 = e1 * aux1
    100.     // x3 = e3 * aux2
    101.  
    102.     // 因为 e1,e2,e3 = 1 or -1
    103.     // 所以有x1和x3有四种组合
    104.     // x1 =  {aux1,aux1,-aux1,-aux1}
    105.     // x3 =  {aux3,-aux3,aux3,-aux3}
    106.  
    107.     float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
    108.     float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
    109.     float x1[] = {aux1,aux1,-aux1,-aux1};
    110.     float x3[] = {aux3,-aux3,aux3,-aux3};
    111.  
    112.  
    113.     // 根据论文eq.(13)有
    114.     // sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2
    115.     // cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 
    116.     // 令  aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)
    117.     // 则  sin(theta) = e1 * e3 * aux_stheta
    118.     //     cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)
    119.     // 因为 e1 e2 e3 = 1 or -1
    120.     // 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}
    121.     float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
    122.     float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
    123.     float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
    124.  
    125.     // 计算旋转矩阵 R'
    126.     //根据不同的e1 e3组合所得出来的四种R t的解
    127.     //      | ctheta      0   -aux_stheta|       | aux1|
    128.     // Rp = |    0        1       0      |  tp = |  0  |
    129.     //      | aux_stheta  0    ctheta    |       |-aux3|
    130.  
    131.     //      | ctheta      0    aux_stheta|       | aux1|
    132.     // Rp = |    0        1       0      |  tp = |  0  |
    133.     //      |-aux_stheta  0    ctheta    |       | aux3|
    134.  
    135.     //      | ctheta      0    aux_stheta|       |-aux1|
    136.     // Rp = |    0        1       0      |  tp = |  0  |
    137.     //      |-aux_stheta  0    ctheta    |       |-aux3|
    138.  
    139.     //      | ctheta      0   -aux_stheta|       |-aux1|
    140.     // Rp = |    0        1       0      |  tp = |  0  |
    141.     //      | aux_stheta  0    ctheta    |       | aux3|
    142.     // 开始遍历这四种情况中的每一种
    143.     for(int i=0; i<4; i++)
    144.     {
    145.         //生成Rp，就是eq.(8) 的 R'
    146.         cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
    147.         Rp.at<float>(0,0)=ctheta;
    148.         Rp.at<float>(0,2)=-stheta[i];
    149.         Rp.at<float>(2,0)=stheta[i];        
    150.         Rp.at<float>(2,2)=ctheta;
    151.  
    152.         // eq.(8) 计算R
    153.         cv::Mat R = s*U*Rp*Vt;
    154.  
    155.         // 保存
    156.         vR.push_back(R);
    157.  
    158.         // eq. (14) 生成tp 
    159.         cv::Mat tp(3,1,CV_32F);
    160.         tp.at<float>(0)=x1[i];
    161.         tp.at<float>(1)=0;
    162.         tp.at<float>(2)=-x3[i];
    163.         tp*=d1-d3;
    164.  
    165.         // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
    166.         // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
    167.         // eq.(8)恢复原始的t
    168.         cv::Mat t = U*tp;
    169.         vt.push_back(t/cv::norm(t));
    170.  
    171.         // 构造法向量np
    172.         cv::Mat np(3,1,CV_32F);
    173.         np.at<float>(0)=x1[i];
    174.         np.at<float>(1)=0;
    175.         np.at<float>(2)=x3[i];
    176.  
    177.         // eq.(8) 恢复原始的法向量
    178.         cv::Mat n = V*np;
    179.         //看PPT 16页的图，保持平面法向量向上
    180.         if(n.at<float>(2)<0)
    181.             n=-n;
    182.         // 添加到vector
    183.         vn.push_back(n);
    184.     }
    185.     
    186.     // Step 1.2 讨论 d' < 0 时的 4 组解
    187.     float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
    188.     // cos_theta项
    189.     float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
    190.     // 考虑到e1,e2的取值，这里的sin_theta有两种可能的解
    191.     float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
    192.  
    193.     // 对于每种由e1 e3取值的组合而形成的四种解的情况
    194.     for(int i=0; i<4; i++)
    195.     {
    196.         // 计算旋转矩阵 R'
    197.         cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
    198.         Rp.at<float>(0,0)=cphi;
    199.         Rp.at<float>(0,2)=sphi[i];
    200.         Rp.at<float>(1,1)=-1;
    201.         Rp.at<float>(2,0)=sphi[i];
    202.         Rp.at<float>(2,2)=-cphi;
    203.  
    204.         // 恢复出原来的R
    205.         cv::Mat R = s*U*Rp*Vt;
    206.         // 然后添加到vector中
    207.         vR.push_back(R);
    208.  
    209.         // 构造tp
    210.         cv::Mat tp(3,1,CV_32F);
    211.         tp.at<float>(0)=x1[i];
    212.         tp.at<float>(1)=0;
    213.         tp.at<float>(2)=x3[i];
    214.         tp*=d1+d3;
    215.  
    216.         // 恢复出原来的t
    217.         cv::Mat t = U*tp;
    218.         // 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的
    219.         vt.push_back(t/cv::norm(t));
    220.  
    221.         // 构造法向量np
    222.         cv::Mat np(3,1,CV_32F);
    223.         np.at<float>(0)=x1[i];
    224.         np.at<float>(1)=0;
    225.         np.at<float>(2)=x3[i];
    226.  
    227.         // 恢复出原来的法向量
    228.         cv::Mat n = V*np;
    229.         // 保证法向量指向上方
    230.         if(n.at<float>(2)<0)
    231.             n=-n;
    232.         // 添加到vector中
    233.         vn.push_back(n);
    234.     }
    235.  
    236.     // 最好的good点
    237.     int bestGood = 0;
    238.     // 其次最好的good点
    239.     int secondBestGood = 0;    
    240.     // 最好的解的索引，初始值为-1
    241.     int bestSolutionIdx = -1;
    242.     // 最大的视差角
    243.     float bestParallax = -1;
    244.     // 存储最好解对应的，对特征点对进行三角化测量的结果
    245.     vector bestP3D;
    246.     // 最佳解所对应的，那些可以被三角化测量的点的标记
    247.     vector<bool> bestTriangulated;
    248.  
    249.     // Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax)
    250.     // We reconstruct all hypotheses and check in terms of triangulated points and parallax
    251.     
    252.     // Step 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
    253.     for(size_t i=0; i<8; i++)
    254.     {
    255.         // 第i组解对应的比较大的视差角
    256.         float parallaxi;
    257.         // 三角化测量之后的特征点的空间坐标
    258.         vector vP3Di;
    259.         // 特征点对是否被三角化的标记
    260.         vector<bool> vbTriangulatedi;
    261.     
    262.         // 调用 Initializer::CheckRT(), 计算good点的数目
    263.         int nGood = CheckRT(vR[i],vt[i],                    //当前组解的旋转矩阵和平移向量
    264.                             mvKeys1,mvKeys2,                //特征点
    265.                             mvMatches12,vbMatchesInliers,   //特征匹配关系以及Inlier标记
    266.                             K,                              //相机的内参数矩阵
    267.                             vP3Di,                          //存储三角化测量之后的特征点空间坐标的
    268.                             4.0*mSigma2,                    //三角化过程中允许的最大重投影误差
    269.                             vbTriangulatedi,                //特征点是否被成功进行三角测量的标记
    270.                             parallaxi);                     // 这组解在三角化测量的时候的比较大的视差角
    271.         
    272.         // 更新历史最优和次优的解
    273.         // 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出
    274.         if(nGood>bestGood)
    275.         {
    276.             // 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优
    277.             secondBestGood = bestGood;
    278.             // 更新历史最优点
    279.             bestGood = nGood;
    280.             // 最优解的组索引为i（就是当前次遍历）
    281.             bestSolutionIdx = i;
    282.             // 更新变量
    283.             bestParallax = parallaxi;
    284.             bestP3D = vP3Di;
    285.             bestTriangulated = vbTriangulatedi;
    286.         }
    287.         // 如果当前组的good计数小于历史最优但却大于历史次优
    288.         else if(nGood>secondBestGood)
    289.         {
    290.             // 说明当前组解是历史次优点，更新之
    291.             secondBestGood = nGood;
    292.         }
    293.     }
    294.  
    295.  
    296.  
    297.     // Step 3 选择最优解。要满足下面的四个条件
    298.     // 1. good点数最优解明显大于次优解，这里取0.75经验值
    299.     // 2. 视角差大于规定的阈值
    300.     // 3. good点数要大于规定的最小的被三角化的点数量
    301.     // 4. good数要足够多，达到总数的90%以上
    302.     if(secondBestGood<0.75*bestGood &&      
    303.        bestParallax>=minParallax &&
    304.        bestGood>minTriangulated && 
    305.        bestGood>0.9*N)
    306.     {
    307.         // 从最佳的解的索引访问到R，t
    308.         vR[bestSolutionIdx].copyTo(R21);
    309.         vt[bestSolutionIdx].copyTo(t21);
    310.         // 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用
    311.         vP3D = bestP3D;
    312.         // 获取特征点的被成功进行三角化的标记
    313.         vbTriangulated = bestTriangulated;
    314.  
    315.         //返回真，找到了最好的解
    316.         return true;
    317.     }
    318.     return false;
    319. }

    2.2.2 总体思路解析 

            1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解（不需要明白）
                  1.1 讨论 d' > 0 时的 4 组解
                  1.2 讨论 d' < 0 时的 4 组解
            2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解。（CheckRT）

            这里分解为八组解为论文中所做的事情，我们不加解释，主要将这八组解解出的R，t，这8 组解进行验证，并选择产生相机前方最多3D点的解为最优解。

            总体思路：我们按照论文所作计算出了八组R,t解，对每一个解进行验证（计算能恢复的3D点数量），记录恢复3D点最多和次多的R,t解。判断：

            如果：

            ①0.75倍最优解的数量大于次优解（保证系统鲁棒性）

            ②视角差大于规定的阈值

            ③good点数要大于规定的最小的被三角化的点数量
            ④good数要足够多，达到总数的90%以上

            则：

            选取这组解的R,t作为地图初始化的R,t，这组解初始化的3D点作为初始地图点使用，获取特征点的被成功进行三角化的标记，向上层函数返回true表示单目初始化成功。

    3.Initializer::CheckRT

    3.1 函数作用

            用位姿来对特征匹配点三角化，从中筛选中合格的三维点。

    3.2 构造函数 

    1.  * @brief 用位姿来对特征匹配点三角化，从中筛选中合格的三维点
    2.  * 
    3.  * @param[in] R                                     旋转矩阵R
    4.  * @param[in] t                                     平移矩阵t
    5.  * @param[in] vKeys1                                参考帧特征点  
    6.  * @param[in] vKeys2                                当前帧特征点
    7.  * @param[in] vMatches12                            两帧特征点的匹配关系
    8.  * @param[in] vbMatchesInliers                      特征点对内点标记
    9.  * @param[in] K                                     相机内参矩阵
    10.  * @param[in & out] vP3D                            三角化测量之后的特征点的空间坐标
    11.  * @param[in] th2                                   重投影误差的阈值
    12.  * @param[in & out] vbGood                          标记成功三角化点？
    13.  * @param[in & out] parallax                        计算出来的比较大的视差角（注意不是最大，具体看后面代码）
    14.  * @return int 
    15.  */
    16. int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2,
    17.                        const vector &vMatches12, vector<bool> &vbMatchesInliers,
    18.                        const cv::Mat &K, vector &vP3D, float th2, vector<bool> &vbGood, float ¶llax)

            传入参数：

            ①参考帧到当前帧的旋转矩阵R和平移矩阵t

            ②参考帧（第一帧）和当前帧（第二帧）的特征点容器vKeys1、vKeys2

            ③两帧特征点的匹配标记vMatches12以及特征点对内点标记vbMatchesInliers

            ④相机内参K，重投影误差阈值th2

            传出参数：

            ①三角化测量之后的特征点的空间坐标v3D

            ②标记成功三角化点vGoog

            ③返回三角化点的数量

    3.3 代码 

    1. int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2,
    2.                        const vector &vMatches12, vector<bool> &vbMatchesInliers,
    3.                        const cv::Mat &K, vector &vP3D, float th2, vector<bool> &vbGood, float ¶llax)
    4. {   
    5.     // 对给出的特征点对及其R t , 通过三角化检查解的有效性，也称为 cheirality check
    6.  
    7.     // Calibration parameters
    8. 	//从相机内参数矩阵获取相机的校正参数
    9.     const float fx = K.at<float>(0,0);
    10.     const float fy = K.at<float>(1,1);
    11.     const float cx = K.at<float>(0,2);
    12.     const float cy = K.at<float>(1,2);
    13.  
    14. 	//特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点
    15.     vbGood = vector<bool>(vKeys1.size(),false);
    16. 	//重设存储空间坐标的点的大小
    17.     vP3D.resize(vKeys1.size());
    18.  
    19. 	//存储计算出来的每对特征点的视差
    20.     vector<float> vCosParallax;
    21.     vCosParallax.reserve(vKeys1.size());
    22.  
    23.     // Camera 1 Projection Matrix K[I|0]
    24.     // Step 1：计算相机的投影矩阵  
    25.     // 投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。
    26.     // 对于第一个相机是 P1=K*[I|0]
    27.  
    28.     // 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵
    29.     cv::Mat P1(3,4,				//矩阵的大小是3x4
    30. 			   CV_32F,			//数据类型是浮点数
    31. 			   cv::Scalar(0));	//初始的数值是0
    32. 	//将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵，因为 K*I = K
    33.     K.copyTo(P1.rowRange(0,3).colRange(0,3));
    34.     // 第一个相机的光心设置为世界坐标系下的原点
    35.     cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
    36.  
    37.     // Camera 2 Projection Matrix K[R|t]
    38.     // 计算第二个相机的投影矩阵 P2=K*[R|t]
    39.     cv::Mat P2(3,4,CV_32F);
    40.     R.copyTo(P2.rowRange(0,3).colRange(0,3));
    41.     t.copyTo(P2.rowRange(0,3).col(3));
    42. 	//最终结果是K*[R|t]
    43.     P2 = K*P2;
    44.     // 第二个相机的光心在世界坐标系下的坐标
    45.     cv::Mat O2 = -R.t()*t;
    46.  
    47. 	//在遍历开始前，先将good点计数设置为0
    48.     int nGood=0;
    49.  
    50. 	// 开始遍历所有的特征点对
    51.     for(size_t i=0, iend=vMatches12.size();i
    52.     {
    53.  
    54. 		// 跳过outliers
    55.         if(!vbMatchesInliers[i])
    56.             continue;
    57.  
    58.         // Step 2 获取特征点对，调用Triangulate() 函数进行三角化，得到三角化测量之后的3D点坐标
    59.         // kp1和kp2是匹配好的有效特征点
    60.         const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
    61.         const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
    62. 		//存储三维点的的坐标
    63.         cv::Mat p3dC1;
    64.  
    65.         // 利用三角法恢复三维点p3dC1
    66.         Triangulate(kp1,kp2,	//特征点
    67. 					P1,P2,		//投影矩阵
    68. 					p3dC1);		//输出，三角化测量之后特征点的空间坐标		
    69.  
    70. 		// Step 3 第一关：检查三角化的三维点坐标是否合法（非无穷值）
    71.         // 只要三角测量的结果中有一个是无穷大的就说明三角化失败，跳过对当前点的处理，进行下一对特征点的遍历 
    72.         if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
    73.         {
    74. 			//其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点
    75.             vbGood[vMatches12[i].first]=false;
    76. 			//继续对下一对匹配点的处理
    77.             continue;
    78.         }
    79.  
    80.         // Check parallax
    81.         // Step 4 第二关：通过三维点深度值正负、两相机光心视差角大小来检查是否合法 
    82.  
    83.         //得到向量PO1
    84.         cv::Mat normal1 = p3dC1 - O1;
    85. 		//求取模长，其实就是距离
    86.         float dist1 = cv::norm(normal1);
    87.  
    88. 		//同理构造向量PO2
    89.         cv::Mat normal2 = p3dC1 - O2;
    90. 		//求模长
    91.         float dist2 = cv::norm(normal2);
    92.  
    93. 		//根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子
    94.         float cosParallax = normal1.dot(normal2)/(dist1*dist2);
    95.  
    96.         // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
    97.         // 如果深度值为负值，为非法三维点跳过该匹配点对
    98.         // ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？
    99.         // ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 
    100.         // !可能导致初始化不稳定
    101.         if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
    102.             continue;
    103.  
    104.         // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
    105.         // 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2
    106.         cv::Mat p3dC2 = R*p3dC1+t;	
    107. 		//判断过程和上面的相同
    108.         if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
    109.             continue;
    110.  
    111.         // Step 5 第三关：计算空间点在参考帧和当前帧上的重投影误差，如果大于阈值则舍弃
    112.         // Check reprojection error in first image
    113.         // 计算3D点在第一个图像上的投影误差
    114. 		//投影到参考帧图像上的点的坐标x,y
    115.         float im1x, im1y;
    116. 		//这个使能空间点的z坐标的倒数
    117.         float invZ1 = 1.0/p3dC1.at<float>(2);
    118. 		//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合，因此这里就直接进行投影就可以了
    119.         im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
    120.         im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
    121.  
    122. 		//参考帧上的重投影误差，这个的确就是按照定义来的
    123.         float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
    124.  
    125.         // 重投影误差太大，跳过淘汰
    126.         if(squareError1>th2)
    127.             continue;
    128.  
    129.         // Check reprojection error in second image
    130.         // 计算3D点在第二个图像上的投影误差，计算过程和第一个图像类似
    131.         float im2x, im2y;
    132.         // 注意这里的p3dC2已经是第二个相机坐标系下的三维点了
    133.         float invZ2 = 1.0/p3dC2.at<float>(2);
    134.         im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
    135.         im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
    136.  
    137. 		// 计算重投影误差
    138.         float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
    139.  
    140.         // 重投影误差太大，跳过淘汰
    141.         if(squareError2>th2)
    142.             continue;
    143.  
    144.         // Step 6 统计经过检验的3D点个数，记录3D点视差角 
    145.         // 如果运行到这里就说明当前遍历的这个特征点对靠谱，经过了重重检验，说明是一个合格的点，称之为good点 
    146.         vCosParallax.push_back(cosParallax);
    147. 		//存储这个三角化测量后的3D点在世界坐标系下的坐标
    148.         vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
    149. 		//good点计数++
    150.         nGood++;
    151.  
    152. 		//判断视差角，只有视差角稍稍大一丢丢的才会给打good点标记
    153. 		//? bug 我觉得这个写的位置不太对。你的good点计数都++了然后才判断，不是会让good点标志和good点计数不一样吗
    154.         if(cosParallax<0.99998)
    155.             vbGood[vMatches12[i].first]=true;
    156.     }
    157.  
    158.     // Step 7 得到3D点中较小的视差角，并且转换成为角度制表示
    159.     if(nGood>0)
    160.     {
    161.         // 从小到大排序，注意vCosParallax值越大，视差越小
    162.         sort(vCosParallax.begin(),vCosParallax.end());
    163.  
    164.         // !排序后并没有取最小的视差角，而是取一个较小的视差角
    165. 		// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)
    166. 		// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 
    167.         size_t idx = min(50,int(vCosParallax.size()-1));
    168. 		//将这个选中的角弧度制转换为角度制
    169.         parallax = acos(vCosParallax[idx])*180/CV_PI;
    170.     }
    171.     else
    172. 		//如果没有good点那么这个就直接设置为0了
    173.         parallax=0;
    174.  
    175. 	//返回good点计数
    176.     return nGood;
    177. }

    3.4 流程解析 

    3.4.0 初始化参数

            ①特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点，将vGood初始化为第一帧特征点的数量，其为bool类型。

            ②三角化测量之后的特征点的空间坐标v3D初始化大小为第一帧中特征点的数量。

    3.4.1 计算初始化两帧的投影矩阵 

            投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。

            由于以第一个相机的光心作为世界坐标系。 其投影矩阵计算推导如下：

    ZP_{uv} = K(RP_{w}+t)=KTP_{w}

            我们默认第一个相机的R,t矩阵为[E|0]，因此第一个相机的投影矩阵为K。第一个相机的光心坐标为(0,0,0)^{T}。

            我们从传入参数可知第一个相机到第二个相机的R,t变换T，因此再左乘相机内参矩阵K就能得到像素坐标，即世界坐标向像素坐标的投影矩阵为KT。

            同时计算第二帧的光心坐标在原点（第一帧）的坐标，计算如下图所示：

            第二个相机的光心在世界坐标系下的坐标，即我们要求第二个相机的光心在第一个相机坐标系下的坐标。

    3.4.2  三角化恢复三维点Initializer::Triangulate

    1.数学原理

            我们将投影方程进行如下描述：

    \begin{bmatrix} x\\y \\1 \end{bmatrix}=\lambda \begin{bmatrix} p_{1} &p_{2} &p_{3} &p_{4} \\ p_{5} & p_{6} &p_{7} & p_{8} \\ p_{9} &p_{10} &p_{11} & p_{12} \end{bmatrix} \begin{bmatrix} X\\Y \\Z \\1 \end{bmatrix}

            为方便推导，简单记为：

            为了化为齐次方程，左右两边同时叉乘，得到：

            利用两对匹配点，得到：

            SVD求解，右奇异矩阵的最后一行就是最终的解。

    2.代码

    1. /** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2，从而计算三维点坐标
    2.  * @brief 
    3.  * 
    4.  * @param[in] kp1               特征点, in reference frame
    5.  * @param[in] kp2               特征点, in current frame
    6.  * @param[in] P1                投影矩阵P1
    7.  * @param[in] P2                投影矩阵P2
    8.  * @param[in & out] x3D         计算的三维点
    9.  */
    10. void Initializer::Triangulate(
    11.     const cv::KeyPoint &kp1,    //特征点, in reference frame
    12.     const cv::KeyPoint &kp2,    //特征点, in current frame
    13.     const cv::Mat &P1,          //投影矩阵P1
    14.     const cv::Mat &P2,          //投影矩阵P2
    15.     cv::Mat &x3D)               //三维点
    16. {
    17.     // 原理
    18.     // Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
    19.     // x' = P'X  x = PX
    20.     // 它们都属于 x = aPX模型
    21.     //                         |X|
    22.     // |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
    23.     // |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
    24.     // |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
    25.     // 采用DLT的方法：x叉乘PX = 0
    26.     // |yp2 -  p1|     |0|
    27.     // |p0 -  xp2| X = |0|
    28.     // |xp1 - yp0|     |0|
    29.     // 两个点:
    30.     // |yp2   -  p1  |     |0|
    31.     // |p0    -  xp2 | X = |0| ===> AX = 0
    32.     // |y'p2' -  p1' |     |0|
    33.     // |p0'   - x'p2'|     |0|
    34.     // 变成程序中的形式：
    35.     // |xp2  - p0 |     |0|
    36.     // |yp2  - p1 | X = |0| ===> AX = 0
    37.     // |x'p2'- p0'|     |0|
    38.     // |y'p2'- p1'|     |0|
    39.     // 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解.
    40.  
    41. 	//这个就是上面注释中的矩阵A
    42.     cv::Mat A(4,4,CV_32F);
    43.  
    44. 	//构造参数矩阵A
    45.     A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);   
    46.     A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    47.     A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    48.     A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
    49.  
    50. 	//奇异值分解的结果
    51.     cv::Mat u,w,vt;
    52. 	//对系数矩阵A进行奇异值分解
    53.     cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    54. 	//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定
    55. 	//别忘了我们更习惯用列向量来表示一个点的空间坐标
    56.     x3D = vt.row(3).t();
    57. 	//为了符合其次坐标的形式，使最后一维为1
    58.     x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
    59. }

            至此我们恢复一对匹配特征点的3D点。

    3.4.3  遍历所有的特征点对检查三维点是否合适

            我们遍历所有匹配的特征点对，如果不是外点，则将匹配好的特征点对传入Initializer::Triangulate函数内得到三维点。我们对三维点进行判断：

            ①检查三角化的三维点坐标是否合法：防止三角化出的点有一维坐标是无穷。

            ②通过三维点深度值正负、两相机光心视差角大小来检查是否合法 。即判断如果深度值为负值，为非法三维点跳过该匹配点对。计算视差角，当视差角比较小时，重投影误差比较大。

            ③计算空间点在参考帧和当前帧上的重投影误差，如果大于阈值则舍弃。计算3D点在第一、二个图像上的投影误差，看是否超过阈值选择是否抛弃。

            如果如上满足了，这个3D点可以被留下来，用vCosParallax向量存储合格3D点生成时计算出来的视差，存储这个三角化测量后的3D点在世界坐标系下的坐标。并将这两帧累计成功初始化的3D点nGood累加。

    vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));

    3.4.4 最后处理 

            如果我们的变量nGood大于0（成功三角化得到的3D点的数目），我们将视差角从小到大排序，排序后并没有取最小的视差角，而是取一个较小的视差角。如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)，如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角。将这个选中的角弧度制转换为角度制输出。

  • 相关阅读:
    rv1126-rknpu-v1.7.3添加opencv库
    社区故事｜SmartX 用户社区技术发烧友独家专访
    Windows 堆管理机制 [3] Windows XP SP2 – Windows 2003 版本
    【计算机网络笔记】IPv6简介
    TCPIP网络编程（一）
    ucos任务调度原理
    以前红火的农村大集，现在为何赶集的人越来越少，生意还好不好做
    MPI简谈
    AAPT: error: resource android:attr/lStar not found
    react组件父子传值
  • 原文地址：https://blog.csdn.net/qq_41694024/article/details/127873711