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

目录

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; iIncreaseFound();
                //查看当前是否是在纯定位过程
                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(NULL);

        }
    }

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

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        根据先前得到的更多的匹配关系，优化当前帧的位姿。

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

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