ORB-SLAM2源码学习：System.cc源文件

一.提醒

1.这部分代码内容过多，我就不做逻辑分析了（其实是我懒），不过大家可配合我之前发的头文件System.h进行学习（效果不减），如果对大家有帮助请点赞收藏支持一下吧，我会持续更新的。

2.同样的如果有疑问和错误请在评论区指出。

二.已标注的代码

满满的代码为您呈上，你就学吧。（一学一个不吱声）

#include "System.h"
#include "Converter.h"
#include <thread>
#include <pangolin/pangolin.h>
#include <iomanip>

namespace ORB_SLAM2
{

//System 类的构造函数的声明（接受4个函数）及其初始化列表。
/*
1.strVocFile  ORB 词汇表（用于特征描述和匹配）的文件路径。
2.strSettingsFile  配置文件路径，包含相机参数和其他系统设置。
3.sensor  枚举类型 eSensor，表示传感器类型 对应system类声明的公共接口处（public）。
4.bUseViewer  布尔值，是否启用可视化界面来查看 SLAM 结果。
*/
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
        mbDeactivateLocalizationMode(false)
//static_cast 是 C++ 中的一种类型转换运算符，提供了一个安全的方式来进行类型转换，相比于 C 风格的强制类型转换（如 (Type)expression），
//static_cast 更加严格，能够在编译时检查转换是否合法，从而避免潜在的错误。
{

    // Output welcome message
    // 输出一些版权信息，说明软件的开放性和自由分发的许可。
    // 该部分文本输出用到了拼接，即可以分成多行输出
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;

    // 传感器类型设置
    cout << "Input sensor was set to: ";

    // 根据传感器类型的选择，输出用户当前使用的是哪种传感器（单目、双目或 RGB-D）。
    if(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;

    //Check settings file
    // 读取配置文件
    /*
      FileStorage 是 OpenCV 提供的一个类，用于读取和写入 XML、YAML 或 JSON 格式的文件。
      cv::FileStorage::READ：指定打开文件的模式为读取模式。此外还有写入（WRITE）、附加（APPEND）模式打开文件。
      创建一个对象fsSettings：将传入的 strSettingsFile转换为 （.c_str()）C 风格的字符串指针
      以读取（cv::FileStorage::READ）的模式读入对象fsSettings中。
     */
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    // 检查配置文件
    /*
       isOpened() 是 cv::FileStorage 的成员函数，返回一个布尔值，用于检查文件是否成功打开。
       如果文件无法打开或不存在，这个函数会返回 false。
    */
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       exit(-1);
       //调用 exit(-1)：强制终止程序并返回错误代码 -1。exit 函数立即结束程序的执行，-1 通常表示程序异常退出。
       //通过这样做，程序可以避免在配置文件未正确加载时继续执行，这样可以防止出现意外行为。
    }


    //Load ORB Vocabulary 加载ORB词汇表。
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
	// 即将开始加载 ORB 词汇表，并告知这个过程可能需要一段时间。


    mpVocabulary = new ORBVocabulary();//创建 ORB 词汇表对象，动态分配一个新的 ORBVocabulary 对象。
    // 调用 ORBVocabulary 类的成员函数 loadFromTextFile 来加载 ORB 词汇表文件。
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
    // 如果词汇表加载失败，程序会输出错误信息，指明词汇表文件路径有误或无法打开
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        exit(-1);
    }
    cout << "Vocabulary loaded!" << endl << endl;

    // Create KeyFrame Database 创建关键帧数据库
    /*System.h中的私有成员private：KeyFrameDatabase* mpKeyFrameDatabase;
    mpKeyFrameDatabase 是一个指向 KeyFrameDatabase 对象的指针。
	KeyFrameDatabase 是 ORB-SLAM2 系统中的一个类（KeyFrameDatabase.h中定义），用于存储和管理关键帧。
     */
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

    //Create the Map 创建地图
    mpMap = new Map();

    //Create Drawers. These are used by the Viewer 创建绘制器
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

    //Initialize the Tracking thread 初始化跟踪线程
    //(it will live in the main thread of execution, the one that called this constructor)
    /*
 	  this：当前 SLAM 系统实例的指针，类型是 ORB_SLAM2*。
	  参数列表中的参数不可以任意调换位置。
     */
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);

    //Initialize the Local Mapping thread and launch  初始化本地建图线程并启动
    //mptLocalMapping 是 LocalMapping 的线程句柄，通过调用 std::thread 创建并启动 LocalMapping 线程，执行 Run 函数。
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

    //Initialize the Loop Closing thread and launch 初始化回环检测线程并启动
    //mptLoopClosing 是 LoopClosing 线程句柄，通过调用 std::thread 启动 LoopClosing 线程，执行其 Run 函数。
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

    //Initialize the Viewer thread and launch 初始化 Viewer 线程并启动
    //mptViewer 是 Viewer 线程句柄，通过 std::thread 启动 Viewer 线程，执行 Viewer 的 Run 函数。
    if(bUseViewer)
    {
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer);
    }

    //Set pointers between threads
    //设置不同线程之间的相互指针，使得它们可以相互通信和协作。
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);

    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);

    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

//处理双目图像输入、管理系统状态并返回相机位姿（变换矩阵）。
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)
{
    //该部分检查当前设置的传感器类型是否为双目（STEREO）。如果不是，则输出错误信息并退出程序。
    if(mSensor!=STEREO)
    {
        cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
        exit(-1);
    }   

    // Check mode change
    /*
      最外层的大括号用于创建一个新的作用域（scope）。在C++中，使用大括号可以控制变量的生命周期和作用域。
      具体来说，这里创建的作用域使得 unique_lock<mutex> lock(mMutexMode); 的生命周期仅限于这个作用域内部。
      在这个作用域结束后，lock 会被销毁，自动释放对互斥量 mMutexMode 的锁。
    */
    {
        unique_lock<mutex> lock(mMutexMode);// 它的主要作用是管理对共享资源的互斥访问，确保在同一时间只有一个线程可以访问某个资源。
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();//请求本地映射器停止工作。

            // Wait until Local Mapping has effectively stopped
            // 确保在切换到定位模式之前，局部地图（Local Mapping）线程已经完全停止。
            while(!mpLocalMapper->isStopped())// 检查局部映射器（mpLocalMapper）是否已停止。
            {
                usleep(1000);//让当前线程休眠1000微秒（1毫秒）
            }

            mpTracker->InformOnlyTracking(true);//通知跟踪器（mpTracker）现在只进行跟踪而不进行地图构建。
            mbActivateLocalizationMode = false;//表示不再需要激活定位模式。
        }
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);//通知跟踪器（mpTracker）现在跟踪和进行地图构建。
            mpLocalMapper->Release();//请求本地映射器工作。
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)//系统是否需要重置。如果这个标志为 true，那么系统需要执行重置操作。
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }

    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);//变换矩阵

    //建了一个 unique_lock 类型的锁 lock2，用于保护共享的状态数据（mTrackingState、mTrackedMapPoints 和 mTrackedKeyPointsUn）。
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;// 将当前跟踪器的状态（mpTracker->mState）赋值给 mTrackingState。
    // mTrackedMapPoints，将当前帧的地图点（mpvMapPoints）赋值给它。这是跟踪过程中检测到的有效地图点，用于后续的处理和分析。
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    // 更新 mTrackedKeyPointsUn，将当前帧的未处理关键点（mvKeysUn）赋值给它。这通常是指在当前图像中提取的特征点，这些特征点将用于跟踪和匹配。
    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 stopped
            while(!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;
}

//处理单目图像输入、管理系统状态并返回相机位姿（变换矩阵）。具体细节对照双目。
cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{
    if(mSensor!=MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

    // Check mode change
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!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->GrabImageMonocular(im,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

    return Tcw;
}

// 停止本地映射线程（即地图构建），并仅执行相机跟踪（定位）。
void System::ActivateLocalizationMode()
{
    unique_lock<mutex> lock(mMutexMode);
    mbActivateLocalizationMode = true;
}

// 重新开始 SLAM 过程，这个函数可以恢复原来的地图构建线程。
void System::DeactivateLocalizationMode()
{
    unique_lock<mutex> lock(mMutexMode);
    mbDeactivateLocalizationMode = true;
}

// 是否发生了重大地图变化
bool System::MapChanged()
{
    static int n=0;//声明了一个静态变量 n，初始值为 0。
    int curn = mpMap->GetLastBigChangeIdx();//调用 mpMap 的 GetLastBigChangeIdx() 方法，获取地图上最后一次大的变化的索引。
    //比较和更新
    if(n<curn)
    {
        n=curn;
        return true;
    }
    else
        return false;
}

//重置系统，清除地图。
void System::Reset()
{
    unique_lock<mutex> lock(mMutexReset);
    mbReset = true;
}

//请求系统中的所有线程完成它们的工作，并等待这些线程结束，保存轨迹之前必须要调用这个函数。
/*
Shutdown() 函数是系统关闭流程的关键部分，确保所有与 SLAM 相关的线程（如本地映射器、回环闭合器和可视化器）都能平稳退出。
通过使用请求和检查机制，函数避免了潜在的资源泄漏或线程崩溃的问题，确保系统的稳定性。
*/
void System::Shutdown()
{
    mpLocalMapper->RequestFinish();//向局部建图线程发送结束请求。
    mpLoopCloser->RequestFinish();//向回环检测线程发送结束请求。
    //检查并结束可视化器
    if(mpViewer)
    {
        mpViewer->RequestFinish();//向可视化器线程发送结束请求。
        //使用 usleep(5000); 让当前线程休眠 5 毫秒，直到可视化器完成结束。
        while(!mpViewer->isFinished())
            usleep(5000);
    }

    // Wait until all thread have effectively stopped
    // 检查本地映射器和回环闭合器线程是否已经完全结束，或者回环闭合器是否仍在运行全局地图优化（GBA）。
    while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
    {
        usleep(5000);
    }
	//判断检查 mpViewer 是否存在且不为空
    if(mpViewer)
        pangolin::BindToContext("ORB-SLAM2: Map Viewer");//允许可视化器在结束前进行最后的绘制和更新
}

//将相机的轨迹保存为 TUM RGB-D 数据集格式。
void System::SaveTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;//提示保存到指定的文件里。
    // 该函数不支持单目，检查传感器类型，提示相应的信息。
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
        return;
    }

    //获取所有的关键帧，并按 ID 排序。
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//KeyFrame::lId 是一个静态成员函数或函数对象，用于比较两个 KeyFrame 指针的 ID 属性。

    // Transform all keyframes so that the first keyframe is at the origin.
    // 将所有关键帧转换，以便第一个关键帧位于原点。
    // After a loop closure the first keyframe might not be at the origin.
    // 在闭环检测后，第一个关键帧可能不再位于原点。
    cv::Mat Two = vpKFs[0]->GetPoseInverse();//坐标变换将关键帧从世界坐标系转换到相机坐标系。

    //打开一个输出文件流并准备将数据写入文件。
    ofstream f;
    f.open(filename.c_str());
    f << fixed;//设置输出格式的操作，fixed：这是一个流操作符，它指示流应该以固定的小数格式输出数字。

    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // 每一帧的位姿（pose）是相对于（经过BA和位姿图优化）关键帧存储的。
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // 在处理每一帧时，首先需要获取对应的参考关键帧的位姿，然后再将相对变换（相机从参考关键帧到当前帧的变换）合并到这个位姿上。
    // Frames not localized (tracking failure) are not saved.
    // 如果某一帧未能成功定位（即出现了跟踪失败），则该帧的信息不会被保存。

    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // // 对于每一帧，我们有一个参考关键帧（lRit）、一个时间戳（lT）以及一个标志
    // which is true when tracking failed (lbL).
    // 当跟踪失败时该标志为真（lbL）。
    /*
      在C++中，迭代器（iterator） 是一种用于遍历容器（如列表、数组、向量等）的对象。
    */
    // 声明并初始化一个迭代器 lRit，用于遍历关键帧引用列表 mlpReferences，该列表由 mpTracker 提供。
    list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
    // 声明一个迭代器 lT，用于遍历时间戳列表 mlFrameTimes，每个时间戳对应一个帧。
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
    // 声明一个迭代器 lbL，用于遍历一个布尔值列表 mlbLost，该列表用于指示每个帧是否丢失（即跟踪失败）。
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();

    /*
      声明了一个迭代器 lit，并初始化为 mlRelativeFramePoses 列表的第一个元素（即开始位置）。
	  lend是另一个迭代器，指向 mlRelativeFramePoses 列表的末尾（即超出最后一个元素的位置）。
	  只要 lit 不等于 lend，就继续循环。
      在每次循环迭代结束时，所有的迭代器都会向前移动一个位置。
      lit 将指向下一个 cv::Mat 元素，lRit、lT 和 lbL 也分别向前移动，依次访问与当前帧相关的关键帧、时间戳和跟踪状态标志。
    */
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        //当前帧是否在跟踪过程中丢失。如果为 true，则表示此帧的位姿无法可靠确定，因此直接跳过（continue）这次循环，不会将此帧的信息写入轨迹文件。
        if(*lbL)
            continue;

        KeyFrame* pKF = *lRit;// 获取当前帧的参考关键帧。

        // cv::Mat::eye 是 OpenCV 提供的一个静态方法，用于生成一个单位矩阵。
        // CV_32F 表示每个元素是 32 位浮点数（float）。
        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.
        // 如果当前参考关键帧被移除（即“culled”），则遍历其关联的关键帧树，以找到一个合适的关键帧。
        //while 循环会在当前关键帧 pKF 被标记为“坏”（isBad() 返回 true）时持续执行。//
        //关键帧的“坏”状态通常表示该关键帧的数据不可靠或已被剔除。
        while(pKF->isBad())
        {
            Trw = Trw*pKF->mTcp;//更新变换矩阵：mTcp 是当前关键帧 pKF 到其父关键帧的变换矩阵（即 Tcp 表示从当前关键帧到父关键帧的变换）。
            pKF = pKF->GetParent();// 移动到父节点
        }

        //pKF->GetPose(): 获取当前参考关键帧 pKF 的位姿（变换矩阵），即该关键帧在世界坐标系中的位置和方向。
        //Two 世界坐标到相机坐标。
        Trw = Trw*pKF->GetPose()*Two;

        cv::Mat Tcw = (*lit)*Trw;//通过参考帧的变换矩阵×当前帧相对与参考帧的坐标变换=当前帧的世界坐标到相机坐标的变换矩阵。
        //Tcw.rowRange(0,3).colRange(0,3)：从 Tcw 变换矩阵中提取前 3 行 3 列。.t()：取矩阵的转置即是逆矩阵（旋转矩阵是正交阵）。
        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
        //Tcw.rowRange(0,3).col(3)：从 Tcw 变换矩阵中提取最后一列前三行旋转加反向即为twc。
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3)

        vector<float> q = Converter::toQuaternion(Rwc);// 将旋转矩阵 Rwc 转换为四元数 q。

        //输出格式：timestamp x y z q[0] q[1] q[2] q[3]，每个值之间用空格分隔，最终形成轨迹文件。
        // setprecision(6)、setprecision(9) 设置的输出精度。
        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;
    }
    f.close();// 关闭文件
    cout << endl << "trajectory saved!" << endl;// 提示轨迹被保存。
}


void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;//提示保存到指定的文件里。

    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();//从地图中获取所有的关键帧，存储在 vpKFs 向量中。
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);// 按 ID 排序。

    // Transform all keyframes so that the first keyframe is at the origin.
    // 将所有关键帧的位姿进行变换，使得第一个关键帧位于坐标系的原点。
    // After a loop closure the first keyframe might not be at the origin.
    // 在闭环检测（loop closure）后，第一个关键帧的位姿可能不再位于原点。
    //cv::Mat Two = vpKFs[0]->GetPoseInverse();

    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];

       // pKF->SetPose(pKF->GetPose()*Two);

        if(pKF->isBad())
            continue;

        cv::Mat R = pKF->GetRotation().t();//调用方法GetRotation()直接获得旋转矩阵在转置。
        vector<float> q = Converter::toQuaternion(R);//转换成四元数。
        cv::Mat t = pKF->GetCameraCenter();//调用方法GetCameraCenter()直接获取平移。
        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;

    }

    f.close();//关闭文件。
    cout << endl << "trajectory saved!" << endl;//提示保存成功。
}

//对照上边TMU数据集格式进行学习。
void System::SaveTrajectoryKITTI(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
        return;
    }

    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

    // Transform all keyframes so that the first keyframe is at the origin.
    // 将所有关键帧转换，使得第一个关键帧位于原点。
    // After a loop closure the first keyframe might not be at the origin.
    // 在进行循环闭合后，第一个关键帧可能不再位于原点。
    cv::Mat Two = vpKFs[0]->GetPoseInverse();

    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // 帧姿态是相对于其参考关键帧存储的（该关键帧经过 BA 和姿态图的优化）。
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // 我们需要首先获取关键帧的姿态，然后再连接相对变换。
    // Frames not localized (tracking failure) are not saved.
    // 未定位的帧（跟踪失败）不会被保存。

    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // 对于每一帧，我们都有一个参考关键帧（lRit）、时间戳（lT）和一个标志
    // which is true when tracking failed (lbL).
    // 当跟踪失败时，该标志为真（lbL）。
    list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
    {
        ORB_SLAM2::KeyFrame* pKF = *lRit;

        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);

        while(pKF->isBad())
        {
          //  cout << "bad parent" << endl;
            Trw = Trw*pKF->mTcp;
            pKF = pKF->GetParent();
        }

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

        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;
    }
    f.close();
    cout << endl << "trajectory saved!" << endl;
}

int System::GetTrackingState()
{
    unique_lock<mutex> lock(mMutexState);
    return mTrackingState;
}

vector<MapPoint*> System::GetTrackedMapPoints()
{
    unique_lock<mutex> lock(mMutexState);
    return mTrackedMapPoints;
}

vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
{
    unique_lock<mutex> lock(mMutexState);
    return mTrackedKeyPointsUn;
}

} //namespace ORB_SLAM