初始化完成后,对于相机获取当前图像mCurrentFrame,通过跟踪匹配上一帧mLastFrame特征点的方式,可以获取一个相机位姿的初始值;为了兼顾计算量和跟踪鲁棒性,处理了三种模型:

  1. TrackWithMotionModel

  2. TrackReferenceKeyFrame

  3. Relocalization

  这三种跟踪模型都是为了获取相机位姿一个粗略的初值,后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment(捆集调整),进一步优化位姿。

  优先选择通过恒速运动模型,从LastFrame直接预测出(乘以一个固定的位姿变换矩阵)当前帧的姿态;如果是静止状态或者运动模型匹配失效(运用恒速模型后反投影发现LastFrame的地图点和CurrentFrame的特征点匹配很少),通过增大参考帧的地图点反投影匹配范围,获取较多匹配后,计算当前位姿;若这两者均失败,即代表tracking失败,mState!=OK,则在KeyFrameDataBase中用Bow搜索CurrentFrame的特征点匹配,进行全局重定位GlobalRelocalization,在RANSAC框架下使用EPnP求解当前位姿。这三种跟踪模式仅仅跟踪了一帧图像中的特征,没有全局的信息,因此获取的位姿误差较大。

  

  一旦我们通过上面三种模型获取了初始的相机位姿和初始的特征匹配,就可以将完整的地图投影到当前帧中去搜索更多的匹配。但是投影完整的地图,在large scale的场景中是很耗计算而且也没有必要的,因此,这里使用了局部地图LocalMap来进行投影匹配。

  LocalMap包含:与当前帧相连的关键帧K1,以及与K1相连的关键帧K2(一级二级相连关键帧);K1、K2对应的地图点;参考关键帧Kf。

  匹配过程如下:

  1. 抛弃投影范围超出相机画面的;

  2. 抛弃观测视角和地图点平均观测方向相差60o以上的;

  3. 抛弃特征点的尺度和地图点的尺度(通过高斯金字塔层数表示)不匹配的;

  4. 计算当前帧中特征点的尺度;

  5. 将地图点的描述子和当前帧ORB特征的描述子匹配,需要根据地图点尺度在初始位姿获取的粗略x投影位置附近搜索;

  6. 根据所有匹配点进行PoseOptimization优化。 

  

  在处理完mCurrentFrame的跟踪定位后,需要判定当前帧是否可以加入关键帧list:

  以下条件必须同时满足,才可以加入关键帧,但是其实ORB-SLAM中关键帧的加入是比较密集的,这样确保了定位的精度,同时在LocalMapping线程最后会进行关键帧的剔除,又确保了关键帧的数量不会无限增加,不会对large scale的场景造成计算负担。但是,ORB的作者又提到了,Tracking中除了提取特征点,TrackLocalMap也挺耗时,可以通过减少关键帧的数量来降低Local Map的规模,提高Tracking速度(但是精确度可能降低)。

  1. 距离上一次重定位距离至少20帧;

  2. 局部地图线程空闲,或者距离上一次加入关键帧过去了20帧;如果需要关键帧插入(过了20帧)而LocalMapping线程忙,则发送信号给LocalMapping线程,停止局部地图优化,使得新的关键帧可以被及时处理(20帧,大概过去了不到1s)。

  3. 当前帧跟踪至少50个点;确保了跟踪定位的精确度

  4. 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%;确保关键帧之间有明显的视觉变化。

  注意这里只是判断当前帧是否需要加入关键帧,并没有真的加入地图,因为Tracking线程的主要功能是局部定位,而处理地图中的关键帧,地图点,包括如何加入,如何删除的工作是在LocalMapping线程完成的,这里也可以看出作者的思路是比较清楚的,Tracking负责localization,LocalMapping负责Mapping,就构建了粗略的完整SLAM框架,然后加入初始化和闭环检测以及一些可视化模块,形成完整的SLAM。

  PTAM中关键帧的插入仅仅依靠关键帧之间的距离,明显逊于ORB-SLAM的适者生存方法(survival of the fittest)。


 对于RGBD和双目,非关键帧均可以恢复出图像点的深度,,可以创建一些临时地图点,用于普通帧之间跟踪,主要用于恒速运动模型,这些临时地图点仅仅用于MotionModel跟踪,不会加入地图或者关键帧中。运动过程中不断会跟丢地图点,因此需要不断补充。

void Tracking::UpdateLastFrame();

然后根据恒速模型估计当前帧位姿

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

使用匀速模型估计的位姿,将LastFrame中临时地图点投影到当前姿态,在投影点附近根据描述子距离进行匹配(需要>20对匹配,否则匀速模型跟踪失败,运动变化太大时会出现这种情况),然后以运动模型预测的位姿为初值,优化当前位姿,优化完成后再剔除外点,若剩余的匹配依然>=10对,则跟踪成功,否则跟踪失败,需要Relocalization:

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

    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points if in Localization Mode
    UpdateLastFrame();

    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));

    // Project points seen in previous frame
    int th;
    if(mSensor!=System::STEREO)
        th=15;
    else
        th=7;
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

    // If few matches, uses a wider window search
    if(nmatches<20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
    }

    if(nmatches<20)
        return false;

    // Optimize frame pose with all matches
    Optimizer::PoseOptimization(&mCurrentFrame);

    // Discard outliers
    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;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                nmatchesMap++;
        }
    }    

    if(mbOnlyTracking)
    {
        mbVO = nmatchesMap<10;
        return nmatches>20;
    }

    return nmatchesMap>=10;
}
View Code

相关文章:

  • 2021-11-22
  • 2022-01-10
  • 2022-01-15
  • 2021-07-07
  • 2021-07-31
  • 2021-05-12
  • 2021-07-15
猜你喜欢
  • 2021-10-12
  • 2022-01-20
  • 2021-09-23
  • 2021-10-20
  • 2021-12-23
相关资源
相似解决方案