ORB-SLAM2源码学习：System.cc: System::TrackStereo、TrackRGBD、TrackMonocular追踪器接口

前言

在之前的单目实例中，我们在初始化一个SLAM的系统后，系统处理了接受的文件数据，初始化并运行了各个线程，之后就要给追踪器传入图像进行整个SLAM的流程了。

1.函数声明

追踪器的函数定义在System源文件下，针对不同的传感器模式，分别实现了三种追踪器的定义。

它们依次是单目、双目、RGBD追踪器。

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
cv::Mat System::TrackStereo(const cv::Mat &imLeft, 		//左侧图像const cv::Mat &imRight, 	//右侧图像const double &timestamp)	//时间戳
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)

2.函数定义

以单目为例：

1.检查传感器

不是指定的传感器内容输出信息提示。

if(mSensor!=MONOCULAR){cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;exit(-1);}

2.改变追踪模式

追踪线程有两个模式，一个是定位建图另一个是纯定位模式，追踪器通过此部分代码来实现模式的改变。

// Check mode change{// 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱unique_lock<mutex> lock(mMutexMode);// mbActivateLocalizationMode为true会关闭局部地图线程if(mbActivateLocalizationMode){mpLocalMapper->RequestStop();// Wait until Local Mapping has effectively stoppedwhile(!mpLocalMapper->isStopped()){usleep(1000);}// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新// 设置mbOnlyTracking为真mpTracker->InformOnlyTracking(true);// 关闭线程可以使得别的线程得到更多的资源mbActivateLocalizationMode = false;}// 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.if(mbDeactivateLocalizationMode){mpTracker->InformOnlyTracking(false);mpLocalMapper->Release();mbDeactivateLocalizationMode = false;}}

3.检查是否有复位操作。

// Check reset{unique_lock<mutex> lock(mMutexReset);if(mbReset){mpTracker->Reset();mbReset = false;}

4.获取相机位姿估计的结果并返回。

//获取相机位姿的估计结果cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);unique_lock<mutex> lock2(mMutexState);mTrackingState = mpTracker->mState;mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;return Tcw;

其他追踪器的代码也差不多，直接列在下面了。

//双目输入时的追踪器接口
cv::Mat System::TrackStereo(const cv::Mat &imLeft, 		//左侧图像const cv::Mat &imRight, 	//右侧图像const double &timestamp)	//时间戳
{//检查输入数据类型是否合法if(mSensor!=STEREO){//不合法那就退出cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;exit(-1);}   //检查是否有运行模式的改变// Check mode change{// TODO 锁住这个变量？防止其他的线程对它的更改？unique_lock<mutex> lock(mMutexMode);//如果激活定位模式if(mbActivateLocalizationMode){//调用局部建图器的请求停止函数mpLocalMapper->RequestStop();// Wait until Local Mapping has effectively stoppedwhile(!mpLocalMapper->isStopped()){usleep(1000);}//运行到这里的时候，局部建图部分就真正地停止了//告知追踪器，现在 只有追踪工作mpTracker->InformOnlyTracking(true);// 定位时，只跟踪//同时清除定位标记mbActivateLocalizationMode = false;// 防止重复执行}//如果激活定位模式if(mbDeactivateLocalizationMode){//如果取消定位模式//告知追踪器，现在地图构建部分也要开始工作了mpTracker->InformOnlyTracking(false);//局部建图器要开始工作呢mpLocalMapper->Release();//清楚标志mbDeactivateLocalizationMode = false;// 防止重复执行}//如果取消定位模式}//检查是否有模式的改变// Check reset，检查是否有复位的操作{//上锁unique_lock<mutex> lock(mMutexReset);//是否有复位请求？if(mbReset){//有，追踪器复位mpTracker->Reset();//清除标志mbReset = false;}//是否有复位请求}//检查是否有复位的操作//用矩阵Tcw来保存估计的相机 位姿，运动追踪器的GrabImageStereo函数才是真正进行运动估计的函数cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);//给运动追踪状态上锁unique_lock<mutex> lock2(mMutexState);//获取运动追踪状态mTrackingState = mpTracker->mState;//获取当前帧追踪到的地图点向量指针mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//获取当前帧追踪到的关键帧特征点向量的指针mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//返回获得的相机运动估计return Tcw;
}//当输入图像 为RGBD时进行的追踪，参数就不在一一说明了
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{//判断输入数据类型是否合法if(mSensor!=RGBD){cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;exit(-1);}    // Check mode change//检查模式改变{unique_lock<mutex> lock(mMutexMode);if(mbActivateLocalizationMode){mpLocalMapper->RequestStop();// Wait until Local Mapping has effectively stoppedwhile(!mpLocalMapper->isStopped()){usleep(1000);}mpTracker->InformOnlyTracking(true);mbActivateLocalizationMode = false;}if(mbDeactivateLocalizationMode){mpTracker->InformOnlyTracking(false);mpLocalMapper->Release();mbDeactivateLocalizationMode = false;}}// Check reset//检查是否有复位请求{unique_lock<mutex> lock(mMutexReset);if(mbReset){mpTracker->Reset();mbReset = false;}}//获得相机位姿的估计cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);unique_lock<mutex> lock2(mMutexState);mTrackingState = mpTracker->mState;mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;return Tcw;
}