ORB-SLAM2从理论到代码实现（六）：Tracking.cc程序详解（上）
终于写到Tracking.cc了，想想还有点小激动呢。
本人邮箱[email protected]，欢迎交流。
本系列代码注释，是在吴博注释的基础上进一步注释。
1.Tracking 框架
Tracking线程流程框图：
各流程对应的主要函数（来自吴博@泡泡机器人）：
Tracking整体流程图（ 来自https://blog.csdn.net/chishuideyu/article/details/75314896）
上面这张图把Tracking.cc讲的特别明白。
tracking线程在获取图像数据后，会传给函数GrabImageStereo、GrabImageRGBD或GrabImageMonocular进行预处理，这里我以GrabImageMonocular为例。
- GrabImageMonocular(const cv::Mat &im, const double ×tamp)
| 函数功能 | 1、将图像转为mImGray并初始化mCurrentFrame，2、进行tracking过程，输出世界坐标系到该帧相机坐标系的变换矩阵 |
| im | 输入图像 |
| timestamp | 时间戳 |
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
{
mImGray = im;//读取图像
// 步骤1：将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
// 步骤2：构造Frame
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
// 步骤3：跟踪
Track();
return mCurrentFrame.mTcw.clone();
}
数据，流到Track（），由于代码超长，我会分几段粘贴注释。
void Tracking::Track()
| 步骤 | 1. 判断tracking状态：如果是未初始化（NOT_INITIALIZED），则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()进行初始化，并更新地图视图。 2.对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断） （1）只进行跟踪不建图 （2）同时跟踪和建图： 初始化之后ORB-SLAM有三种跟踪模型可供选择 a.TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。 b.TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。 c.Relocalization()；重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。 这三个模型的选择方法： 首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪彻底丢失，要么等待相机回转，要不进行重置。
|
| 无参数 |
A.初始化部分
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态
// SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock lock(mpMap->mMutexMapUpdate);
// 步骤1：初始化
if(mState==NOT_INITIALIZED)//判断是否初始化
{
if(mSensor==System::STEREO || mSensor==System::RGBD)//双目或深度相机
StereoInitialization();//双目初始化
else
MonocularInitialization();//单目初始化
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
//。。。。。。。 B.跟踪步骤1.跟踪上一帧或者参考帧或者重定位
else// 步骤2：跟踪
{
// System is initialized. Track Frame.系统完成初始化，跟踪帧
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)运用运动模型或重定位初始化相机位姿估计
// 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
// 正常初始化成功
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// 运动模型是空的或刚完成重定位
// mCurrentFrame.mnId vpMPsMM;//记录地图点
vector vbOutMM;//记录外点
cv::Mat TcwMM;//变换矩阵
if(!mVelocity.empty())//有速度
{
bOKMM = TrackWithMotionModel();//用运动模型追踪
vpMPsMM = mCurrentFrame.mvpMapPoints;//记录地图点
vbOutMM = mCurrentFrame.mvbOutlier;//记录外点
TcwMM = mCurrentFrame.mTcw.clone();//当前帧的变换矩阵
}
bOKReloc = Relocalization();//用重定位
// 重定位没有成功，但是运动模型跟踪成功
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
// 更新当前帧的MapPoints被观测程度
for(int i =0; iIncreaseFound();
}
}
}
}
else if(bOKReloc)// 只要重定位成功整个跟踪过程正常进行（定位与跟踪，更相信重定位）
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
跟踪步骤2.在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿，local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系，在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化。
步骤3：更新恒速运动模型TrackWithMotionModel中的mVelocity
// 将最新的关键帧作为reference frame，接上面代码
mCurrentFrame.mpReferenceKF = mpReferenceKF;
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是Tcl
}
else
mVelocity = cv::Mat();
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
步骤4.清除UpdateLastFrame中为当前帧临时添加的MapPoints
5.清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
6.检测并插入关键帧，对于双目会产生新的MapPoints
// 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
for(int i=0; iObservations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
}
}
// Delete temporal MapPoints
// 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
// 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
for(list::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)我们允许具有高创新点（考虑胡贝尔函数的异常值）
// pass to the new keyframe, so that bundle adjustment will finally decide传递到新的关键帧，这样束调整将最终决定
// if they are outliers or not. We don't want next frame to estimate its position如果它们是外点。我们不希望下一帧估计它的位置。
// with those points so we discard them in the frame.
// 删除那些在bundle adjustment中检测为outlier的3D map点
for(int i=0; i(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟踪失败，并且relocation也没有搞定，只能重新Reset
if(mState==LOST)
{
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一帧的数据
mLastFrame = Frame(mCurrentFrame);
} C.记录位姿信息，用于轨迹复现
if(!mCurrentFrame.mTcw.empty())
{
// 计算相对姿态T_currentFrame_referenceKeyFrame
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
// 如果跟踪失败，则相对位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}未完，待续。。。