ORB-SLAM2 --- ORBmatcher::SearchByProjection函数

最新推荐文章于 2023-05-16 09:18:05 发布

APS2023

最新推荐文章于 2023-05-16 09:18:05 发布

阅读量851

点赞数 2

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签：人工智能计算机视觉 slam

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

版权

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

66 篇文章 46 订阅

订阅专栏

1.函数作用

我们在Tracking::SearchLocalPoints函数中调用，目的是找到更多匹配点。

通过投影地图点到当前帧，对Local MapPoint进行跟踪。

2.函数步骤

Step 1 遍历有效的局部地图点
Step 2 设定搜索搜索窗口的大小。取决于视角, 若当前视角和平均视角夹角较小时, r取一个较小的值
Step 3 通过投影点以及搜索窗口和预测的尺度进行搜索, 找出搜索半径内的候选匹配点索引
Step 4 寻找候选匹配点中的最佳和次佳匹配点
Step 5 筛选最佳匹配点
@param[in] F 当前帧
@param[in] vpMapPoints 局部地图点，来自局部关键帧
@param[in] th 搜索范围
@return int 成功匹配的数目

3.code

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)
{
    int nmatches=0;

    // 如果 th！=1 (RGBD 相机或者刚刚进行过重定位), 需要扩大范围搜索
    const bool bFactor = th!=1.0;

    // Step 1 遍历有效的局部地图点
    for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
    {
        MapPoint* pMP = vpMapPoints[iMP];

        // 判断该点是否要投影
        if(!pMP->mbTrackInView)
            continue;

        if(pMP->isBad())
            continue;
            
        // 通过距离预测的金字塔层数，该层数相对于当前的帧
        const int &nPredictedLevel = pMP->mnTrackScaleLevel;

        // The size of the window will depend on the viewing direction
        // Step 2 设定搜索搜索窗口的大小。取决于视角, 若当前视角和平均视角夹角较小时, r取一个较小的值
        float r = RadiusByViewingCos(pMP->mTrackViewCos);
        
        // 如果需要扩大范围搜索，则乘以阈值th
        if(bFactor)
            r*=th;

        // Step 3 通过投影点以及搜索窗口和预测的尺度进行搜索, 找出搜索半径内的候选匹配点索引
        const vector<size_t> vIndices =
                F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY,      // 该地图点投影到一帧上的坐标
                                    r*F.mvScaleFactors[nPredictedLevel],    // 认为搜索窗口的大小和该特征点被追踪到时所处的尺度也有关系
                                    nPredictedLevel-1,nPredictedLevel);     // 搜索的图层范围

        // 没找到候选的,就放弃对当前点的匹配
        if(vIndices.empty())
            continue;

        const cv::Mat MPdescriptor = pMP->GetDescriptor();

        // 最优的次优的描述子距离和index
        int bestDist=256;
        int bestLevel= -1;
        int bestDist2=256;
        int bestLevel2 = -1;
        int bestIdx =-1 ;

        // Get best and second matches with near keypoints
        // Step 4 寻找候选匹配点中的最佳和次佳匹配点
        for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
        {
            const size_t idx = *vit;

            // 如果Frame中的该兴趣点已经有对应的MapPoint了,则退出该次循环
            if(F.mvpMapPoints[idx])
                if(F.mvpMapPoints[idx]->Observations()>0)
                    continue;

            //如果是双目数据
            if(F.mvuRight[idx]>0)
            {
                //计算在X轴上的投影误差
                const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]);
                //超过阈值,说明这个点不行,丢掉.
                //这里的阈值定义是以给定的搜索范围r为参考,然后考虑到越近的点(nPredictedLevel越大), 相机运动时对其产生的影响也就越大,
                //因此需要扩大其搜索空间.
                //当给定缩放倍率为1.2的时候, mvScaleFactors 中的数据是: 1 1.2 1.2^2 1.2^3 ... 
                if(er>r*F.mvScaleFactors[nPredictedLevel])
                    continue;
            }

            const cv::Mat &d = F.mDescriptors.row(idx);

            // 计算地图点和候选投影点的描述子距离
            const int dist = DescriptorDistance(MPdescriptor,d);
            
            // 寻找描述子距离最小和次小的特征点和索引
            if(dist<bestDist)
            {
                bestDist2=bestDist;
                bestDist=dist;
                bestLevel2 = bestLevel;
                bestLevel = F.mvKeysUn[idx].octave;
                bestIdx=idx;
            }
            else if(dist<bestDist2)
            {
                bestLevel2 = F.mvKeysUn[idx].octave;
                bestDist2=dist;
            }
        }

        // Apply ratio to second match (only if best and second are in the same scale level)
        // Step 5 筛选最佳匹配点
        // 最佳匹配距离还需要满足在设定阈值内
        if(bestDist<=TH_HIGH)
        {
            // 条件1：bestLevel==bestLevel2 表示 最佳和次佳在同一金字塔层级
            // 条件2：bestDist>mfNNratio*bestDist2 表示最佳和次佳距离不满足阈值比例。理论来说 bestDist/bestDist2 越小越好
            if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
                continue;

            //保存结果: 为Frame中的特征点增加对应的MapPoint
            F.mvpMapPoints[bestIdx]=pMP; 
            nmatches++;
        }
    }

    return nmatches;
}

4.函数解析

我们回忆一下，在Tracking::SearchLocalPoints函数中，我们遍历每一个局部地图点，看这个地图点是否被当前帧mCurrentFrame发现，若能，就表明这个局部地图点已经有匹配了，我们将这个局部地图点的mbTrackInView属性设置为false。

因此我们这个函数其实就是一个遍历所有局部地图点找到和当前帧匹配的一个过程：

对于每一个没和当前帧进行局部地图点，进行匹配，要是满足一系列条件最后为该该帧F添加对该地图点的观测，F.mvpMapPoints[bestIdx]=pMP; ，最后返回成功匹配成功的题图点的数量。