ORB-SLAM2 --- Tracking::TrackLocalMap函数

news2025/1/14 0:51:57

1.函数作用

2.函数步骤

3.code

4.函数解释

ORB-SLAM2 --- Tracking::UpdateLocalKeyFrames函数https://blog.csdn.net/qq_41694024/article/details/128307627

1.函数作用

在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿。
前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化。

2.函数步骤

@brief 用局部地图进行跟踪，进一步优化位姿
1. 更新局部地图，包括局部关键帧和关键点
2. 对局部MapPoints进行投影匹配
3. 根据匹配对估计当前帧的姿态
4. 根据姿态剔除误匹配
@return true if success
Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
Step 2：在局部地图中查找与当前帧匹配的MapPoints，其实也就是对局部地图点进行跟踪
Step 3：更新局部所有MapPoints后对位姿再次优化
Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
Step 5：决定是否跟踪成功

3.code

bool Tracking::TrackLocalMap()
{
    // We have an estimation of the camera pose and some map points tracked in the frame.
    // We retrieve the local map and try to find matches to points in the local map.

    // Update Local KeyFrames and Local Points
    // Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
    UpdateLocalMap();

    // Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
    SearchLocalPoints();

    // Optimize Pose
    // 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，
    // Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = 0;

    // Update MapPoints Statistics
    // Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            // 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1
            if(!mCurrentFrame.mvbOutlier[i])
            {
                // 找到该点的帧数mnFound 加 1
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                //查看当前是否是在纯定位过程
                if(!mbOnlyTracking)
                {
                    // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1
                    // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                        mnMatchesInliers++;
                }
                else
                    // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果
                    mnMatchesInliers++;
            }
            // 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点
            // ?单目就不管吗
            else if(mSensor==System::STEREO)  
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

        }
    }

    // Decide if the tracking was succesful
    // More restrictive if there was a relocalization recently
    // Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功
    // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
        return false;

    //如果是正常的状态话只要跟踪的地图点大于30个就认为成功了
    if(mnMatchesInliers<30)
        return false;
    else
        return true;
}

4.函数解释

首先更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints：

ORB-SLAM2 --- Tracking::UpdateLocalKeyFrames函数https://blog.csdn.net/qq_41694024/article/details/128307627

ORB-SLAM2 --- Tracking::UpdateLocalPoints函数https://blog.csdn.net/qq_41694024/article/details/128321282ORB-SLAM2 --- Tracking::SearchLocalPoints函数解析https://blog.csdn.net/qq_41694024/article/details/128334584 根据先前得到的更多的匹配关系，优化当前帧的位姿。

更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目。

最后判断匹配的地图点是否足够。