ORB-SLAM2跟踪之估计初始位姿

上一次主要说明了ORB-SLAM2跟踪中提取ORB特征的过程,跟踪的主要接口是Tracking类中的cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);函数,其对输入图像进行预处理,提取ORB特征,并且调用Track();函数。Track()Tracking类的protected函数。

Track()函数首先检测系统是否初始化完成,若完成,则进行下一步工作——估计当前帧的初始位姿。ORB-SLAM支持两个模式,一个是正常的SLAM模式,一个是Localization模式,区别在于是否开启了Local Mapping,Local Mapping会改变地图,所以Localization关闭了Local Mapping。在SLAM模式下,ORB-SLAM2估计初始位姿的方式有两种:通过从前一帧估计在跟踪丢失情况下通过全局重定位估计:

// Tracking.cc  line:302~321
if(mState==OK)
{
  // Local Mapping might have changed some MapPoints tracked in last frame
  CheckReplacedInLastFrame();

  if(mVelocity.empty() || mCurrentFrame.mnId2)
  {
    bOK = TrackReferenceKeyFrame();
  }
  else
  {
    bOK = TrackWithMotionModel();
    if(!bOK)
      bOK = TrackReferenceKeyFrame();
  }
}
else
{
  bOK = Relocalization();
}

从前一帧进行初始位姿的估计

在上一帧成功跟踪后,ORB-SLAM2会假设相机为恒速运动模型来估计当前帧的相机位姿,并且在当前帧中搜索前一帧观察到的地图点时,可以根据该运动来缩小搜索范围。使用Tracking类的protected函数bool TrackWithMotionModel();:

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(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(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);

    // 省略
    ...
}

这个模型假设相机处于匀速运动,那么就可以用上一帧的位姿和速度来估计当前帧的位姿:mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);,其中mTcw是相机的SE(3)姿态,mVelocity是速度模型矩阵。作为备忘,注释解释ORB-SLAM2中关于位姿的设置:

// Frame.cc  line:255~268
void Frame::SetPose(cv::Mat Tcw)
{
    mTcw = Tcw.clone();
    UpdatePoseMatrices();
}

void Frame::UpdatePoseMatrices()
{ 
    // mTcw是SE(3)矩阵, c:camera; w:world
    // 旋转矩阵,world2camera Rotation Matrix
    mRcw = mTcw.rowRange(0,3).colRange(0,3);
    // 旋转矩阵取转置
    mRwc = mRcw.t();
    // 平移矩阵
    mtcw = mTcw.rowRange(0,3).col(3);
    // 相机中心相当于mRcw.t()*(-mtcw)=mRwc*mtwc
    mOw = -mRcw.t()*mtcw;
}

估计完初始化位姿后,调用函数int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);,将上一帧跟踪到的地图点投影到当前帧,然后寻找匹配点,如果匹配点过少,则增大搜索范围。寻找到足够匹配点后,利用匹配点优化估计的初始位姿。

如果根据运动模型得到的匹配点数目少于20,就会利用关键帧进行跟踪,即与关键帧进行匹配。使用Tracking类的protected函数bool TrackReferenceKeyFrame();

bool Tracking::TrackReferenceKeyFrame()
{
    // Compute Bag of Words vector
    mCurrentFrame.ComputeBoW();

    // We perform first an ORB matching with the reference keyframe
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.7,true);
    vector vpMapPointMatches;

    int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

    if(nmatches<15)
        return false;

    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw);

    Optimizer::PoseOptimization(&mCurrentFrame);

    // 省略
    ...
}

其首先计算当前帧的Bow,然后利用Bow来进行匹配:int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);,之后将当前帧的初始位置设置为上一帧的位姿,并利用匹配点进行位姿优化。

通过全局重定位进行初始位姿的估计

当利用恒速运动模型和用关键帧进行跟踪时,匹配点都小于设定的阈值,那么说明跟踪失败,需要全局重定位才能继续跟踪,使用Tracking类的protected函数bool Relocalization();

bool Tracking::Relocalization()
{
    // Compute Bag of Words Vector
    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

    // 省略
    ...

    int nCandidates=0;

    for(int i=0; iif(pKF->isBad())
            vbDiscarded[i] = true;
        else
        {
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            if(nmatches<15)
            {
                vbDiscarded[i] = true;
                continue;
            }
            else
            {
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

    // 省略
    ...


}

首先计算当前的Bow,然后查询关键帧进行匹配,匹配点对不小于15的话,该关键帧作为候选,然后根据ORB匹配点计算关键帧对应的地图点。之后使用PnP算法,利用5 Ransac迭代计算相机位姿并进行优化。如果有足够inlier,那么跟踪继续进行。

通过以上过程,我们可以对每一帧图像估计出其初始的位姿,然后就可以进行跟踪的下一步工作:局部地图跟踪。

在Localization模式下,可能出现当前帧与地图点没有匹配的情况,这个时候只能通过运动模型方法来进行初始位姿的估计,相当于一个视觉里程计。其他时候也是通过运动模型和关键帧来估计初始位姿。

你可能感兴趣的:(ORB-SLAM算法)