ORB-SLAM2 ---- Initializer::FindHomography函数

最新推荐文章于 2024-05-29 23:31:41 发布

APS2023

最新推荐文章于 2024-05-29 23:31:41 发布

阅读量762

点赞数

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签：算法矩阵 python slam

本文链接：https://blog.csdn.net/qq_41694024/article/details/127825372

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 46 订阅

订阅专栏

1.函数作用

2.Initializer::FindHomography函数构造函数

3.函数解析

3.1 每次RANSAC迭代中的细节

3.2 Initializer::CheckHomography函数解析

3.2.1 函数作用

3.2.2 函数解析

1.函数作用

计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分。

调用函数情况：调用函数为Initializer::Initialize即单目初始化函数，单目初始化要用H矩阵或者F矩阵初始化，用哪个矩阵初始化取决于那个矩阵的重投影误差低。本函数就是算 $H$ 矩阵并返回其得分。

2.Initializer::FindHomography函数构造函数

thread threadH(&Initializer::FindHomography,	//该线程的主函数
				   this,							//由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针
				   ref(vbMatchesInliersH), 			//输出，特征点对的Inlier标记
				   ref(SH), 						//输出，计算的单应矩阵的RANSAC评分
				   ref(H));							//输出，计算的单应矩阵结果

 * @brief 计算单应矩阵，假设场景为平面情况下通过前两帧求取Homography矩阵，并得到该模型的评分
 * 原理参考Multiple view geometry in computer vision  P109 算法4.4
 * 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)

上层函数输入为当前函数的指针，即当前函数能用父函数所有变量。

本函数输出内容为外点标记向量vbMatchesInliers，由第一帧向第二帧变换的单应矩阵结果H21与单应矩阵的评分score。

3.函数解析

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
    // Number of putative matches
	//匹配的特征点对总数
    const int N = mvMatches12.size();

    // Normalize coordinates
    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
    // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
    // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值
    // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标
   

	//归一化后的参考帧1和当前帧2中的特征点坐标
    vector<cv::Point2f> vPn1, vPn2;
	// 记录各自的归一化矩阵
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);

	//这里求的逆在后面的代码中要用到，辅助进行原始尺度的恢复
    cv::Mat T2inv = T2.inv();

    // Best Results variables
	// 记录最佳评分
    score = 0.0;
	// 取得历史最佳评分时,特征点对的inliers标记
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
	//某次迭代中，参考帧的特征点坐标
    vector<cv::Point2f> vPn1i(8);
	//某次迭代中，当前帧的特征点坐标
    vector<cv::Point2f> vPn2i(8);
	//以及计算出来的单应矩阵、及其逆矩阵
    cv::Mat H21i, H12i;

    // 每次RANSAC记录Inliers与得分
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
	//下面进行每次的RANSAC迭代
    for(int it=0; it<mMaxIterations; it++)
    {
        // Select a minimum set
		// Step 2 选择8个归一化之后的点对进行迭代
        for(size_t j=0; j<8; j++)
        {
			//从mvSets中获取当前次迭代的某个特征点对的索引信息
            int idx = mvSets[it][j];

            // vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
			// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
            vPn1i[j] = vPn1[mvMatches12[idx].first];    //first存储在参考帧1中的特征点索引
            vPn2i[j] = vPn2[mvMatches12[idx].second];   //second存储在参考帧1中的特征点索引
        }//读取8对特征点的归一化之后的坐标

		// Step 3 八点法计算单应矩阵
        // 利用生成的8个归一化特征点对, 调用函数 Initializer::ComputeH21() 使用八点法计算单应矩阵  
        // 关于为什么计算之前要对特征点进行归一化，后面又恢复这个矩阵的尺度？
        // 可以在《计算机视觉中的多视图几何》这本书中P193页中找到答案
        // 书中这里说,8点算法成功的关键是在构造解的方称之前应对输入的数据认真进行适当的归一化
   
        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
        
        // 单应矩阵原理：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;
		//然后计算逆
        H12i = H21i.inv();

        // Step 4 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckHomography(H21i, H12i, 			//输入，单应矩阵的计算结果
									   vbCurrentInliers, 	//输出，特征点对的Inliers标记
									   mSigma);				//TODO  测量误差，在Initializer类对象构造的时候，由外部给定的

    
        // Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
        if(currentScore>score)
        {
			//如果当前的结果得分更高，那么就更新最优计算结果
            H21 = H21i.clone();
			//保存匹配好的特征点对的Inliers标记
            vbMatchesInliers = vbCurrentInliers;
			//更新历史最优评分
            score = currentScore;
        }
    }
}

①前面的函数Initializer::Initialize我们对初始化的两帧进行了特征点匹配，mvMatches12向量记录的是特征点匹配的关系。

②用Normalize函数将去畸变后的在第一帧的特征点mvKeys1和第二帧的特征点mvKeys2归一化处理，得到vPn1和vPn2。变换矩阵为T1和T2。即 $vPn1 = T1* mvKeys1$

③进行mMaxIterations次RANSAC迭代，每一次迭代选择8个归一化之后的点对进行迭代并计算出单应矩阵与评分值，在mMaxIterations次迭代中更新具有最优评分的单应矩阵计算结果，并且保存所对应的特征点对的内点标记。作为函数输出。

3.1 每次RANSAC迭代中的细节

1.循环变量

vector<bool> vbCurrentInliers(N,false); 记录在一次RANSAC循环中是不是内点

float currentScore; 记录在一次RANSAC迭代中的单应矩阵得分

vPn1i[j]，vPn2i[j]; 记录在一次RANSAC迭代中的归一化特征点在帧一帧二中的索引

cv::Mat Hn; 八点法计算出的单应矩阵

2.细节

循环遍历我们选择的 200 × 8 对点（mMaxIterations × 8）

mvSets是<int,int>型对组，其存储着一对匹配的特征点在帧一帧二的索引，我们用vPn1i[j]、vPn2i[j]存储一次迭代中的特征点向量，将特征点向量传入ComputeH21函数：

Compute21函数解析https://blog.csdn.net/qq_41694024/article/details/127824875得到单应矩阵 $H_{21}$ ，但这个单应矩阵是利用归一化的相机坐标算出来的，我们记归一化的相机坐标为 $vPn1$ ，正常坐标为 $mvKeys1$ 。

归一化用算式表达为： $vPn1= T1 \times mvKeys1,vPn2= T2 \times mvKeys2$

单应变换用算式表达为： $vPn2i = H_{21}\times vPn1i$

将两个式子结合：

$T2 \times mvKeys2 = vPn2 = H_{21}\times vPn1i = H_{21}\times T1 \times mvKeys1$

得到相机坐标系（不是归一化后的单应矩阵变换方程）：

$mvKeys2 =T2 ^{-1}\times H_{21}\times T1 \times mvKeys1$

得到相机坐标系点的单应矩阵 $T2 ^{-1}\times H_{21}\times T1$ 后，利用重投影误差为当次RANSAC的结果评分，再一次迭代后，更新具有最优评分的单应矩阵计算结果，并且保存所对应的特征点对的内点标记。

3.2 Initializer::CheckHomography函数解析

3.2.1 函数作用

1.函数作用

对给定的homography matrix打分,需要使用到卡方检验的知识。

2.构造函数

/**
 * @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
 * 
 * @param[in] H21                       从参考帧到当前帧的单应矩阵
 * @param[in] H12                       从当前帧到参考帧的单应矩阵
 * @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckHomography(
    const cv::Mat &H21,                 //从参考帧到当前帧的单应矩阵
    const cv::Mat &H12,                 //从当前帧到参考帧的单应矩阵
    vector<bool> &vbMatchesInliers,     //匹配好的特征点对的Inliers标记
    float sigma)

该函数的输入为从参考帧到当前帧的单应矩阵 $H21$ 与从当前帧到参考帧的单应矩阵
$H12$ ，sigma参数。

该函数的输出为匹配好的特征点对的Inliers标记vbMatchesInliers。返回值为得分数。

3.2.2 函数解析

关于函数中的变量定义：

mvMatches12：在Initializer::Initialize定义，是<int,int>型向量，存储的是第一帧到第二帧的所有特征点的匹配关系。

/**
 * @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
 * 
 * @param[in] H21                       从参考帧到当前帧的单应矩阵
 * @param[in] H12                       从当前帧到参考帧的单应矩阵
 * @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckHomography(
    const cv::Mat &H21,                 //从参考帧到当前帧的单应矩阵
    const cv::Mat &H12,                 //从当前帧到参考帧的单应矩阵
    vector<bool> &vbMatchesInliers,     //匹配好的特征点对的Inliers标记
    float sigma)                        //估计误差
{
    // 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时
    // 其误差加权最小二乘结果为  sum_error = SUM(e(i)^T * Q^(-1) * e(i))
    // 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵
    // 误差加权最小二次结果越小，说明观测数据精度越高
    // 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高
    // 算法目标： 检查单应变换矩阵
    // 检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差

    // 算法流程
    // input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1
    //    do:
    //        for p1(i), p2(i) in mvKeys:
    //           error_i1 = ||p2(i) - H21 * p1(i)||2
    //           error_i2 = ||p1(i) - H12 * p2(i)||2
    //           
    //           w1 = 1 / sigma / sigma
    //           w2 = 1 / sigma / sigma
    // 
    //           if error1 < th
    //              score +=   th - error_i1 * w1
    //           if error2 < th
    //              score +=   th - error_i2 * w2
    // 
    //           if error_1i > th or error_2i > th
    //              p1(i), p2(i) are inner points
    //              vbMatchesInliers(i) = true
    //           else 
    //              p1(i), p2(i) are outliers
    //              vbMatchesInliers(i) = false
    //           end
    //        end
    //   output: score, inliers

	// 特点匹配个数
    const int N = mvMatches12.size();

	// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素
    const float h11 = H21.at<float>(0,0);
    const float h12 = H21.at<float>(0,1);
    const float h13 = H21.at<float>(0,2);
    const float h21 = H21.at<float>(1,0);
    const float h22 = H21.at<float>(1,1);
    const float h23 = H21.at<float>(1,2);
    const float h31 = H21.at<float>(2,0);
    const float h32 = H21.at<float>(2,1);
    const float h33 = H21.at<float>(2,2);

	// 获取从当前帧到参考帧的单应矩阵的各个元素
    const float h11inv = H12.at<float>(0,0);
    const float h12inv = H12.at<float>(0,1);
    const float h13inv = H12.at<float>(0,2);
    const float h21inv = H12.at<float>(1,0);
    const float h22inv = H12.at<float>(1,1);
    const float h23inv = H12.at<float>(1,2);
    const float h31inv = H12.at<float>(2,0);
    const float h32inv = H12.at<float>(2,1);
    const float h33inv = H12.at<float>(2,2);

	// 给特征点对的Inliers标记预分配空间
    vbMatchesInliers.resize(N);

	// 初始化score值
    float score = 0;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
	// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 5.991;

    //信息矩阵，方差平方的倒数
    const float invSigmaSquare = 1.0/(sigma * sigma);

    // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差
    // H21 表示从img1 到 img2的变换矩阵
    // H12 表示从img2 到 img1的变换矩阵 
    for(int i = 0; i < N; i++)
    {
		// 一开始都默认为Inlier
        bool  = true;

		
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        
        const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);
        const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
        const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;
   
        // 计算重投影误差 = ||p1(i) - H12 * p2(i)||2
        const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
        const float chiSquare1 = squareDist1 * invSigmaSquare;

        // Step 2--- 用阈值标记离群点，内点的话累加得分
        if(chiSquare1>th)
            bIn = false;    
        else
            // 误差越大，得分越低
            score += th - chiSquare1;

        // 计算从img1 到 img2 的投影变换误差
        // x1in2 = H21*x1
        // 将图像2中的特征点通过单应变换投影到图像1中
        // |u2|   |h11 h12 h13||u1|   |u1in2|
        // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
        // |1 |   |h31 h32 h33||1 |   |  1  |
		// 计算投影归一化坐标
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;

        // 计算重投影误差 
        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
        const float chiSquare2 = squareDist2*invSigmaSquare;
 
        // 用阈值标记离群点，内点的话累加得分
        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;   

        // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    return score;
}

该函数在一次RANSAC迭代中计算一次。

该函数的主要思想为计算重投影误差：利用在函数Initializer::FindHomography中在一次RANSAC迭代计算出的单应矩阵 $H_{21}$ 将帧1的所有特征点投影到帧2中，然后利用帧2中的与帧1中特征点相匹配的特征点坐标计算方差，再与信息矩阵相乘，得到一个评分。

如果这个评分高于阈值th（重投影误差高）则代表特征点在这一次RANSAC迭代计算出的单应矩阵 $H_{21}$ 匹配不准确，我们认为这对特征点是外点赋予其外点标记即vbMatchesInliers[i] = false。

如果这个评分低于阈值th（重投影误差高）则代表特征点匹配准确，我们认为这对特征点可以计算得分，vbMatchesInliers[i] = true，并且将得分累加。返回上层函数得分。