zl程序教程

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

当前栏目

ORB-SLAM2 --- Tracking::SearchLocalPoints函数解析

函数 解析 --- Tracking ORB SLAM2
2023-09-14 09:01:37 时间

1.函数作用

        用局部地图点进行投影匹配,得到更多的匹配关系。

        局部地图点中已经是当前帧地图点的不需要再投影,只需要将此外的并且在视野范围内的点和当前帧进行投影匹配。

2.函数流程 

Step 1:遍历当前帧的地图点,标记这些地图点不参与之后的投影搜索匹配

Step 2:判断所有局部地图点中除当前帧地图点外的点,是否在当前帧视野范围内

Step 3:如果需要进行投影匹配的点的数目大于0,就进行投影匹配,增加更多的匹配关系

3.code 

void Tracking::SearchLocalPoints()
{
    // Do not search map points already matched
    // Step 1:遍历当前帧的地图点,标记这些地图点不参与之后的投影搜索匹配
    for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        if(pMP)
        {
            if(pMP->isBad())
            {
                *vit = static_cast<MapPoint*>(NULL);
            }
            else
            {
                // 更新能观测到该点的帧数加1(被当前帧观测了)
                pMP->IncreaseVisible();
                // 标记该点被当前帧观测到
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                // 标记该点在后面搜索匹配时不被投影,因为已经有匹配了
                pMP->mbTrackInView = false;
            }
        }
    }

    // 准备进行投影匹配的点的数目
    int nToMatch=0;

    // Project points in frame and check its visibility
    // Step 2:判断所有局部地图点中除当前帧地图点外的点,是否在当前帧视野范围内
    for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        // 已经被当前帧观测到的地图点肯定在视野范围内,跳过
        if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
            continue;
        // 跳过坏点
        if(pMP->isBad())
            continue;
        
        // Project (this fills MapPoint variables for matching)
        // 判断地图点是否在在当前帧视野内
        if(mCurrentFrame.isInFrustum(pMP,0.5))
        {
        	// 观测到该点的帧数加1
            pMP->IncreaseVisible();
            // 只有在视野范围内的地图点才参与之后的投影匹配
            nToMatch++;
        }
    }

    // Step 3:如果需要进行投影匹配的点的数目大于0,就进行投影匹配,增加更多的匹配关系
    if(nToMatch>0)
    {
        ORBmatcher matcher(0.8);
        int th = 1;
        if(mSensor==System::RGBD)   //RGBD相机输入的时候,搜索的阈值会变得稍微大一些
            th=3;

        // If the camera has been relocalised recently, perform a coarser search
        // 如果不久前进行过重定位,那么进行一个更加宽泛的搜索,阈值需要增大
        if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
            th=5;

        // 投影匹配得到更多的匹配关系
        matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
    }
}

4.函数解析 

        一、遍历当前帧的地图点,对于是坏点的地图点将其删除。若该地图点不是坏点,则

        ①该地图点能被观测到的帧数mnVisible加1 

        ②通过mnLastFrameSeen标记位设置为当前帧从而得到该点被当前帧观测到的标记。

        ③标记该点在后面搜索匹配时不被投影的标记mbTrackInView,因为已经有匹配了。

        二、判断所有局部地图点mvpLocalMapPoints中除当前帧地图点外的点,是否在当前帧视野范围内:

        注意:局部地图点在Tracking::UpdateLocalPoints函数中得到

ORB-SLAM2 --- Tracking::UpdateLocalPoints函数解析https://blog.csdn.net/qq_41694024/article/details/128321282

        ①若已经被当前帧观测到的地图点肯定在视野范围内,跳过。(通过mnLastFrameSeen标记判断是否该地图点被当前帧观测)

        ②跳过坏点

        ③判断地图点是否在在当前帧视野内,若该地图点在当前帧的视野中,将观测到该点的帧数加1。(因为我们是一帧帧传进来的,不存在重复计数)

ORB-SLAM2 --- Frame::isInFrustum函数解析https://blog.csdn.net/qq_41694024/article/details/128334862

        三、如果需要进行投影匹配的点的数目nToMatch大于0,就进行投影匹配,增加更多的匹配关系

        如果不久前进行过重定位,那么进行一个更加宽泛的搜索,阈值需要增大。

        我们利用投影匹配法找到更多的匹配关系。

ORB-SLAM2 --- ORBmatcher::SearchByProjection函数解析icon-default.png?t=M85Bhttps://mp.csdn.net/mp_blog/creation/editor/128365489