试图学会ORB-SLAM2(2)——System类

System类我认为可以看作是整个ORB-SLAM2的一个入口，在主函数里会初始化一个System类来完成系统的初始化，包括Tracking线程的初始化、LocalMapping和LoopClosing线程的初始化和调用，下边分别对其中的一些变量和函数进行说明。

成员变量

变量名	访问控制	简单解释
eSensor mSensor	private	传感器类型(单目、双目或RGBD)
ORBVocabulary* mpVocabulary	private	指针指向的是ORB字典
KeyFrameDatabase* mpKeyFrameDatabase	private	指针指向关键帧数据库,这个数据库用于重定位和回环检测
Map* mpMap	private	指针指向地图数据库
Tacking* mpTracker	private	追踪器，除了进行运动追踪外还要负责创建关键帧、创建新地图点和进行重定位的工作
LocalMapping* mpLocalMapper	private	局部建图
LoopClosing* mpLoopCloser	private	回环检测
Viewer* mpViewer	private	可视化界面
FrameDrawer* mpFrameDrawer	private	帧绘制器
MapDrawer* mpMapDrawer	private	地图绘制器
std::thread* mptLocalMapping std::thread* mptLoopClosing std::thread* mptViewer	private	创建的局部建图线程、回环检测线程以及可视化的线程
std::mutex mMutexReset bool mbReset	private	复位的标志符
std::mutex mMutexMode bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode	private	模式是否改变的标志符，局部建图线程是否开启以及是否释放
int mTrackingState std::vector<MapPoint*> mTrackedMapPoints std::vector<cv::KeyPoint> mTrackedKeyPointsUn std::mutex mMutexState	private	追踪状态的标志

成员函数

构造函数

System::System(const string &strVocFile,					    //词典文件路径
			   const string &strSettingsFile,				    //配置文件路径
			   const eSensor sensor,						    //传感器类型
               const bool bUseViewer):						    //是否使用可视化界面
					 mSensor(sensor), 							//初始化传感器类型
					 mpViewer(static_cast<Viewer*>(NULL)),		//可视化对象的指针
					 mbReset(false),							//复位标识符，默认为false
					 mbActivateLocalizationMode(false),			//默认启动局部建图线程
        			 mbDeactivateLocalizationMode(false)		//默认启动局部建图线程

构造函数使用了OpenCV的读取配置文件的函数cv::FileStorage，但这里只是对文件能否打开进行了一个判断

cv::FileStorage fsSettings(strSettingsFile.c_str(), 	//将配置文件名转换成为字符串
    					   cv::FileStorage::READ);		//只读
//如果打开失败，就输出调试信息
if(!fsSettings.isOpened())
{
	cerr << "Failed to open settings file at: " << strSettingsFile << endl;
	//然后退出
	exit(-1);
}

之后新建了一个ORB字典，调用ORBVocabulary类中的loadFromTextFile函数来读取字典文件，这里也是对是否加载成功进行一个判断(字典比较大，在运行中通常要加载个几秒)。
之后又用得到的ORB字典初始化了关键帧数据库mpKeyFrameDatabase，主要是在重定位和回环检测中使用。
之后就是初始化了地图类mpMap，mpFrameDrawer等等，这里就不一一赘述，总之就是成员变量都会被初始化。

跟踪函数(以单目TrackMonocular为例)

函数名	参数
TrackMonocular	const cv::Mat &im 图像 const double &timestamp时间戳
TrackStereo	const cv::Mat &imLeft 左目图像 const cv::Mat &imRight 右目图像 const double &timestamp时间戳
TrackRGBD	const cv::Mat &im彩色图像 const cv::Mat &depthmap深度图像 const double &timestamp时间戳

三个函数分别对应着三种传感器的跟踪函数，也是调用跟踪功能的主体，与局部建图和回环检测不同，跟踪功能直接在主函数中被调用。
以TrackMonocular为例，首先对模式是否改变进行判断，这里主要是根据mbActivateLocalizationMode和mbDeactivateLocalizationMode判断是否是仅定位模式，以及局部建图线程是否需要关闭或释放

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

之后会对mbReset进行判断，如果需要复位，就调用Reset函数来将各个线程终止复位。
之后调用Tracking中的GrabImageMonocular，来实现跟踪功能，并对跟踪状态、地图点、特征点等变量赋值。

// 跟踪功能主体
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
unique_lock<mutex> lock2(mMutexState);
// 把状态、地图点、特征点等赋值，方便其他线程使用？
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

ShutDown关闭系统

ShutDown函数就是把各个线程都请求停止

//对局部建图线程和回环检测线程发送终止请求
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
//如果使用了可视化窗口查看器
if(mpViewer)
{
	//向查看器发送终止请求
	mpViewer->RequestFinish();
	//等到，知道真正地停止
	while(!mpViewer->isFinished())
	usleep(5000);
}
// Wait until all thread have effectively stopped
while(!mpLocalMapper->isFinished() || 
	  !mpLoopCloser->isFinished()  || 
       mpLoopCloser->isRunningGBA())			
{
	usleep(5000);
}

if(mpViewer)
// 如果使用了可视化的窗口查看器执行这个
// 看一些博客里提到如果Pangolin版本冲突可能在这里卡住，感觉注释掉也没啥问题
pangolin::BindToContext("ORB-SLAM2: Map Viewer");

保存结果文件函数

函数名	对应格式	参数
SaveTrajectoryTUM	TUM	const string &filename
SaveKeyFrameTrajectoryTUM	TUM	const string &filename
SaveTrajectoryKITTI	KITTI	const string &filename

// TUM轨迹
// 之前帧的轨迹都是相对参考关键帧的，这里恢复到相对原点
//获取其对应的参考关键帧
KeyFrame* pKF = *lRit;

//变换矩阵的初始化，初始化为一个单位阵
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);

// If the reference keyframe was culled（剔除）, traverse（扫描？） the spanning tree to get a suitable keyframe.
while(pKF->isBad())
{
	//更新关键帧变换矩阵的初始值，
	Trw = Trw*pKF->mTcp;
	//并且更新到原关键帧的父关键帧
	pKF = pKF->GetParent();
}//查看当前使用的参考关键帧是否为bad

//最后一个Two是原点校正
//最终得到的是参考关键帧相对于世界坐标系的变换
Trw = Trw*pKF->GetPose()*Two;

//在此基础上得到相机当前帧相对于世界坐标系的变换
cv::Mat Tcw = (*lit)*Trw;
//然后分解出旋转矩阵
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
//以及平移向量
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);

//用四元数表示旋转
vector<float> q = Converter::toQuaternion(Rwc);
// 然后输出
f << setprecision(6) << *lT << " " 			// 时间戳
<<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) // 平移
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; // 四元数表示旋转

// TUM关键帧轨迹
f << setprecision(6) << pKF->mTimeStamp 	// 时间戳
<< setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2) // 平移
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; // 四元数表示旋转

// KITTI轨迹
// 和保存TUM轨迹一样，也有一个原点矫正的过程
// KITTI 这里的输出就是按变换矩阵T=[R|t]那样输出的
f << setprecision(9) 
  << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1)  << " " << Rwc.at<float>(0,2)    	  << " "  << twc.at<float>(0) << " " 
  << Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1)  << " " <<Rwc.at<float>(1,2) << " "  << twc.at<float>(1) << " " 
  << Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1)  << " " << Rwc.at<float>(2,2) << " "  << twc.at<float>(2) << endl;