07 跟踪线程 Tracking

算法导航

于 2024-02-27 21:36:18 发布

阅读量605

点赞数 10

分类专栏： ORB-SLAM2 文章标签： ORB-SLAM2 SLAM

本文链接：https://blog.csdn.net/qq_44175983/article/details/136332422

版权

ORB-SLAM2 专栏收录该内容

9 篇文章 0 订阅

订阅专栏

文章目录

- 07 跟踪线程 Tracking

07 跟踪线程 Tracking

7.0 总览

在这里插入图片描述

ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

三种跟踪方式：

参考关键帧跟踪：没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。（与最近一个关键帧匹配）
恒速模型跟踪：大部分时间都用这个跟踪，只利用到了上一帧的信息。（当前帧与上一帧的变换矩阵等于上一帧与上上帧的变换矩阵）
重定位跟踪：跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。（在关键帧词库中匹配）

7.1 跟踪状态

Tracking 类中定义了一个枚举类型 eTrackingState，用于表示跟踪状态。
（关于枚举类型，其成员变量的值在定义后，不能再被修改。一般第一个变量默认为0，后面的依次加1。）

成员变量	含义
`SYSTEM_NOT_READY`	系统没有准备好，一般就是在启动后加载配置文件和词典文件时候的状态
`NO_IMAGES_YET`	还没有接收到图像
`NOT_INITIALIZED`	接收到图像但未初始化成功
`OK`	跟踪成功
`LOST`	跟踪失败

然后用这个枚举类型实例化了两个变量，分别表示当前帧的跟踪状态和上一帧的跟踪状态

成员变量	访问控制	含义
`eTrackingState mState`	`public`	当前帧 `mCurrentFrame` 的跟踪状态
`eTrackingState mLastProcessedState`	`public`	前一帧 `mLastFrame` 的跟踪状态

跟踪状态转移图

7.2 构造函数

从配置文件中读取相关参数，包括相机内参、畸变参数、特征点数量、金字塔层数等参数，以及确定单目、双目或是 RGBD 模式。

7.3 初始化

系统接收到图像后便开始初始化操作，包括两种情况：

单目初始化：MonocularInitialization()
双目/RGBD相机初始化: StereoInitialization()

7.3.1 单目相机初始化: MonocularInitialization()

相关函数/变量

成员函数/变量	访问控制	含义
`void MonocularInitialization()`	`protected`	单目相机初始化
`void CreateInitialMapMonocular()`	`protected`	单目初始化后生成初始地图点
`Initializer* mpInitializer`	`protected`	单目初始化器（自定义类）
`Frame mInitialFrame`	`public`	单目初始化的第一帧
`std::vector<cv::Point3f> mvIniP3D`	`public`	单目初始化中三角化得到的地图点坐标
`std::vector<cv::Point2f> mvbPrevMatched`	`public`	单目初始化参考帧的关键点坐标
`std::vector<int> mvIniMatches`	`public`	记录初始帧中各个特征点与当前帧匹配的特征点索引
`std::vector<int> mvIniLastMatches`	`public`	记录初始帧与上一帧的匹配特征点关系

单目相机初始化条件: 连续两帧间成功三角化超过 100 个点，则初始化成功。

void Tracking::MonocularInitialization() {
    // step1. 若单目初始化器还没创建,则创建初始化器
    if (!mpInitializer) {
        if (mCurrentFrame.mvKeys.size() > 100) {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for (size_t i = 0; i < mCurrentFrame.mvKeysUn.size(); i++)
                mvbPrevMatched[i] = mCurrentFrame.mvKeysUn[i].pt;
            mpInitializer = new Initializer(mCurrentFrame, 1.0, 200);
            fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
            return;
        }
    } else  {
		// step2. 若上一帧特征点足够,但当前帧特征点太少,则匹配失败,删除初始化器
        if ((int) mCurrentFrame.mvKeys.size() <= 100) {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer *>(NULL);
            fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
            return;
        }

		// step3. 在mInitialFrame和mLastFrame间进行匹配搜索
        ORBmatcher matcher(0.9, true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame, mCurrentFrame, mvbPrevMatched, mvIniMatches, 100);

        // step4. 匹配的特征点数目太少,则匹配失败,删除初始化器
        if (nmatches < 100) {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer *>(NULL);
            return;
        }

        // step5. 进行单目初始化
        cv::Mat Rcw; 
        cv::Mat tcw; 
        vector<bool> vbTriangulated;
        if (mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {
            // step6. 单目初始化成功后,删除未成功三角化的匹配点对
            for (size_t i = 0, iend = mvIniMatches.size(); i < iend; i++) {
                if (mvIniMatches[i] >= 0 && !vbTriangulated[i]) {
                    mvIniMatches[i] = -1;
                    nmatches--;
                }
            }
			
            // step7. 创建初始化地图,以mInitialFrame为参考系
            cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);
            Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
            tcw.copyTo(Tcw.rowRange(0, 3).col(3));
            mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
            mCurrentFrame.SetPose(Tcw);
            CreateInitialMapMonocular();
        }
    }
}

具体流程为：
①（未创建单目初始化器）得到用于初始化的第一帧，但初始化需要两帧；

②（已创建单目初始化器）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧；

③ 在初始化帧和当前帧中找到匹配的特征点对；

④ 如果初始化的两帧之间的匹配点太少，重新初始化；

⑤ 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始地图点；

⑥ 删除那些无法进行三角化的匹配点；

⑦ 将三角化得到的3D点包装成地图点

初始化成功后调用 CreateInitialMapMonocular() 将 3D 点包装成地图点，即创建初始化地图。

void Tracking::CreateInitialMapMonocular() {
    // mInitialFrame 和 mCurrentFrame 是最早的两个关键帧
    KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB);  
    KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);  

    // step1. 计算两个关键帧的词袋
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // step2. 将两个关键帧插入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // step3. 处理所有地图点
    for (size_t i = 0; i < mvIniMatches.size(); i++) {
        // 创建并添加地图点
        MapPoint *pMP = new MapPoint(mvIniP3D[i], pKFcur, mpMap);		
        mpMap->AddMapPoint(pMP);
        // 添加关键帧到地图点的观测
        pKFini->AddMapPoint(pMP, i);
        pKFcur->AddMapPoint(pMP, mvIniMatches[i]);
		// 添加地图点到关键帧的观测
        pMP->AddObservation(pKFini, i);
        pMP->AddObservation(pKFcur, mvIniMatches[i]);
        // 计算地图点描述子并更新平均观测数据
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();
    }

    // 基于观测到的地图点,更新关键帧共视图
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // step4. 全局BA: 优化所有关键帧位姿和地图点
    Optimizer::GlobalBundleAdjustemnt(mpMap, 20);

	// step5. 将两帧间的平移尺度归一化(以场景的中值深度为参考)
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f / medianDepth;
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;
    pKFcur->SetPose(Tc2w);

	// step6. 将坐标点尺度也归一化
    vector<MapPoint *> vpAllMapPoints = pKFini->GetMapPointMatches();
    for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++) {
        if (vpAllMapPoints[iMP]) {
            MapPoint *pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
        }
    }

    // step7. 将关键帧插入局部地图,更新归一化后的位姿和地图点坐标  
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);
    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;
    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints = mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;
    mLastFrame = Frame(mCurrentFrame);
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
    mpMap->mvpKeyFrameOrigins.push_back(pKFini);
    
   	// step8. 更新跟踪状态变量mState
    mState = OK;
}

几个点：

① 初始的两帧（参考帧和关键帧）都是关键帧；

② 建立帧和地图点的对应关系：
让帧能看见地图点：

pKFini->AddMapPoint(pMP,i);     // 地图点加入对应的关键帧中
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

让地图点能看见关键帧：

pMP->AddObservation(pKFini,i);   // 该地图点是被哪些关键帧观测到的
pMP->AddObservation(pKFcur,mvIniMatches[i]);

7.3.2 双目/RGBD相机初始化: StereoInitialization()

成员函数	访问控制	意义
`StereoInitialization()`	`protected`	双目/RGBD相机初始化

双目/RGBD 相机有深度信息，可以单帧初始化，所以只要左目图像特征点数量大于500即可进入初始化。

函数 StereoInitialization() 内部既完成了初始化,又构建了初始化局部地图。

void Tracking::StereoInitialization() {
    if (mCurrentFrame.N > 500) {
        
        // 基于当前帧构造初始关键帧
        mCurrentFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
        KeyFrame *pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
        mpMap->AddKeyFrame(pKFini);
        mpLocalMapper->InsertKeyFrame(pKFini);
        // 为每个特征点构造地图点
        for (int i = 0; i < mCurrentFrame.N; i++) {
            float z = mCurrentFrame.mvDepth[i];
            if (z > 0) {
                cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                MapPoint *pNewMP = new MapPoint(x3D,pKFini,mpMap);
			             pNewMP->AddObservation(pKFini, i);  // 表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
                pNewMP->ComputeDistinctiveDescriptors();
                pNewMP->UpdateNormalAndDepth();
				            mpMap->AddMapPoint(pNewMP);
                pKFini->AddMapPoint(pNewMP, i);  // 表示该KeyFrame的哪个特征点可以观测到哪个3D点
				            mCurrentFrame.mvpMapPoints[i] = pNewMP;
            }
        }

		// 构造局部地图
        mvpLocalKeyFrames.push_back(pKFini);
        mvpLocalMapPoints = mpMap->GetAllMapPoints();
        mpReferenceKF = pKFini;
        mCurrentFrame.mpReferenceKF = pKFini;
		
        // 更新跟踪状态变量mState
        mState = OK;
    }
}

7.4 三种跟踪方法–初始位姿估计

当 Tracking 线程接收到一帧图像后，会先估计其初始位姿，再根据估计出的初始位姿跟踪局部地图并进一步优化位姿。

初始位姿估计有三种手段:

根据恒速运动模型估计位姿 TrackWithMotionModel()
根据参考帧估计位姿 TrackReferenceKeyFrame()
通过重定位估计位姿 Relocalization()

7.4.1 恒速运动模型估计位姿 `TrackWithMotionModel()`

大部分时间都是恒速跟踪模式，只需要用到前一帧的信息。也就是根据运动速度 mVelocity 和上一帧的位姿 mLastFrame.mTcw 计算出本帧位姿的估计值。流程：

① 用恒速模型先估计一个初始位姿
② 用该位姿进行投影匹配 SearchByProjection，候选点来自GetFeaturesInArea，未使用BoW
③ BA优化（仅优化位姿），提供比较粗糙的位姿

成员变量 mVelocity 保存前一帧的速度，主函数 Tracking::Track() 中调用完函数 Tracking::TrackLocalMap() 更新局部地图和当前帧位姿后，就计算速度并赋值给 mVelocity。

成员函数/变量	访问控制	意义
`TrackWithMotionModel()`	`protected`	根据恒速运动模型估计初始位姿
`Frame mLastFrame`	`protected`	前一帧，`TrackWithMotionModel()`与该帧匹配搜索关键点
`cv::Mat mVelocity`	`protected`	相机前一帧运动速度,跟踪完局部地图后更新该成员变量
`list<MapPoint*> mlpTemporalPoints`	`protected`	双目/RGBD相机输入时,为前一帧生成的临时地图点跟踪成功后该容器会被清空,其中的地图点会被删除

注意这里的速度 mVelocity 并不是我们平常理解的速度，而是当前帧和上一帧的位姿变换矩阵。我们假设位姿是匀速变化的，也就是说上一帧到当前帧的位姿变换和上上帧到上一帧的位姿变换是相同的，于是就可以用上一帧的位姿与速度 mVelocity 相乘得到当前位姿。那么第一个速度是怎么得到的呢？地图初始化后，首先用参考关键帧跟踪，也就是第三帧和最开始的两个关键帧跟踪，计算相对位姿也就是速度。

bool Tracking::TrackWithMotionModel() {
    ORBmatcher matcher(0.9, true);

    // step1. 更新上一帧的位姿,对于双目/RGBD相机,还会生成临时地图点
    UpdateLastFrame();
    // step2. 根据运动模型设置初始位姿估计值
    mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);
    fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));

    // step3. 根据初始位姿估计值进行投影匹配
    int th;
    if (mSensor != System::STEREO)
        th = 15;//单目
    else
        th = 7;//双目
    // step3.1. 寻找匹配特征点
    int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR);
    // step3.2. 匹配特征点数目太少就放宽条件重新搜索匹配
    if (nmatches < 20) {	
        fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR);
    }
    // step3.3. 实在找不到足够的匹配特征点,运动模型跟踪失败
    if (nmatches < 20)
        return false;

    // step4. 位姿BA: 只优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // step5. 剔除外点
    int nmatchesMap = 0;
    for (int i = 0; i < mCurrentFrame.N; i++) {
        if (mCurrentFrame.mvpMapPoints[i]) {
            if (mCurrentFrame.mvbOutlier[i]) {
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                mCurrentFrame.mvbOutlier[i] = false;
            } else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                nmatchesMap++;
        }
    }

    // step6. 匹配的地图点数超过10个就认为是跟踪成功
    return nmatchesMap >= 10;
}

具体来看 UpdateLastFrame() 函数：
参考：https://blog.csdn.net/qq_41694024/article/details/128192577

7.4.2 根据参考帧估计位姿: TrackReferenceKeyFrame()

两种情况使用参考帧估计位姿：一是，当使用恒速模型匹配到的特征点数较少，跟踪失败时，就会选用关键帧模式跟踪；二是刚初始化时，恒速模型中的速度为空，只能用参考帧估计位姿。

思路是：尝试和最近一个关键帧去做匹配。为了快速匹配，利用了bag of words（BoW）来加速匹配。

成员函数/变量	访问控制	意义
`bool TrackReferenceKeyFrame()`	`protected`	根据关键帧估计位姿
`KeyFrame* mpReferenceKF`	`protected`	参考关键帧，`TrackReferenceKeyFrame()` 与该关键帧匹配搜索关键点

计算当前帧描述子的 Bow 向量，与参考关键帧匹配，匹配点过少则失败。

7.4.3 通过重定位估计位姿: Relocalization()

当 TrackWithMotionModel() 和 TrackReferenceKeyFrame() 都失败后，就会调用函数 Relocalization() 通过重定位估计位姿.

成员函数/变量	访问控制	意义
`bool Relocalization()`	`protected`	重定位
`KeyFrameDatabase* mpKeyFrameDB`	`protected`	关键帧词库

计算当前帧描述子的 Bow 向量；在关键帧词库中寻找候选帧；遍历候选帧，匹配优化。

7.5 跟踪局部地图: TrackLocalMap()

这是跟踪线程的第二阶段，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，从而对第一阶段的位姿再次优化得到相对准确的位姿。

成员函数/变量	访问控制	意义
`bool TrackLocalMap()`	`protected`	更新局部地图并优化当前帧位姿
`void UpdateLocalMap()`	`protected`	更新局部地图
`std::vector<KeyFrame*> mvpLocalKeyFrames`	`protected`	局部关键帧列表
`std::vector<MapPoint*> mvpLocalMapPoints`	`protected`	局部地图点列表
`void SearchLocalPoints()`	`protected`	将局部地图点投影到当前帧特征点上

首先明确几个概念。

7.5.1 几个概念

在这里插入图片描述

当前帧：mCurrentFrame（当前帧是普通帧）；
参考关键帧: 与当前帧共视程度最高的关键帧作为参考关键帧；
父关键帧：和当前关键帧共视程度最高的关键帧；
子关键帧：是上述父关键帧的子关键帧；
一级共视关键帧：能够观测到当前帧 F 中地图点的共视关键帧即 KF1、KF2；
二级共视关键帧：一级共视关键帧的共视关键帧（代码中取前10个共视程度最高的关键帧）；
局部关键帧：包括当前普通帧的一级共视关键帧、二级共视关键帧及其子关键帧和父关键帧；
局部地图点：局部关键帧对应的地图点；
局部地图：局部关键帧对应的地图点构成局部地图。

7.5.2 主要流程

成员函数/变量	访问控制	意义
`bool TrackLocalMap()`	`protected`	更新局部地图并优化当前帧位姿
`void UpdateLocalMap()`	`protected`	更新局部地图
`std::vector<KeyFrame*> mvpLocalKeyFrames`	`protected`	局部关键帧列表
`std::vector<MapPoint*> mvpLocalMapPoints`	`protected`	局部地图点列表
`void SearchLocalPoints()`	`protected`	将局部地图点投影到当前帧特征点上

采用上面三种方法估计完初始位姿后，需要进一步优化，即基于当前位姿更新局部地图并优化当前帧位姿，主要流程：

① 更新局部地图，包括局部关键帧列表 mvpLocalKeyFrames 和局部地图点列表 mvpLocalMapPoints；

将局部地图点投影到当前帧特征点上；

进行位姿BA,优化当前帧位姿；

更新地图点观测数值，遍历当前帧中的所有特征点，对于每个特征点，检查是否有一个地图点与之对应，以及是否被标记为外点，并统计内点个数；

根据内点数判断是否跟踪成功。

详细来看各个子函数

UpdateLocalMap() 调用 UpdateLocalKeyFrames() 和 UpdateLocalPoints() 更新局部关键帧列表和局部地图点列表。

（1）首先是 UpdateLocalKeyFrames() 函数，更新局部关键帧：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧；把当前帧的级共视关键帧、二级共视关键帧及其子关键帧和父关键帧都加入列表并更新。

https://blog.csdn.net/qq_41694024/article/details/128307627

（2）前面的局部关键帧列表更新完毕后，调用此函数：遍历局部关键帧 mvpLocalKeyFrames，对于每个局部关键帧，得到它的所有地图点，存入 mvpLocalMapPoints。

（3）函数 SearchLocalPoints() 用局部地图点进行投影匹配，得到更多匹配关系，已经是当前帧地图点的不需要再投影，除此之外的点如果在当前帧的视野范围内，就进行投影匹配。至于如何判断是否在视野范围内，需调用 Frame::isInFrustum() 函数。

7.6 判断是否需要创建新关键帧：NeedNewKeyFrame()

是否需要生成关键帧，需要考虑以下方面：

（1）纯定位模式不创建关键帧；
（2）如果LocalMapping线程还有很多KeyFrame没处理的话,不适合再给它增加负担了；
（3）距离上次创建关键帧经过的时间: 如果很长时间没创建关键帧了的话,就要抓紧创建关键帧了；
…

总体而言，ORB-SLAM2 插入关键帧的策略还是比较宽松的，因为后面 LocalMapping 线程的函数 LocalMapping::KeyFrameCulling() 会剔除冗余关键帧，因此在系统处理得过来的情况下，要尽量多创建关键帧。

7.7 创建新关键帧: CreateNewKeyFrame()

将当前帧构造成关键帧；
将当前关键帧设置为当前帧的参考关键帧；
对双目或RGB-D相机，为当前帧生成新的 Mappoints，单目无操作。

7.8 其他函数

成员函数/变量	访问控制	意义
`GrabImageStereo()` `GrabImageRGBD` `GrabImageMonocular`	`public`	处理输入图像、特征点提取与匹配以及 `Track()`
`ChangeCalibration()`	`public`	更改相机配置文件后，重新读取参数

7.9 Tracking流程中的关键问题

7.9.1 参考关键帧: mpReferenceKF

参考关键帧的用途：
（1）Tracking::TrackReferenceKeyFrame() 根据参考关键帧估计初始位姿；
（2）用于初始化新创建的 MapPoint 的参考帧 mpRefKF，函数 MapPoint::UpdateNormalAndDepth() 中根据参考关键帧 mpRefKF 更新地图点的平均观测距离。
参考关键帧的指定：
（1）Tracking::CreateNewKeyFrame() 创建完新关键帧后，会将新创建的关键帧设为参考关键帧；
（2）Tracking::TrackLocalMap() 跟踪局部地图过程中调用函数 Tracking::UpdateLocalMap()，其中调用函数 Tracking::UpdateLocalKeyFrames()，将与当前帧共视程度最高的关键帧设为参考关键帧。

算法导航

关注

10
点赞
踩
13

收藏

觉得还不错? 一键收藏
0
评论
07 跟踪线程 Tracking

第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。用局部地图点进行投影匹配，得到更多匹配关系，已经是当前帧地图点的不需要再投影，除此之外的点如果在当前帧的视野范围内，就进行投影匹配。函数，更新局部关键帧：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧；这是跟踪线程的第二阶段，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，从而对第一阶段的位姿再次优化得到相对准确的位姿。
复制链接

扫一扫