提供了编程的基础技术教程

zl程序教程

您现在的位置是：首页 >  其它

当前栏目

ORB-SLAM2 ---- Initializer::FindFundamental

---- ORB SLAM2 initializer
2023-09-14 09:01:37 时间

目录

1.函数作用

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

3.函数解析 

3.1 每次RANSAC迭代中的细节 

3.1.1 八点法计算基础矩阵Initializer::ComputeF21函数的数学基础

3.1.2 八点法计算基础矩阵Initializer::ComputeF21函数解析

3.1.3 每次RANSAC迭代中的细节 

3.2 对给定的Fundamental matrix打分：Initializer::CheckFundamental 函数解析

3.2.1 代码

3.2.2 代码数学解释 

1.函数作用

        计算基础矩阵，假设场景为非平面情况下通过前两帧求取Fundamental矩阵，得到该模型的评分。

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

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

thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
/**
 * @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)

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

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

3.函数解析 

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
    // 计算基础矩阵,其过程和上面的计算单应矩阵的过程十分相似.

    // Number of putative matches
	// 匹配的特征点对总数
    // const int N = vbMatchesInliers.size();  // !源代码出错！请使用下面代替
    const int N = mvMatches12.size();
    // Normalize coordinates
    // Step 1 将当前帧和参考帧中的特征点坐标进行归一化，主要是平移和尺度变换
    // 具体来说,就是将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
    // 这里所谓的一阶绝对矩其实就是随机变量到取值的中心的绝对值的平均值
    // 归一化矩阵就是把上述归一化的操作用矩阵来表示。这样特征点坐标乘归一化矩阵可以得到归一化后的坐标

    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
	// ! 注意这里取的是归一化矩阵T2的转置,因为基础矩阵的定义和单应矩阵不同，两者去归一化的计算也不相同
    cv::Mat T2t = T2.t();

    // Best Results variables
	//最优结果
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
	// 某次迭代中，参考帧的特征点坐标
    vector<cv::Point2f> vPn1i(8);
    // 某次迭代中，当前帧的特征点坐标
    vector<cv::Point2f> vPn2i(8);
    // 某次迭代中，计算的基础矩阵
    cv::Mat F21i;

    // 每次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(int j=0; j<8; j++)
        {
            int idx = mvSets[it][j];

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

        // Step 3 八点法计算基础矩阵
        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);

        // 基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标    
        // 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2  
        // 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0   
        // 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0
        F21i = T2t*Fn*T1;

        // Step 4 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

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

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

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

ORB-SLAM2 ---- Initializer::Normalize函数解析https://blog.csdn.net/qq_41694024/article/details/127821115

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

3.1 每次RANSAC迭代中的细节 

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

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

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

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

3.1.1 八点法计算基础矩阵Initializer::ComputeF21函数的数学基础

        特征点对p_{1},p_{2}用基础矩阵F21来描述特征点对之间的变换关系：

p_{2}^{T}*F_{21}*p_{1}=0

         我们写成矩阵形式：

\begin{bmatrix} u_{2} &v_{2} & 1 \end{bmatrix} \begin{bmatrix} f_{1} & f_{2} & f_{3} \\ f_{4} &f_{5} &f_{6} \\ f_{7} & f_{8} & f_{9} \end{bmatrix} \begin{bmatrix} v_{1}\\ u_{1}\\ 1 \end{bmatrix}=0

         为方便计算先展开前两项得到：

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}

         那么，上面的矩阵可以化为：

\begin{bmatrix} a & b &c \end{bmatrix} \begin{bmatrix} u_{1}\\v_{1} \\ 1 \end{bmatrix}=0

         转化为矩阵形式：

\begin{bmatrix} u_{1}*u_{2} &v_{1}*v_{2} & u_{2} & u_{1}*v_{2} &v_{1}*v_{2} &v_{2} &u_{1} &v_{1} & 1 \end{bmatrix} \begin{bmatrix} f_{1}\\f_{2} \\f_{3} \\ f_{4} \\ f_{5} \\ f_{6} \\ f_{7} \\ f_{8} \\ f_{9} \end{bmatrix}=0

        简记为A_{1\times9}f=0，我们有八对点可以构成A_{8\times9}f=0。

        一对点提供一个约束方程，基础矩阵F总共有9个元素，7个自由度（尺度等价性，秩为2），所以8对点提供8个约束方程就可以求解F。

        SVD分解结果：A = UDV^{T}

        假设我们使用8对点求解，A 是 8\times 9 矩阵，分解后 U 是左奇异向量，它是一个8\times 8的正交矩阵，V是右奇异向量，是一个9\times9的正交矩阵， V^{T}是V的转置 ，D是一个8\times 9对角矩阵，除了对角线其他元素均为0，对角线元素称为奇异值，一般来说奇异值是按照从大到小的顺序降序排列。

        因为每个奇异值都是一个残差项，因此最后一个奇异值最小，其含义就是最优的残差。因此其对应的奇异值向量就是最优值，即最优解。

        V^{T}中的每个列向量对应着D中的每个奇异值，最小二乘最优解就是对应的第9个列向量，也就是基础矩阵F的元素。这里我们先记做F_{pre}，因为这个还不是最终的F：

        F矩阵秩为2，基础矩阵F有个很重要的性质，就是秩为2，可以进一步约束求解准确的F上面的方法使用V^{T}对应的第9个列向量构造的F_{pre}秩通常不为2，我们可以继续进行SVD分解。

F_{pre} = UDV^{T} = U\begin{bmatrix} \sigma _{1} &0 &0 \\ 0 &\sigma _{2} &0 \\ 0 &0 & \sigma _{3} \end{bmatrix}V^{T}

其最小奇异值人为置为0，这样F矩阵秩为2。

F = UDV^{T} = U\begin{bmatrix} \sigma _{1} &0 &0 \\ 0 &\sigma _{2} &0 \\ 0 &0 &0 \end{bmatrix}V^{T}

        此时的F就是最终得到的基础矩阵。

3.1.2 八点法计算基础矩阵Initializer::ComputeF21函数解析

/**
 * @brief 根据特征点匹配求fundamental matrix（normalized 8点法）
 * 注意F矩阵有秩为2的约束，所以需要两次SVD分解
 * 
 * @param[in] vP1           参考帧中归一化后的特征点
 * @param[in] vP2           当前帧中归一化后的特征点
 * @return cv::Mat          最后计算得到的基础矩阵F
 */
cv::Mat Initializer::ComputeF21(
    const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
    const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
{
    // 原理详见附件推导
    // x'Fx = 0 整理可得：Af = 0
    // A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |
    // 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解

	//获取参与计算的特征点对数
    const int N = vP1.size();

	//初始化A矩阵
    cv::Mat A(N,9,CV_32F); // N*9维

    // 构造矩阵A，将每个特征点添加到矩阵A中的元素
    for(int i=0; i<N; i++)
    {
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;

        A.at<float>(i,0) = u2*u1;
        A.at<float>(i,1) = u2*v1;
        A.at<float>(i,2) = u2;
        A.at<float>(i,3) = v2*u1;
        A.at<float>(i,4) = v2*v1;
        A.at<float>(i,5) = v2;
        A.at<float>(i,6) = u1;
        A.at<float>(i,7) = v1;
        A.at<float>(i,8) = 1;
    }

    //存储奇异值分解结果的变量
    cv::Mat u,w,vt;

	
    // 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
	// 转换成基础矩阵的形式
    cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列

    //基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2
    // 对初步得来的基础矩阵进行第2次奇异值分解
    cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

	// 秩2约束，强制将第3个奇异值设置为0
    w.at<float>(2)=0; 
    
    // 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回 
    return  u*cv::Mat::diag(w)*vt;
}

        这个和我们的数学推导如出一辙。

3.1.3 每次RANSAC迭代中的细节 

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

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

        得到一次迭代单应矩阵F_{21}，但这个基础矩阵是利用归一化的相机坐标算出来的，我们记归一化的相机坐标为vPn1，正常坐标为mvKeys1。

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

        基础变换用算式表达为：vPn2i ^{T} \times F_{21}\times vPn1i = 0

        将两个式子结合：

(T2\times mvKeys2)^{T} \times F21 \times T1 \times mvKeys1=0

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

(mvKeys2)^{T}(T2)^{T} \times F21 \times T1 \times mvKeys1=0

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

3.2 对给定的Fundamental matrix打分：Initializer::CheckFundamental 函数解析

3.2.1 代码

/**
 * @brief 对给定的Fundamental matrix打分
 * 
 * @param[in] F21                       当前帧和参考帧之间的基础矩阵
 * @param[in] vbMatchesInliers          匹配的特征点对属于inliers的标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckFundamental(
    const cv::Mat &F21,             //当前帧和参考帧之间的基础矩阵
    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)))的分数就越高
    // 算法目标：检查基础矩阵
    // 检查方式：利用对极几何原理 p2^T * F * p1 = 0
    // 假设：三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p1 和 p2（两个为同名点）
    //   则：p2 一定存在于极线 l2 上，即 p2*l2 = 0. 而l2 = F*p1 = (a, b, c)^T
    //      所以，这里的误差项 e 为 p2 到 极线 l2 的距离，如果在直线上，则 e = 0
    //      根据点到直线的距离公式：d = (ax + by + c) / sqrt(a * a + b * b)
    //      所以，e =  (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)

    // 算法流程
    // input: 基础矩阵 F 左右视图匹配点集 mvKeys1
    //    do:
    //        for p1(i), p2(i) in mvKeys:
    //           l2 = F * p1(i)
    //           l1 = p2(i) * F
    //           error_i1 = dist_point_to_line(x2,l2)
    //           error_i2 = dist_point_to_line(x1,l1)
    //           
    //           w1 = 1 / sigma / sigma
    //           w2 = 1 / sigma / sigma
    // 
    //           if error1 < th
    //              score +=   thScore - error_i1 * w1
    //           if error2 < th
    //              score +=   thScore - 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 f11 = F21.at<float>(0,0);
    const float f12 = F21.at<float>(0,1);
    const float f13 = F21.at<float>(0,2);
    const float f21 = F21.at<float>(1,0);
    const float f22 = F21.at<float>(1,1);
    const float f23 = F21.at<float>(1,2);
    const float f31 = F21.at<float>(2,0);
    const float f32 = F21.at<float>(2,1);
    const float f33 = F21.at<float>(2,2);

	// 预分配空间
    vbMatchesInliers.resize(N);

	// 设置评分初始值（因为后面需要进行这个数值的累计）
    float score = 0;

    // 基于卡方检验计算出的阈值
	// 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值
    // ?是因为点到直线距离是一个自由度吗？
    const float th = 3.841;

    // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float thScore = 5.991;

	// 信息矩阵，或 协方差矩阵的逆矩阵
    const float invSigmaSquare = 1.0/(sigma*sigma);


    // Step 2 计算img1 和 img2 在估计 F 时的score值
    for(int i=0; i<N; i++)
    {
		//默认为这对特征点是Inliers
        bool bIn = true;

	    // Step 2.1 提取参考帧和当前帧之间的特征匹配点对
        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;

        // Reprojection error in second image
        // Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2)
		const float a2 = f11*u1+f12*v1+f13;
        const float b2 = f21*u1+f22*v1+f23;
        const float c2 = f31*u1+f32*v1+f33;
    
        // Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)
        const float num2 = a2*u2+b2*v2+c2;
        const float squareDist1 = num2*num2/(a2*a2+b2*b2);
        // 带权重误差
        const float chiSquare1 = squareDist1*invSigmaSquare;
		
        // Step 2.4 误差大于阈值就说明这个点是Outlier 
        // ? 为什么判断阈值用的 th（1自由度），计算得分用的thScore（2自由度）
        // ? 可能是为了和CheckHomography 得分统一？
        if(chiSquare1>th)
            bIn = false;
        else
            // 误差越大，得分越低
            score += thScore - chiSquare1;

        // 计算img2上的点在 img1 上投影得到的极线 l1= p2 * F21 = (a1,b1,c1)
        const float a1 = f11*u2+f21*v2+f31;
        const float b1 = f12*u2+f22*v2+f32;
        const float c1 = f13*u2+f23*v2+f33;

        // 计算误差 e = (a * p2.x + b * p2.y + c) /  sqrt(a * a + b * b)
        const float num1 = a1*u1+b1*v1+c1;
        const float squareDist2 = num1*num1/(a1*a1+b1*b1);

        // 带权重误差
        const float chiSquare2 = squareDist2*invSigmaSquare;

        // 误差大于阈值就说明这个点是Outlier 
        if(chiSquare2>th)
            bIn = false;
        else
            score += thScore - chiSquare2;
        
        // Step 2.5 保存结果
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    //  返回评分
    return score;
}

3.2.2 代码数学解释 

        我们将第一帧的图像中的特征点投影到第二帧中，由于         

假设：三维空间中的点 P 在 img1 和 img2 两图像上的投影分别为 p_{1} 和 p_{2}（两个为同名点），p_{2}一定存在于极线 l_{2} 上，即 p2\times l2=0 ，而l_{2}=F_{21}\times p_{1} = (a,b,c)^{T}，所以，这里的误差项 e 为 p_{2} 到 极线 l_{2} 的距离，如果在直线上，则 e = 0，但有误差则不可能为0，我们以第一帧所有特征点投影到第二个特征点的位置到极线的举例作为阈值判断条件，根据点到直线的距离公式：

d = \frac{ ax + by + c}{\sqrt{a * a + b * b }}

        所以，e = \frac{a * p2.x + b * p2.y + c}{\sqrt{a * a + b * b}}。

        在我们的代码中和我们推导一致，不再赘述！

相关文章