ORB-SLAM2 ----- LocalMapping::ComputeF12和ORBmatcher::CheckDistEpipolarLine

文章目录

• 一、函数意义
• 二、LocalMapping::ComputeF12()
• • 1.函数讲解
  • 2.函数代码
• 三、ORBmatcher::CheckDistEpipolarLine()
• • 1.函数讲解
  • 2.函数代码
• 四、总结

一、函数意义

这两个函数在LocalMapping::CreateNewMapPoints()被调用，之所以单独拿出来讲，是因为这两个函都是计算的函数，而且这里是可以用来优化ORB-SLAM2代码的部分，我们完全可以在这部分不用词袋来匹配，直接使用对极约束来进行匹配，匹配效果更好。

二、LocalMapping::ComputeF12()

1.函数讲解

本函数是计算F12矩阵的函数，这个矩阵描述的是从图像1到图像2的变换矩阵。原理是，获取图像1和图像2的旋转矩阵和平移矩阵，然后计算由图2坐标系到图1坐标系的旋转平移计算，这里比较难理解，因为代码直接给出了计算公式，没有给出推到过程，这里我将自己的推到过程放在下图。后续的计算中考虑到t12为31的向量，不好和旋转矩阵以及内参相乘，将其改造为33的矩阵t12x（构造情况见下图）,最终的对极约束加入了相机的内参（最后一图的公式）。经三个图的讲解，大家应该能看懂代码为什么是这样的了。

2.函数代码

// 计算两个关键帧之间的基准矩阵
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
    // 获取第一个关键帧的旋转矩阵和平移向量
    cv::Mat R1w = pKF1->GetRotation();
    cv::Mat t1w = pKF1->GetTranslation();

    // 获取第二个关键帧的旋转矩阵和平移向量
    cv::Mat R2w = pKF2->GetRotation();
    cv::Mat t2w = pKF2->GetTranslation();
    
    // 获取从相机2坐标系到相机1坐标系的旋转矩阵
    cv::Mat R12 = R1w*R2w.t();
    // 相机2坐标系到相机1坐标系的平移向量
    cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;

    // 获取t12的反对称矩阵
    cv::Mat t12x = SkewSymmetricMatrix(t12);
    
    // 获取第一帧相机内参K1和第二帧内参K2
    const cv::Mat &K1 = pKF1->mK;
    const cv::Mat &K2 = pKF2->mK;

    // 返回两个关键帧之间的基础矩阵 F
    return K1.t().inv()*t12x*R12*K2.inv();
}

三、ORBmatcher::CheckDistEpipolarLine()

1.函数讲解

这个函数是用来判断匹配了的点是否符合对极约束的，是一种剔除误匹配点的判断条件，这里构造了一条直线，根据F12讲图2的点转换到图一中，按照理论来说，图一中点的应该在这条直线上（要满足约束），但实际情况是有噪声干扰的，几乎不可能那么准确的落在直线上，这时为了保证误差不太大，计算图1中匹配的点到这个直线的垂直距离，只要他在一定区域内就说明没有误匹配，只是噪声引起的误差，如果超出范围，我们认为是出现了误匹配。这个函数的原理不太难理解，相信大家看了我的讲解后对着代码可以看懂。这个函数最重要的点是，完全可以不用词袋匹配，直接使用对极约束匹配，每个点只找在对极约束内的点进行匹配，然后用直方图的方法来筛选误匹配的点，在修改代码后发现匹配的效果更好了。

2.函数代码

// 实现了一个用于检测两个特征点（关键点）是否满足对极几何约束的函数 CheckDistEpipolarLine
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
    // Epipolar line in second image l = x1'F12 = [a b c]
    const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
    const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
    const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);

    const float num = a*kp2.pt.x+b*kp2.pt.y+c;

    const float den = a*a+b*b;

    if(den==0)
        return false;

    const float dsqr = num*num/den;

    return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
}

四、总结

这两个函数在ORB-SLAM2中看起来不那么重要，只是一个判断条件，但是思想非常的好，为我们提供了一种新的匹配思路，这也是我在学中发现的可以优化的点之一。我认为学习看源码不应该只看别人怎么做的，还需要加入自己的思考，在学习源码的过程中优化源码，在学玩后可以讲代码进行改进。对这部分还有问题或者尤其问题都欢迎交流。