ORB-SLAM2 —- ORBmatcher::SearchForTriangulation函数

1.推荐先读博客

2.源码解析

2.1 源代码

2.2 关于基础矩阵

2.3 计算KF1的相机中心在KF2图像平面的二维像素坐标

2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配

2.1 源代码

/*
 * @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
 * 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点
 * @param pKF1          关键帧1
 * @param pKF2          关键帧2
 * @param F12           从2到1的基础矩阵
 * @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示
 * @param bOnlyStereo   在双目和rgbd情况下，是否要求特征点在右图存在匹配
 * @return              成功匹配的数量
 */
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector > &vMatchedPairs, const bool bOnlyStereo)
{
    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;

    // Compute epipole in second image
    // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    // KF1相机光心在世界坐标系坐标Cw
    cv::Mat Cw = pKF1->GetCameraCenter();
    // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    cv::Mat R2w = pKF2->GetRotation();
    cv::Mat t2w = pKF2->GetTranslation();
    // KF1的相机光心转化到KF2坐标系中的坐标
    cv::Mat C2 = R2w*Cw+t2w;
    const float invz = 1.0f/C2.at(2);
    // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    const float ex =pKF2->fx*C2.at(0)*invz+pKF2->cx;
    const float ey =pKF2->fy*C2.at(1)*invz+pKF2->cy;

    // Find matches between not tracked keypoints
    // Matching speed-up by ORB Vocabulary
    // Compare only ORB that share the same node

    int nmatches=0;
    // 记录匹配是否成功，避免重复匹配
    vector vbMatched2(pKF2->N,false);
    vector vMatches12(pKF1->N,-1);
    // 用于统计匹配点对旋转差的直方图
    vector rotHist[HISTO_LENGTH];
    for(int i=0;ifirst对应node编号，f1it->second对应属于该node的所有特特征点编号
    DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

    // Step 2.1：遍历pKF1和pKF2中的node节点
    while(f1it!=f1end && f2it!=f2end)
    {
        // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
        if(f1it->first == f2it->first)
        {
            // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];

                // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
                MapPoint* pMP1 = pKF1->GetMapPoint(idx1);

                // If there is already a MapPoint skip
                // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
                if(pMP1)
                    continue;

                // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
                const bool bStereo1 = pKF1->mvuRight[idx1]>=0;

                if(bOnlyStereo)
                    if(!bStereo1)
                        continue;

                // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];

                // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
                const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);

                int bestDist = TH_LOW;
                int bestIdx2 = -1;

                // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
                for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];

                    // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                    MapPoint* pMP2 = pKF2->GetMapPoint(idx2);

                    // If we have already matched or there is a MapPoint skip
                    // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                    if(vbMatched2[idx2] || pMP2)
                        continue;

                    const bool bStereo2 = pKF2->mvuRight[idx2]>=0;

                    if(bOnlyStereo)
                        if(!bStereo2)
                            continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                    const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);

                    // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                    const int dist = DescriptorDistance(d1,d2);

                    if(dist>TH_LOW || dist>bestDist)
                        continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];

                    //? 为什么双目就不需要判断像素点到极点的距离的判断？
                    // 因为双目模式下可以左右互匹配恢复三维点
                    if(!bStereo1 && !bStereo2)
                    {
                        const float distex = ex-kp2.pt.x;
                        const float distey = ey-kp2.pt.y;
                        // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                        // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                        // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                        //? 对这个阈值的有效性持怀疑态度
                        if(distex*distex+distey*disteymvScaleFactors[kp2.octave])
                            continue;
                    }

                    // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                    if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                    {
                        // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                        bestIdx2 = idx2;
                        bestDist = dist;
                    }
                }

                if(bestIdx2>=0)
                {
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                    // 记录匹配结果
                    vMatches12[idx1]=bestIdx2;

                    vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                    nmatches++;

                    // 记录旋转差直方图信息
                    if(mbCheckOrientation)
                    {
                        // angle：角度，表示匹配点对的方向差。
                        float rot = kp1.angle-kp2.angle;
                        if(rot=0 && binfirst < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }

    // Step 3 用旋转差直方图来筛掉错误匹配对
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i

2.2 关于基础矩阵

在第一帧的坐标系下，设

的空间位置为
根据针孔相机模型，两个像素点的像素位置为：，这里为相机内参矩阵，为两个坐标系的相机运动。
(注意：为什么是，因为是世界坐标即在参考系下看见的坐标，但不知道第二个坐标系看到的是什么样子，于是乘以，转化为第二个坐标系即看到的坐标值，那是什么，是，是在参考系下的像素矩阵)
具体来说，这里计算的是和，因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。
有时，我们会使用齐次坐标表示像素点。在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如，和成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为 尺度意义下相等(equal up to a scale)，记作：
那么，上述两个投影关系可写为：
现在取
这里的是两个像素点的 归一化平面上的坐标。代入上式，得
两边同时左乘。回忆^的定义，这相当于两侧同时与做外积：
两侧同时左乘：
等式左侧，是一个与和都垂直的向量。它再和做内积时，将得到0。
由于等式左侧严格为零，乘以任意非零常数之后也为零，于是我们可以把写成通常的等号。因此，我们就得到：
重新带入有
这两个式子都称为对极约束。它的几何意义是三者共面。
对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵： 基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，于是可以进一步简化对极约束：

3.总结

对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：
1.根据配对点的像素位置求出

或者。
2.根据或者求出。
由于和只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的。我们以为例，介绍上面两个问题如何求解。

4.本质矩阵的特性

根据定义，本质矩阵

。它是一个3×3的矩阵，内有9个未知数。那么，是不是任意一个3×3的矩阵都可以被当成本质矩阵呢？
本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足。我们把这件事情称为 E在不同尺度下是等价的。
根据，可以证明，本质矩阵的奇异值必定是的形式。这称为本质矩阵的内在性质。
另外，由于平移和旋转各有3个自由度，故共有6个自由度。但由于尺度等价性，故实际上有5个自由度。

5.八点法求本质矩阵

具有5个自由度的事实，表明我们最少可以用5对点来求解。但是，的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计——这就是经典的 八点法（Eight-point-algorithm ) 。八点法只利用了的线性性质，因此可以在线性代数框架下求解。下面我们来看八点法是如何工作的：
考虑一对匹配点，它们的归一化坐标为。根据对极约束，有

我们把矩阵

展开，写成向量的形式：
那么，对极约束可以写成与有关的线性形式： ​​​​​

同理，对于其他点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组(表示第个特征点,依此类推)：

这8个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为8×9。
位于该矩阵的零空间中。如果系数矩阵是满秩的(即秩为8)，那么它的零空间维数为1，也就是构成一条线。这与的尺度等价性是一致的。如果8对匹配点组成的矩阵满足秩为8的条件，那么的各元素就可由上述方程解得。

6.根据本质矩阵估计相机运用参数R,t

接下来的问题是如何根据已经估得的本质矩阵E,恢复出相机的运动

。这个过程是由奇异值分解（SVD）得到的。设E的SVD为：
其中为正交阵，为奇异值矩阵。根据的内在性质，我们知道。在SVD分解中，对于任意一个，存在两个可能的与它对应：

其中，表示沿轴旋转90°得到旋转矩阵。同时，由于和等价，所以对任意一个取负号，也会得到同样的结果。因此，从分解到时，一共存在4个可能的解。
下图展示了分解本质矩阵得到的4个解。我们已知空间点在相机(蓝色线)上的投影(红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况:

只有第一种解中P在两个相机中都具有正的深度。因此，只要把任意一点代入4种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。
如果利用E的内在性质，那么它只有5个自由度。所以最少可以通过5对点来求解相机运动。然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从8对减至5对意义并不明显。为保持简单，我们这里就只介绍基本的八点法。
剩下的一个问题是：根据线性方程解出的

，可能不满足的内在性质——它的奇异值不一定为的形式。这时，我们会刻意地把矩阵调整成上面的样子。通常的做法是，对八点法求得的进行SVD，会得到奇异值矩阵，不妨设。取:
​​​​​​​

这相当于是把求出来的矩阵投影到了所在的流形上。当然，更简单的做法是将奇异值矩阵取成，因为具有尺度等价性，所以这样做也是合理的。

2.3 计算KF1的相机中心在KF2图像平面的二维像素坐标

    const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
    const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;

    // Compute epipole in second image
    // Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
    // KF1相机光心在世界坐标系坐标Cw
    cv::Mat Cw = pKF1->GetCameraCenter();
    // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
    cv::Mat R2w = pKF2->GetRotation();
    cv::Mat t2w = pKF2->GetTranslation();
    // KF1的相机光心转化到KF2坐标系中的坐标
    cv::Mat C2 = R2w*Cw+t2w;
    const float invz = 1.0f/C2.at(2);
    // 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
    const float ex =pKF2->fx*C2.at(0)*invz+pKF2->cx;
    const float ey =pKF2->fy*C2.at(1)*invz+pKF2->cy;

    // Find matches between not tracked keypoints
    // Matching speed-up by ORB Vocabulary
    // Compare only ORB that share the same node

    int nmatches=0;
    // 记录匹配是否成功，避免重复匹配
    vector vbMatched2(pKF2->N,false);
    vector vMatches12(pKF1->N,-1);
    // 用于统计匹配点对旋转差的直方图
    vector rotHist[HISTO_LENGTH];
    for(int i=0;i

我们计算KF1的相机中心在KF2图像平面的二维像素坐标即e2。步骤如下：
1.获取KF1相机光心在世界坐标系坐标Cw。
2.获取KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系。
3.计算KF1的相机光心转化到KF2坐标系中的坐标C2。
4.计算极点坐标ex，ey。

2.4 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配

// Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配
    // FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历
    // FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}
    // f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号
    DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
    DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
    DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
    DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();

    // Step 2.1：遍历pKF1和pKF2中的node节点
    while(f1it!=f1end && f2it!=f2end)
    {
        // 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
        if(f1it->first == f2it->first)
        {
            // Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
            for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];

                // Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
                MapPoint* pMP1 = pKF1->GetMapPoint(idx1);

                // If there is already a MapPoint skip
                // 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
                if(pMP1)
                    continue;

                // 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
                const bool bStereo1 = pKF1->mvuRight[idx1]>=0;

                if(bOnlyStereo)
                    if(!bStereo1)
                        continue;

                // Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];

                // 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
                const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);

                int bestDist = TH_LOW;
                int bestIdx2 = -1;

                // Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
                for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];

                    // 通过特征点索引idx2在pKF2中取出对应的MapPoint
                    MapPoint* pMP2 = pKF2->GetMapPoint(idx2);

                    // If we have already matched or there is a MapPoint skip
                    // 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
                    if(vbMatched2[idx2] || pMP2)
                        continue;

                    const bool bStereo2 = pKF2->mvuRight[idx2]>=0;

                    if(bOnlyStereo)
                        if(!bStereo2)
                            continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
                    const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);

                    // Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
                    const int dist = DescriptorDistance(d1,d2);

                    if(dist>TH_LOW || dist>bestDist)
                        continue;

                    // 通过特征点索引idx2在pKF2中取出对应的特征点
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];

                    //? 为什么双目就不需要判断像素点到极点的距离的判断？
                    // 因为双目模式下可以左右互匹配恢复三维点
                    if(!bStereo1 && !bStereo2)
                    {
                        const float distex = ex-kp2.pt.x;
                        const float distey = ey-kp2.pt.y;
                        // Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
                        // 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
                        // 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
                        //? 对这个阈值的有效性持怀疑态度
                        if(distex*distex+distey*disteymvScaleFactors[kp2.octave])
                            continue;
                    }

                    // Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
                    if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
                    {
                        // bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
                        bestIdx2 = idx2;
                        bestDist = dist;
                    }
                }

                if(bestIdx2>=0)
                {
                    const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
                    // 记录匹配结果
                    vMatches12[idx1]=bestIdx2;

                    vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！
                    nmatches++;

                    // 记录旋转差直方图信息
                    if(mbCheckOrientation)
                    {
                        // angle：角度，表示匹配点对的方向差。
                        float rot = kp1.angle-kp2.angle;
                        if(rot=0 && binfirst < f2it->first)
        {
            f1it = vFeatVec1.lower_bound(f2it->first);
        }
        else
        {
            f2it = vFeatVec2.lower_bound(f1it->first);
        }
    }

我们建立两帧图像的FeatureVector迭代器，因为只有同一个node节点特征点才可能进行匹配，于是我们先判断帧一的初始节点起始位置和帧二的起始节点是否相同，若不同，执行else if或者else循环先使迭代器指向同一节点位置。
然后依次遍历该node_id下的每个特征点，先取出该特征点的索引值idx1，然后根据索引值一次取得地图点pMP1和特征点kp1，然后取出该特征点的描述子d1，对第二帧也是如此进行，然后计算两特征点的描述子满足阈值要求取出特征点进行一系列的判断。然后就和之前的一样了

Original: https://blog.csdn.net/qq_41694024/article/details/126332164
Author: Courage2022
Title: ORB-SLAM2 —- ORBmatcher::SearchForTriangulation函数