上一次主要说明了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模式下,可能出现当前帧与地图点没有匹配的情况,这个时候只能通过运动模型方法来进行初始位姿的估计,相当于一个视觉里程计。其他时候也是通过运动模型和关键帧来估计初始位姿。