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

1. 计算单应矩阵H

（1）大致流程

• 对匹配的特征点对进行归一化
• 进行RANSAC迭代，在每次迭代中执行如下操作：
  • 根据前面得到的mvSets，获取用于当前迭代的经归一化后的8对匹配点
  • 8点法计算单应矩阵ComputeH21
  • 将基于归一化点的单应矩阵恢复为基于输入的匹配特征点的单应矩阵
  • 利用重投影误差计算当前迭代的评分CheckHomography
  • 更新最佳评分时的单应矩阵与内点标记

（2）代码实现

/**
 * @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
 * Step 1 将当前帧和参考帧中的特征点坐标进行归一化
 * Step 2 选择8个归一化之后的点对进行迭代
 * Step 3 八点法计算单应矩阵
 * Step 4 利用重投影误差为当次RANSAC的结果评分
 * Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
 * 
 * @param[in & out] vbMatchesInliers          标记是否为内点
 * @param[in & out] score                     计算单应矩阵的得分
 * @param[in & out] H21                       单应矩阵结果
 */
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, 
                                 cv::Mat &H21)
{
   
   
    // 匹配的特征点对总数
    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);
    cv::Mat T2inv = T2.inv();  // 提前计算出T2inv, 后面会用于恢复出基于输入特征点的单应矩阵
    
    // 记录最佳评分
    score = 0.0;
	// 取得历史最佳评分时,特征点对的inliers标记
    vbMatchesInliers = vector<bool>(N,false);
    // 某次迭代中，参考帧1的特征点坐标
    vector<cv::Point2f> vPn1i(8);
	// 某次迭代中，当前帧2的特征点坐标
    vector<cv::Point2f> vPn2i(8);
	// 计算出来的单应矩阵、及其逆矩阵
    cv::Mat H21i, H12i;
    // 每次RANSAC迭代时的Inliers标记，用于更新最佳评分时的vbMatchesInliers
    vector<bool> vbCurrentInliers(N,false);
    // 每次RANSAC迭代时的评分
    float currentScore;
    
    // 下面进行每次的RANSAC迭代
    for(int it=0; it<mMaxIterations; it++)
    {
   
   
        // Step 2 根据mvStes选择8个归一化之后的点对进行迭代
        for(size_t 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 Hn = ComputeH21(vPn1i,vPn2i);
/*
    前面计算出的单应矩阵Hn是基于归一化点计算出来的，现在需要将其转换为基于输入的普通特征点得到的单应矩阵
    单应矩阵原理: X2=H21*X1，其中X1,X2 为归一化后的特征点    
    特征点归一化: vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  
    得到: T2 * mvKeys2 =  Hn * T1 * mvKeys1   
    进一步得到: mvKeys2  = T2.inv * Hn * T1 * mvKeys1
*/
        H21i = T2inv*Hn*T1;  // 第i次迭代得到的1到2的单应矩阵变换
        
        // Step 4 利用重投影误差为当次RANSAC的结果评分
        H12i = H21i.inv();
        currentScore = CheckHomography(
                				H21i, H12i, 		 // 输入，单应矩阵的计算结果
							    vbCurrentInliers,    // 输出，特征点对的Inliers标记
								mSigma);			 // 测量误差，在构造Initializer时确定的
        
        // Step 5 更新最优评分的单应矩阵计算结果H21, 并保存此时的特征点对的内点标记vbMatchesInliers
        if(currentScore>score)
        {
   
   
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

计算单应矩阵ComputeH21

（1）原理

​ 假设特征匹配点对分别为$$p_1=[u_1,v_1,1{]}^T,p_2=[u_2,v_2,1{]}^T$$，用单应矩阵$H_{21}$描述二者之间的变换关系：
$$p_2=H_{21}{p}_1$$
​ 写成矩阵形式为：
$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]$$
​ 为了转化为齐次方程，左右两边同时叉乘$p_2$，得到：
$$p_2\times H_{21}{p}_{}$$