ORB-SLAM2源码逐行解析系列（九）：ORB-SLAM2中基于F矩阵的单目初始化

1. 计算基础矩阵F

（1）大致流程

• 对匹配的特征点对进行归一化
• 进行RANSAC迭代，在每次迭代中执行如下操作：
  • 根据前面得到的mvSets，获取用于当前迭代的经归一化后的8对匹配点
  • 8点法计算基础矩阵ComputeF21
  • 将基于归一化点的基础矩阵恢复为基于输入的匹配特征点的基础矩阵
  • 将特征点到对应极线的距离作为误差，结合卡方分布获取当前基础矩阵的得分CheckFundamental
  • 更新最佳评分时的基础矩阵与内点标记

（2）代码实现

/**
 * @brief 计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分
 * Step 1 将当前帧和参考帧中的特征点坐标进行归一化
 * Step 2 选择8个归一化之后的点对进行迭代
 * Step 3 八点法计算基础矩阵
 * Step 4 利用重投影误差为当次RANSAC的结果评分
 * Step 5 更新具有最优评分的基础矩阵计算结果,并且保存所对应的特征点对的内点标记
 * 
 * @param[in & out] vbMatchesInliers          标记是否为内点
 * @param[in & out] score                     计算基础矩阵得分
 * @param[in & out] F21                       从特征点1到2的基础矩阵
 */
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, 
                                  float &score, cv::Mat &F21)
{
   
    // 匹配的特征点对总数  
    const int N = mvMatches12.size();
    
    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
    // 归一化后的参考帧1和当前帧2中的特征点坐标
    vector<cv::Point2f> vPn1, vPn2;
    // 记录各自的归一化矩阵
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
	// 提前计算出T2inv, 后面会用于恢复出基于输入特征点的单应矩阵
    cv::Mat T2t = T2.t();
    
    // 记录最佳评分
    score = 0.0;
    // 取得历史最佳评分时,特征点对的inliers标记
    vbMatchesInliers = vector<bool>(N,false);
	// 某次迭代中，参考帧1的特征点坐标
    vector<cv::Point2f> vPn1i(8);
    // 某次迭代中，当前帧2的特征点坐标
    vector<cv::Point2f> vPn2i(8);
    // 某次迭代中计算出来的基础矩阵
    cv::Mat F21i;
	// 每次RANSAC迭代时的Inliers标记，用于更新最佳评分时的vbMatchesInliers
    vector<bool> vbCurrentInliers(N,false);
    // 每次RANSAC迭代时的评分
    float currentScore;

    // 下面进行每次的RANSAC迭代
    for(int it=0; it<mMaxIterations; it++)
    {
   
        // Step 2 根据mvStes选择8个归一化之后的点对进行迭代
        for(int j=0; j<8; j++)
        {
   
            int idx = mvSets[it][j];
/*
   vPn1为参考帧1中特征点的归一化坐标，vPn2为当前帧2中特征点的归一化坐标
   mvMatches12的first表示第一帧(参考帧)的特征点索引，mvMatches12的second表示与之匹配的第二帧(当前帧)的特征点索引
    故vPn1i[j]表示第idx个匹配对中参考帧的特征点，vPn2i[j]表示第idx个匹配对中当前帧的特征点
*/
            vPn1i[j] = vPn1[mvMatches12[idx].first];        
            vPn2i[j] = vPn2[mvMatches12[idx].second];       
        }
        // Step 3 八点法计算基础矩阵
        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
/*
    前面计算出的基础矩阵Fn是基于归一化点计算出来的，现在需要将其转换为基于输入的普通特征点得到的基础矩阵
    基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标     
    特征点归一化: vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  
    根据基础矩阵约束得到:(T2 * mvKeys2)^t* Fn * T1 * mvKeys1 = 0  
    进一步得到:mvKeys2^t * (T2^t * Fn * T1) * mvKeys1 = 0
    故: T2^t * Hn * T1为基于输入的普通特征点对应的基础矩阵
*/
        F21i = T2t*Fn*T1;  
        
        // Step 4 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

		// Step 5 更新具有最优评分的基础矩阵计算结果F21,并保存此时特征点对的内点标记vbMatchesInliers
        if(currentScore>score)
        {
   
            F21 = F21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

计算基础矩阵ComputeF21

（1）原理

​ 假设特征匹配点对分别为$$p_1=[u_1,v_1,1{]}^T,p_2=[u_2,v_2,1{]}^T$$，用基础矩阵$F_{21}$​描述二者之间的变换关系：
$$p_2^T{F}_{21}{p}_1=0$$
​ 将上式展开，写成矩阵形式为：
$$\left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]\left[\begin{array}{ccc}{f}_1& f_2& f_3\\ f_4& f_5& f_6\\ f_7& f_8& f_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
​ 为方便计算，将前两项计算结果表示为$$\left[\begin{array}{ccc}a& b& c\end{array}\right]$$​，则有：
$$\begin{gathered} a=f_1{u}_2+f_4{v}_2+f_7 \\ b=f_2{u}_2+f_5{v}_2+f_8 \\ c=f_3{u}_2+f_6{v}_2+f_9 \end{gathered}$$
​ 写成矩阵形式为：
$$\left[\begin{array}{ccc}a& b& c\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
​ 展开后可得：
$$a{u}_1+b{v}_1+c=0$$
​ 代入前面的矩阵方程中，并整理可得：
$$f_1{u}_1{u}_2+f_2{v}_1{u}_2+f_3{u}_2+f_4{u}_1{v}_2+f_5{v}_1{v}_2+f_6{v}_2+f_7{u}_1+f_8{v}_1+f_9=0$$
​ 转化为矩阵形式：
$$[\begin{array}{cccccccc}{u}_1{u}_2& v_1{u}_2& u_2& u_1{v}_2& v_1{v}_2& v_2& u_1& v_1\end{array}$$