3.7ORB-SLAM3之跟踪局部地图

总述

之前讲，在tracking线程中跟踪主要分为两个阶段，一阶段是帧间跟踪包括跟踪恒速运动模型以及跟踪参考关键帧，这两种跟踪方式都是在两帧之间进行，而跟踪局部地图则是将当前时刻能够被当前帧观测到的地图点进行投影匹配，然后对局部地图中包含的关键帧进行位姿的BA优化。

在ORB-SLAM中，局部建图线程的作用之一就是为了跟踪局部地图而服务的，该线程和跟踪线程同时启动并列运行。局部建图线程时刻维护一个局部地图，这个局部地图包括和当前帧存在共视关系的关键帧，共视关键帧的共视关键帧、这些关键帧对应的地图点。搭载相机的智能体每经过一帧，局部建图线程均会根据环境的情况实时地更新局部地图。

1.更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

函数：UpdateLocalMap()

void Tracking::UpdateLocalMap()
{
    // This is for visualization
    // 设置参考地图点用于绘图显示局部地图点（红色）
    mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints);

    // Update
    // 用共视图来更新局部关键帧和局部地图点
    UpdateLocalKeyFrames();
    UpdateLocalPoints();
}

1.1更新局部地图的关键帧UpdateLocalKeyFrames

更新规则：方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames

Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧,将关键帧的信息记录在map容器keyframeCounter中，first为关键帧，second为该关键帧能观测到当前帧地图点的数量。

Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有以下几种类型

• 类型1：能观测到当前帧地图点的关键帧作为局部关键帧(一级共视关键帧）
• 类型2:一级共视关键帧的共视关键帧(二级共视关键帧),先从二级共视关键帧中挑选共视程度最大的前10个，在排除无效关键帧后再选择共视程度最高的那个。
• 类型3:将一级共视关键帧的子关键帧作为局部关键帧
• 类型4:将一级共视关键帧的父关键帧
• 类型5:当前帧的往前数20个连续的关键帧(仅IMU模式)

要注意的是，这里最高只需要80个关键帧，对于类型1中的关键帧全部都要，不过类型1不满80则按顺序从类型2至类型5中进行挑选

存在父子关系的关键帧是指共视程度最高的两个关键帧，扩展树就是由父子关键帧构成

Step 3：更新当前帧的参考关键帧，与当前图像帧共视程度最高的关键帧作为参考关键帧

1.2更新局部地图的3D地图点UpdateLocalPoints

过程较为简单，首先清空当前局部地图中所有的地图点，然后将更新后的局部地图关键帧中的地图点进行添加进去。

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

函数：SearchLocalPoints()

第一步首先要排除当前帧对应的地图点(在之前的帧间跟踪已经投影匹配过了)，以及不在当前帧视野范围内的地图点

第二步确定匹配时的搜索半径，这里搜索半径的确定有点复杂,整体的思路就是当前的跟踪状态越稳定搜索半径越小，反之搜索半径越大

• 默认情况下th=1;
• RGBD相机th=3;
• 当前子地图已经完成了两次IMU参与的BA优化th=2;
• 当前子地图已经完成了一次IMU参与的BA优化th=6;
• IMU模式下但是没有完成IMU的初始化th=10;
• 如果刚刚重定位th=5
• 如果当前跟踪的状态机处于LOST或者RENSENT_LOST状态th=15

第三步将局部地图点投影到当前帧，寻找投影匹配

int matches = matcher.SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th, mpLocalMapper->mbFarPoints, mpLocalMapper->mThFarPoints);

3.基于非线性优化估计当前帧的位姿

根据系统此时的状态会进入不同的优化函数中：

情况一：如果是纯视觉模式或者IMU还没初始化，则仅对位姿进行优化

    if (!mpAtlas->isImuInitialized())
        Optimizer::PoseOptimization(&mCurrentFrame);

情况二：IMU初始化了但是刚刚初始化不久(1s以内)，仅对位姿进行优化

if(mCurrentFrame.mnId<=mnLastRelocFrameId+mnFramesToResetIMU)
{
    Verbose::PrintMess("TLM: PoseOptimization ", Verbose::VERBOSITY_DEBUG);
    Optimizer::PoseOptimization(&mCurrentFrame);
}

情况三：如果距离IMU初始化已经过去足够久(1s)，考虑视觉和IMU的联合优化

• 如果此时地图已经进行了更新，则使用上一关键帧和当前帧的视觉以及IMU信息优化当前帧对应的位姿、速度和IMU偏置
• 如果此时地图还未进行更新，则使用上一图像帧和当前帧的视觉以及IMU信息优化当前帧对应的位姿、速度和IMU偏置

if(!mbMapUpdated) //  && (mnMatchesInliers>30))
 {
     Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG);
     // 使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
     inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1());
 }
 else
 {
     Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG);
     // 使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
     inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1());
 }

这里的mbMapUpdated是地图更新的标志，在以下几种情况下发生：

• 回环或融合
• 局部地图LocalBundleAdjustment
• IMU三阶段的初始化

4判断是否跟踪成功

最终根据内点的数量和当前跟踪线程的状态机判断当前跟踪局部地图环节是否成功，不同的跟踪状态对成功匹配的内点数量要求不同。

• 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪

if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers < 50)
    return false;

• RECENTLY_LOST状态下，至少成功跟踪10个才算成功

if((mnMatchesInliers>10)&&(mState==RECENTLY_LOST))
    return true;

• 单目IMU模式下做完初始化至少成功跟踪15个才算成功，没做初始化需要50个

 if (mSensor == System::IMU_MONOCULAR)
 {
     if((mnMatchesInliers<15 && mpAtlas->isImuInitialized())||(mnMatchesInliers<50 && !mpAtlas->isImuInitialized()))
         return false;
     else
         return true;
 }

• 双目imu或者RGBD_IMU模式，只要和局部地图点匹配数目大于15就可以了

 else if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
 {
     if(mnMatchesInliers<15)
     {
         return false;
     }
     else
         return true;
 }

• 纯视觉模式需要匹配点大于30

else
{
    if(mnMatchesInliers<30)
        return false;
    else
        return true;
}