ORB-SLAM3 单目惯导ros-system-track

xiaoma_bk

已于 2022-07-11 10:53:11 修改

阅读量3.7k

点赞数 7

分类专栏： # ORB_SLAM3 文章标签：自动驾驶人工智能计算机视觉

于 2021-11-03 16:08:21 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/121017148

版权

ORB_SLAM3 专栏收录该内容

8 篇文章 9 订阅

订阅专栏

文章目录

ROS
- ros_mono_interial.cc
ORB_SLAM3::System
Tracking

ROS

程序分为ROS接口和非ROS接口的。
主程序 main 在ROS/ORB_SLAM3/src/中：

ros_mono_interial.cc

Function

主函数：读取参数，并创建了slam系统，订阅两topic+创建同步线程。
回调函数：只将数据放入队列，上锁
同步线程：1s一次，先数据同步，后进行跟踪

主函数

读取参数 argc
创建SLAM系统，它初始化所有系统线程并准备好处理帧。
订阅IMU+CAMERA topic，回调中将数据放入队列
数据同步线程 ( Image 与 imu)

int main(int argc, char **argv)
{
  ros::init(argc, argv, "Mono_Inertial");
  ros::NodeHandle n("~");
  ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
  bool bEqual = false;
  if(argc < 3 || argc > 4)
  {
    cerr << endl << "Usage: rosrun ORB_SLAM3 Mono_Inertial path_to_vocabulary path_to_settings [do_equalize]" << endl;
    ros::shutdown();
    return 1;
  }

  if(argc==4)
  {
    std::string sbEqual(argv[3]);
    if(sbEqual == "true")
      bEqual = true;
  }

  // Create SLAM system. It initializes all system threads and gets ready to process frames.
  ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR,true);

  ImuGrabber imugb;
  ImageGrabber igb(&SLAM,&imugb,bEqual); // TODO
  
  // Maximum delay, 5 seconds
  ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); 
  ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb);

  std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);

  ros::spin();

  return 0;
}

imu抓取

class ImuGrabber
{
public:
    ImuGrabber(){};
    void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);

    queue<sensor_msgs::ImuConstPtr> imuBuf;
    std::mutex mBufMutex;
};
// 直接放入imubuf中
void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{
  mBufMutex.lock();
  imuBuf.push(imu_msg);
  mBufMutex.unlock();
  return;
}

image 抓取

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), mbClahe(bClahe){}

    void GrabImage(const sensor_msgs::ImageConstPtr& msg);
    cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
    void SyncWithImu();

    queue<sensor_msgs::ImageConstPtr> img0Buf;
    std::mutex mBufMutex;
   
    ORB_SLAM3::System* mpSLAM;
    ImuGrabber *mpImuGb;

    const bool mbClahe;
    cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
};

// 回调就是将其数据放入队列
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg)
{
  mBufMutex.lock();
  if (!img0Buf.empty())
    img0Buf.pop();
  img0Buf.push(img_msg);
  mBufMutex.unlock();
}

image与imu同步

步骤：

确保imu时间是否包含住image时间，否则continue
取出 image + vector
基于参数确定图像是否需要直方图均衡。

void ImageGrabber::SyncWithImu()
{
  while(1)
  {
    cv::Mat im;
    double tIm = 0;
    if (!img0Buf.empty()&&!mpImuGb->imuBuf.empty())
    {
      tIm = img0Buf.front()->header.stamp.toSec();
      // 保证：imu 最新时间比 image 新
      if(tIm>mpImuGb->imuBuf.back()->header.stamp.toSec())
          continue;
      {  // 取出图片
      this->mBufMutex.lock();
      im = GetImage(img0Buf.front());
      img0Buf.pop();
      this->mBufMutex.unlock();
      }

	  // 取出 小于image时间的imu数据，{time，线加速度，角加速度}
      vector<ORB_SLAM3::IMU::Point> vImuMeas;
      mpImuGb->mBufMutex.lock();
      if(!mpImuGb->imuBuf.empty())
      {
        // Load imu measurements from buffer
        vImuMeas.clear();
        while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tIm)
        {
          double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
          cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
          cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
          vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
          mpImuGb->imuBuf.pop();
        }
      }
      mpImuGb->mBufMutex.unlock();
      if(mbClahe)
        mClahe->apply(im,im);
     
      mpSLAM->TrackMonocular(im,tIm,vImuMeas);
    }

    std::chrono::milliseconds tSleep(1);
    std::this_thread::sleep_for(tSleep);
  }
}
// cv::CLAHE 是另外一种直方图均衡算法，CLAHE 和 AHE 的区别在于前者对区域对比度实行了限制，并且利用插值来加快计算。它能有效的增强或改善图像（局部）对比度，从而获取更多图像相关边缘信息有利于分割。还能够有效改善 AHE 中放大噪声的问题。另外，CLAHE 的有一个用途是被用来对图像去雾。

ros-image->Mat

cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
{
  // Copy the ros image message to cv::Mat.
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }
  
  if(cv_ptr->image.type()==0)
  {
    return cv_ptr->image.clone();
  }
  else
  {
    std::cout << "Error type" << std::endl;
    return cv_ptr->image.clone();
  }
}

ORB_SLAM3::System

Function

main主函数定义了System类，他创建了跟踪、局部地图线程、闭环线程。
- 跟踪：image+imu同步线程中。
System.cc这个文件主要是SLAM系统的初始化、跟踪线程的入口以及轨迹的保存。

构造

构建字典并加载已有字典
建立关键帧库和地图集
创建显示 Frame和地图的类，由viewer调用
创建跟踪线程
创建局部地图线程
创建闭环线程

System::System(
const string &strVocFile  //词典文件路径
, const string &strSettingsFile //配置文件路径 
, const eSensor sensor  //传感器类型 
,const bool bUseViewer // 是否使用可视化界面
, const int initFr // 初始化Frame
, const string &strSequence 
, const string &strLoadingFile):
    mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false)
{
    // 打印 什么传感器
    if(mSensor==MONOCULAR) //单目
        cout << "Monocular" << endl;
    else if(mSensor==STEREO) //双目
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;
    else if(mSensor==IMU_MONOCULAR) //IMU+单目
        cout << "Monocular-Inertial" << endl;
    else if(mSensor==IMU_STEREO) // IMU+双目
        cout << "Stereo-Inertial" << endl;

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

    // 加载Atlas的标志，目前没有用
    bool loadedAtlas = false;
    
    //Load ORB Vocabulary
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
    // 建立一个新的ORB字典，并加载已有的orb字典词袋
    mpVocabulary = new ORBVocabulary();
    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 创建关键帧数据库
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

    //Create the Atlas  创建地图集，参数0表示初始化关键帧id为0
    mpAtlas = new Atlas(0);

	// 若使用了imu，则mpAtlas设定imu传感器
    if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
        mpAtlas->SetInertialSensor();

    // 这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用
    mpFrameDrawer = new FrameDrawer(mpAtlas);
    mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile);

    //Initialize the Tracking thread  初始化跟踪线程
    //（它将存在于执行的主线程中，即调用此构造函数的线程中）
    cout << "Seq. Name: " << strSequence << endl;
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence);
    
    //创建并开启local mapping线程，回调函数为LocalMapping::Run()
    mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO, strSequence);
    mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
    //设置区分远点和近点的阈值
    mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
    if(mpLocalMapper->mThFarPoints!=0)
    {
        cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
        mpLocalMapper->mbFarPoints = true;
    }
    else
        mpLocalMapper->mbFarPoints = false;

    // 创建并开启闭环线程，回调函数为LoopClosing::Run()
    mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); // mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);

    //Initialize the Viewer thread and launch
    if(bUseViewer) //如果指定了，程序的运行过程中需要运行可视化部分
    {
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer);
        mpLoopCloser->mpViewer = mpViewer;
        mpViewer->both = mpFrameDrawer->both;
    }

    //Set pointers between threads 设置进程间的指针
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);

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

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

    // Fix verbosity 设置打印信息的等级，只有小于这个等级的信息才会被打印
    Verbose::SetTh(Verbose::VERBOSITY_QUIET);

}

TrackMonocular 单目跟踪

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{   // 单目 和 IMU+单目
    if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << 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);
            }
            // 运行到这里的时候，局部建图部分就真正地停止了
			// 通知Tracking线程，现在只进行跟踪，不建图
            mpTracker->InformOnlyTracking(true);
            // 设置为false，避免重复进行以上操作
            mbActivateLocalizationMode = false;
        }
        if(mbDeactivateLocalizationMode) //取消定位模式？，重新打开局部建图线程
        {   // 仅跟踪设置为false，localmap =Release,同时避免重复
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release(); //局部建图器要开始工作呢
            mbDeactivateLocalizationMode = false; //防止重复执行
        }
    }

    // Check reset  检查是否有复位的操作
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset) // 是否有复位请求？
        {	//有，追踪器复位+清除标志+不重置活跃子图
            mpTracker->Reset();
            mbReset = false;
            mbResetActiveMap = false;
        }
        else if(mbResetActiveMap) // 重置活跃地图？
        {
            cout << "SYSTEM-> Reseting active map in monocular case" << endl;
            mpTracker->ResetActiveMap();
            mbResetActiveMap = false;
        }
    }
	
	// 若imu+单目，则 tracking 中抓imu数据
    if (mSensor == System::IMU_MONOCULAR)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);
	
	// 开始跟踪，获取相机位姿的估计结果  ****运动估计的函数
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);

    unique_lock<mutex> lock2(mMutexState);
    // 记录track追踪的状态
    mTrackingState = mpTracker->mState; 
    ///获取当前帧追踪到的地图点向量指针
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    // 获取当前帧追踪到的关键帧特征点向量的指针
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
	
	// //返回获得的相机运动估计
    return Tcw; 
}

TrackStereo

双目或双目+IMU

TrackRGBD

RGBD

Shutdown 关闭

void System::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())
    {
        if(!mpLocalMapper->isFinished())
            cout << "mpLocalMapper is not finished" << endl;
        if(!mpLoopCloser->isFinished())
            cout << "mpLoopCloser is not finished" << endl;
        if(mpLoopCloser->isRunningGBA()){
            cout << "mpLoopCloser is running GBA" << endl;
            cout << "break anyway..." << endl;
            break;
        }
        usleep(5000);
    }

    if(mpViewer) 
        pangolin::BindToContext("ORB-SLAM2: Map Viewer");

#ifdef REGISTER_TIMES
    mpTracker->PrintTimeStats();
#endif
}

SaveTrajectoryTUM 按TUM保存轨迹

以TUM格式(tx,ty,tz,qx,qy,qz,qw)保存所有成功定位的帧的位姿
关键帧是相对于其参考关键帧（由 BA 和姿势图优化）存储的。我们需要首先获得关键帧姿势，然后连接相对变换。未定位的帧（跟踪失败）不会保存。

void System::SaveTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    // 只有在传感器为双目或者RGBD时才可以工作
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
        return;
    }
	// 获取地图集中所有的关键帧，并给予Id排序
    vector<KeyFrame*> vpKFs = mpAtlas->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).
    // 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
    // which is true when tracking failed (lbL).
    //  对于每一帧，我们都有一个参考关键帧 (lRit)、时间戳 (lT) 和跟踪失败时为真的标志 (lbL)。
    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();  // 关键帧list
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();//时间戳
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();//是否跟踪失败
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        if(*lbL) //如果跟踪失败，则跳过，不保存这一帧
            continue;

        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();
        }
		// 最终得到的是参考关键帧相对于世界坐标系的变换
        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;
    }
    f.close();
    // cout << endl << "trajectory saved!" << endl;
}

其他数据的保存

以TUM格式保存所有关键帧的位姿
void SaveKeyFrameTrajectoryTUM(const string &filename);
以EuRoC格式保存所有帧的位姿
void SaveTrajectoryEuRoC(const string &filename);
以EuRoC格式保存所有关键帧的位姿
void SaveKeyFrameTrajectoryEuRoC(const string &filename);
以KITTI格式保存所有帧的位姿
void SaveTrajectoryKITTI(const string &filename);

Tracking

在这里插入图片描述

核心函数

构造

namespace ORB_SLAM3
{

Tracking::Tracking(
System *pSys   // systerm 中 this指针
, ORBVocabulary* pVoc  // BOW词袋，包括了已加载的词袋
, FrameDrawer *pFrameDrawer //帧绘制器
, MapDrawer *pMapDrawer // 地图点绘制器
, Atlas *pAtlas // 地图集句柄
, KeyFrameDatabase* pKFDB  // 关键帧产生的词袋数据库
, const string &strSettingPath // 配置文件路径
, const int sensor // 传感器类型
, const string &_nameSeq):
    mState(NO_IMAGES_YET) // 当前状态还没准备好
    , mSensor(sensor), mTrackedFr(0), mbStep(false),
    mbOnlyTracking(false) // 处于slam模式
    , mbMapUpdated(false), mbVO(false), //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏
    mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB),
    mpInitializer(static_cast<Initializer*>(NULL)), //暂时给地图初始化器设置为空指针
    mpSystem(pSys), mpViewer(NULL),
    mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), time_recently_lost_visual(2.0),
    mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr)
{
    // Load camera parameters from settings file
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
	
	///< step 1: 从配置文件中加载相机参数
	    // Load ORB parameters 加载相机参数文件
    // 相机参数，两类：
    // 		1. PinHole: fx,fy,cx,cy,k1,k2,p1,p2  --k3
    //  	2. KannalaBrandt8:fx,fy,cx,cy,k1,k2,k3,k4 其他就不支持了
    // STEREO or IMU_STEREO: br
    // fps , fps==0 ? 30 : fps
    // RGB ? RGB : BGR
    // STEREO || RGBD || IMU_STEREO ? ThDepth (深度阈值（近/远点))
    // RGBD ? DepthMapFactor
    // 若上述都存在且真实，则 return true 否则 false
    bool b_parse_cam = ParseCamParamFile(fSettings);
    if(!b_parse_cam) // 读取失败则打印error
    {
        std::cout << "*Error with the camera parameters in the config file*" << std::endl;
    }

	///< step 2: 加载ORB特征点有关的参数，并构造ORB提取对象
	// nFeatures: 1000 每一帧提取的特征点数 1000
	// scaleFactor: 1.2 图像建立金字塔时的变化尺度 1.2
	// nLevels: 8 尺度金字塔的层数 8
	// iniThFAST: 20 提取fast特征点的默认阈值 20
	// minThFAST: 7 如果默认阈值提取不出足够fast特征点，则使用最小阈值 7
	/// 构造orb提取对象：构造左目提取器，若双目或imu_双目时？再构造右目提取器，若单目或imu_单目时？构造mpIniORBextractor
	/// 特征数：mpIni=5*(mpLeft=mpRight)
    bool b_parse_orb = ParseORBParamFile(fSettings);
    if(!b_parse_orb)
    {
        std::cout << "*Error with the ORB parameters in the config file*" << std::endl;
    }

    initID = 0; lastID = 0;

    // Load IMU parameters 
    bool b_parse_imu = true; // 加载 imu 参数
    if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO) // 只使用imu时加载
    {   // 外参 Tcw 4*4 
    	///< **IMU noise (Use those from VINS-mono)**
		// IMU.NoiseGyro: 0.00016 # rad/s^0.5 
		// IMU.NoiseAcc: 0.0028 # m/s^1.5
		// IMU.GyroWalk: 0.000022 # rad/s^1.5
		// IMU.AccWalk: 0.00086 # m/s^2.5
		// IMU.Frequency: 200
        b_parse_imu = ParseIMUParamFile(fSettings);
        if(!b_parse_imu)
        {
            std::cout << "*Error with the IMU parameters in the config file*" << std::endl;
        }

        mnFramesToResetIMU = mMaxFrames;
    }

    mbInitWith3KFs = false;

    mnNumDataset = 0;

    if(!b_parse_cam || !b_parse_orb || !b_parse_imu)
    {
        std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
        try
        {
            throw -1;
        }
        catch(exception &e)
        {

        }
    }

#ifdef REGISTER_TIMES
    vdRectStereo_ms.clear();
    vdORBExtract_ms.clear();
    vdStereoMatch_ms.clear();
    vdIMUInteg_ms.clear();
    vdPosePred_ms.clear();
    vdLMTrack_ms.clear();
    vdNewKF_ms.clear();
    vdTrackTotal_ms.clear();

    vdUpdatedLM_ms.clear();
    vdSearchLP_ms.clear();
    vdPoseOpt_ms.clear();
#endif

    vnKeyFramesLM.clear();
    vnMapPointsLM.clear();
}

读取参数文件时，是否存在以及是否real案例：

 // Camera calibration parameters
 cv::FileNode node = fSettings["Camera.fx"];
 if(!node.empty() && node.isReal())
 {
     fx = node.real();
 }
 else
 {
     std::cerr << "*Camera.fx parameter doesn't exist or is not a real number*" << std::endl;
     b_miss_params = true;
 }

GrabImageMonocular

输入：左目RGB或RGBA图像
1、将图像转为mImGray并初始化mCurrentFrame
2、进行tracking过程
输出：世界坐标系到该帧相机坐标系的变换矩阵

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, string filename)
{
    mImGray = im;

	// step 1 ：将RGB或RGBA图像转为灰度图像
    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY);
    }

	// step 2 ：构造Frame  
    if (mSensor == System::MONOCULAR)   // 若是单目
    {	// 《状态没有初始化or没有图片or lastid与初始id间隔太小》时？
    	//  初始特征提取；否则orb特征提取，提取特征值的数量不一样
        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames)
            mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
        else  
            mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
    }
    else if(mSensor == System::IMU_MONOCULAR) // 单目Imu
    {
    	// 《状态没有初始化or没有图片》时，初始特征提取；否则orb特征提取
        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
        {
            mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
        }
        else
            mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
    }

    if (mState==NO_IMAGES_YET)
        t0=timestamp;

    mCurrentFrame.mNameFile = filename;
    mCurrentFrame.mnDataset = mnNumDataset;

#ifdef REGISTER_TIMES
    vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
#endif

    lastID = mCurrentFrame.mnId;
	// step 3 ：跟踪
    Track();
	// 返回当前帧的位姿
    return mCurrentFrame.mTcw.clone();
}

Track 跟踪

若 localmap中imu数据有问题，则系统：重置活跃地图ResetActiveMap
地图集中取出当前地图
mState!=NO_IMAGES_YET：
1. 当前帧时间回跳时，清空imu数据，并创建地图CreateMapInAtlas，return
2. 当前帧时间与上一帧相差了1s以上时，若地图集未使用imu则return，否则：
  1. imu未初始化成功，则重置活跃子图ResetActiveMap
  2. 若imu初始化成功时：且GetIniertialBA2成功时在地图集中创建地图CreateMapInAtlas，否则重置活跃子图ResetActiveMap
若使用imu且上一帧是关键帧时，当前帧设置新的bias
若 mState == NO_IMAGES_YET，则：mState = NOT_INITIALIZED
若使用imu且非创建地图时，imu进行预积分 (两套)
上一状态和创建地图标记赋值
1. mLastProcessedState=mState;
2. mbCreatedMap = false;
若 mState == NOT_INITIALIZED
1. 基于传感器参数进行相应初始化，如MonocularInitialization
2. 绘制地图更新 mpFrameDrawer->Update(this)
3. 若未正常初始化时， mLastFrame = currentFrame ,return
4. 若地图集 == 1时，mnFirstFrameId = mCurrentFrame.mnId;
否则 mState 已经初始化：
1. 若不是仅跟踪模式：
  1. 若 mState == ok时：
    1. 检查上一帧中被替换的地图点，将地图点替换成新的地图点CheckReplacedInLastFrame
    2. 状态跟踪
      1. 若运动模式是空且imu未初始化或刚刚完成初始化时，跟踪参考关键帧TrackReferenceKeyFrame
      2. 否则，跟踪运动模式TrackWithMotionModel，若运动模式跟踪失败，则跟踪参考帧
      3. 若跟踪失败时，
      4. 若刚刚进行了重定位且imu模式，则直接将mState置为lost
      5. 否则，如果地图中关键帧数量大于10，则 RECENTLY_LOST
      6. 否则 mState = LOST;
  2. 若状态不是ok时：
    1. 如果是最近丢失时 :mState == RECENTLY_LOST
      1. bOK = true;
      2. 如果使用imu时，
        若：imu初始化成功，则用imu跟踪 PredictStateIMU
        否则初始化失败，bOK = false;
        3. 若与前一次跟踪相隔5s,记为lost
        mState = LOST;，OK=false;
      3. 纯视觉模式下进行重定位，重定位失败记为lost
        mState = LOST;，OK=false;
        2. 若状态为 lost: mState == LOST
        若地图关键帧数量小于10个时，则重置活跃地图
        否则新建地图，删除mLastKeyFrame,从头开始
2. 若是仅跟踪模式：
  1. 若定位状态为lost，定位丢失则重定位 Relocalization
  2. 否则若跟踪正常时：
    1. 恒速模式时使用速度，bOK = TrackWithMotionModel
    2. 否则使用参考帧， bOK = TrackReferenceKeyFrame
  3. 状态不正常时
    1. 恒速模式非空时用恒速模型，bOKMM = TrackWithMotionModel
    2. 重定位 Relocalization
    3. 恒速模型且重定位失败时，使用了恒速预测的数据
      1. mCurrentFrame 设置为恒速模型的数据
      2. 若仍失败，则当前帧地图点 IncreaseFound
    4. 否则若重定位成功时：设置状态正常 mbVO = false;
    5. 跟踪成功：bOKMM || 重定位成功
      3. 若当前帧是关键帧，则更新关键帧
      4. 若不是仅跟踪模式： // slam模式
      1. 若跟踪成功时，则跟综局部地图（寻找更多的匹配，优化当前帧的pose） TrackLocalMap
      2. 若跟踪失败，则打印
      5. 若定位模式时：
      1. 跟踪成功且状态ok(!mbVO)：跟踪localmap（寻找更多的匹配，优化当前帧的pose）TrackLocalMap
      2. mbVO true 表示地图中与 MapPoints 的匹配项很少。我们无法检索本地地图，因此我们不执行 TrackLocalMap()。一旦系统重新定位相机，我们将再次使用本地地图。
      6. 跟踪成功时：bOK //包括了跟踪局部地图的结果
      1. mstate =ok。
      7. 跟踪失败时：bOKfalse
      1. 若前面位姿跟踪成功时：mState == OK
      1. 使用imu，且当前地图未初始化完imu 或 imu做ba2失败时，系统重置活跃子图ResetActiveMap， mState = RECENTLY_LOST;
      2. 不使用imu时： mState = RECENTLY_LOST;
      3. 更新丢失时间 mTimeStampLost
      8. 如果最近重新定位，且当前帧id>上次重置imu，且使用imu，且地图已经初始化imu
      1. 当前帧构建frame，并进行预积分 Preintegrated
      9. 若pCurrentMap已初始化imu成功时：
      1. 若跟踪成功，且未重置imu，则更新mLastBias
      显示设置当前帧位姿
      10. 如果跟踪成功bOK或者 mStateRECENTLY_LOST,
    6. 更新恒速模型，计算上一帧到当前帧的位姿变换
    7. 若使用imu，则更新当前相机pose SetCurrentCameraPose
    8. 清除恒速跟踪中的临时地图点
    9. 清除观测不到的地图点
    10. 判断是否需要插入关键帧，插入关键帧(对双目或rgb-d会产生新的地图点)
      11. 若状态失败 mState==LOST)时：
    11. 若：关键帧小于5个，或使用imu且imu未初始化时：系统重置活跃子图
    12. 否则创建新地图
      12. 若当前帧不是关键帧，则赋值临时帧。赋值mLastFrame
若状态ok 或状态是最近丢失时：
存储帧姿态信息以在之后检索完整的相机轨迹。

void Tracking::Track()
{
    if (bStepByStep)
    {
        while(!mbStep)
            usleep(500);
        mbStep = false;
    }
	
	// 若 localmap中imu数据有问题，则系统重置活跃地图
    if(mpLocalMapper->mbBadImu)
    {
        cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;
        mpSystem->ResetActiveMap();
        return;
    }
	
	// 地图集中 取出 当前地图
    Map* pCurrentMap = mpAtlas->GetCurrentMap();
    
    // 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
    if(mState!=NO_IMAGES_YET)  // 有图片时
    {
    	// 当前帧时间回跳时，清空imu数据，并创建地图
        if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
        {
            cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
            unique_lock<mutex> lock(mMutexImuQueue);
            mlQueueImuData.clear();
            CreateMapInAtlas();
            return;
        }
        else  // 当前帧与上一帧相差了1s以上时，若未使用imu，直接return，若使用imu,则创建地图 or 重置地图
        if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)
        {
            cout << "id last: " << mLastFrame.mnId << "    id curr: " << mCurrentFrame.mnId << endl;
            if(mpAtlas->isInertial()) // 使用了IMU
            {

                if(mpAtlas->isImuInitialized()) // imu已经初始化
                {
                    cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
                    // 1. 若imu初始化成功时：若得到imu ba 错误时重置活跃子图，否则创建地图在地图集中
                    if(!pCurrentMap->GetIniertialBA2())
                    {
                        mpSystem->ResetActiveMap();
                    }
                    else
                    {
                        CreateMapInAtlas();
                    }
                }
                else
                {
                    cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
                    // 重置活跃子图
                    mpSystem->ResetActiveMap();
                }
            }

            return;
        }
    }

	// 若使用imu且上一帧是关键帧时，当前帧设置新的bias
    if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame)
        mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());

    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }

    mLastProcessedState=mState;

    if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap)
    {
#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now();
#endif
        PreintegrateIMU();
#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now();

        double timePreImu = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPreIMU - time_StartPreIMU).count();
        vdIMUInteg_ms.push_back(timePreImu);
#endif

    }
    mbCreatedMap = false;

    // Get Map Mutex -> Map cannot be changed
    unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);

    mbMapUpdated = false;

    int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();
    int nMapChangeIndex = pCurrentMap->GetLastMapChange();
    if(nCurMapChangeIndex>nMapChangeIndex)
    {
        // cout << "Map update detected" << endl;
        pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
        mbMapUpdated = true;
    }


    if(mState==NOT_INITIALIZED)
    {
    	// 如果没有初始化，则先进行初始化
        if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO)
            StereoInitialization();
        else
        {
            MonocularInitialization();
        }
		// 地图更新此
        mpFrameDrawer->Update(this);
		// 如果正确初始化，mState=OK
        if(mState!=OK) // If rightly initialized, mState=OK
        {	// 若状态不是ok，则当前帧赋值last，return
            mLastFrame = Frame(mCurrentFrame);
            return;
        }

        if(mpAtlas->GetAllMaps().size() == 1)
        {
            mnFirstFrameId = mCurrentFrame.mnId;
        }
    }
    else
    {
        // System is initialized. Track Frame.
        bool bOK;

#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now();
#endif

        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        // 使用运动模型或重新定位的初始相机姿态估计（如果跟踪丢失）
        if(!mbOnlyTracking)  // 不仅仅是跟踪模式
        {  

            // State OK
            // 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
                // 检查上一帧中被替换的地图点，将地图点替换成新的地图点
                CheckReplacedInLastFrame();
				
				// 若运动模式是空且imu未初始化 或刚刚完成初始化时 跟踪参考关键帧
                if((mVelocity.empty() && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
                    //Verbose::PrintMess("TRACK: Track with respect to the reference KF ", Verbose::VERBOSITY_DEBUG);
                    // 跟踪参考关键帧
                    bOK = TrackReferenceKeyFrame();
                }
                else
                {
                    //Verbose::PrintMess("TRACK: Track with motion model", Verbose::VERBOSITY_DEBUG);
                    // 跟踪运动模式
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        bOK = TrackReferenceKeyFrame();
                }


                if (!bOK) //如果跟踪失败了
                {
                	// 如果刚刚进行了重定位且imu模式，则直接将mState置为lost
                    if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&
                         (mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO))
                    {
                        mState = LOST;
                    } // 否则，如果地图中关键帧数量大于10，则 RECENTLY_LOST
                    else if(pCurrentMap->KeyFramesInMap()>10)
                    {
                        cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
                        mState = RECENTLY_LOST;
                        mTimeStampLost = mCurrentFrame.mTimeStamp;
                        //mCurrentFrame.SetPose(mLastFrame.mTcw);
                    }
                    else
                    {
                        mState = LOST;
                    }
                }
            }
            else // 状态不ok时
            {
				// 如果是最近丢失时
                if (mState == RECENTLY_LOST)
                {
                    Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);

                    bOK = true;
                    // 如果是imu模式，则用imu跟踪，最多跟踪5s,否则记为lost
                    if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO))
                    {
                        if(pCurrentMap->isImuInitialized())
                            PredictStateIMU();
                        else
                            bOK = false;

                        if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost)
                        {
                            mState = LOST;
                            Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
                            bOK=false;
                        }
                    }
                    else 
                    {	// 纯视觉模式下进行重定位，重定位失败记为lost
                        // TODO fix relocalization
                        bOK = Relocalization();
                        if(!bOK && mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost_visual)
                        {
                            mState = LOST;
                            Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
                            bOK=false;
                        }
                    }
                }
                else if (mState == LOST) // 状态处于lost时
                {

                    Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
					// 若地图关键帧数量小于10个时，则重置活跃地图
                    if (pCurrentMap->KeyFramesInMap()<10)
                    {
                        mpSystem->ResetActiveMap();
                        cout << "Reseting current map..." << endl;
                    }else // 否则新建地图，删除mLastKeyFrame,从头开始
                        CreateMapInAtlas();

                    if(mpLastKeyFrame)
                        mpLastKeyFrame = static_cast<KeyFrame*>(NULL);

                    Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);

                    return;
                }
            }

        }
        else // 若处于定位模式时（局部地图不工作）
        {
            // Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
            if(mState==LOST) //若定位状态为lost，定位丢失则重定位
            {
                if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
                    Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
                bOK = Relocalization();
            }
            else
            {
                if(!mbVO) // 跟踪正常时
                {
                    // In last frame we tracked enough MapPoints in the map
                    // 恒速模式时使用速度，否则使用参考帧
                    if(!mVelocity.empty())
                    {	
                        bOK = TrackWithMotionModel();
                    }
                    else
                    {
                        bOK = TrackReferenceKeyFrame();
                    }
                }
                else // 状态不正常时
                {	
                    // In last frame we tracked mainly "visual odometry" points.

                    // We compute two camera poses, one from motion model and one doing relocalization.
                    // If relocalization is sucessfull we choose that solution, otherwise we retain
                    // the "visual odometry" solution.
					//在最后一帧中，我们主要跟踪“视觉里程计”点。 我们计算两个相机姿势，一个来自运动模型，另一个进行重新定位。 如果重新定位成功，我们选择该解决方案，否则我们保留“视觉里程计”解决方案。
                    bool bOKMM = false;
                    bool bOKReloc = false;
                    vector<MapPoint*> vpMPsMM;
                    vector<bool> 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)
                        {
                            for(int i =0; i<mCurrentFrame.N; i++)
                            {
                                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                                {
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
                        mbVO = false;
                    }

                    bOK = bOKReloc || bOKMM;
                }
            }
        }
		// 若当前帧是关键帧，则更新关键帧
        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;

#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now();

        double timePosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPosePred - time_StartPosePred).count();
        vdPosePred_ms.push_back(timePosePred);
#endif


#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now();
#endif
        // If we have an initial estimation of the camera pose and matching. Track the local map.
        if(!mbOnlyTracking) // 若不是仅跟踪模式：
        {	// 若跟踪成功时，则跟综局部地图，寻找更多的匹配，优化当前帧的pose
            if(bOK) 
            {
                bOK = TrackLocalMap();

            }
            if(!bOK)
                cout << "Fail to track local map!" << endl;
        }
        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.
            // mbVO true 表示地图中与 MapPoints 的匹配项很少。 我们无法检索本地地图，因此我们不执行 TrackLocalMap()。 一旦系统重新定位相机，我们将再次使用本地地图。
            if(bOK && !mbVO) //跟踪成功 且 状态ok(!mbVO)：跟踪localmap（寻找更多的匹配，优化当前帧的pose）
                bOK = TrackLocalMap();
        }
		
		// 局部地图跟踪成功，mstate =ok。局部地图失败但前面位姿跟踪成功，则imu模式置为recent_lost，纯视觉置为lost
        if(bOK)
            mState = OK;
        else if (mState == OK)
        {
            if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
            {
                Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
                if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2())
                {
                    cout << "IMU is not or recently initialized. Reseting active map..." << endl;
                    mpSystem->ResetActiveMap();
                }

                mState = RECENTLY_LOST;
            }
            else
                mState = RECENTLY_LOST; // visual to lost

            if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames)
            {
                mTimeStampLost = mCurrentFrame.mTimeStamp;
            }
        }

        // Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified)
// 如果最近重新定位，且帧id>上次重置imu，且使用imu，且地图已经初始化imu
if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) && ((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO)) && pCurrentMap->isImuInitialized())
        {
            // TODO check this situation
            Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL);
            Frame* pF = new Frame(mCurrentFrame);
            pF->mpPrevFrame = new Frame(mLastFrame);

            // Load preintegration
            pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
        }

        if(pCurrentMap->isImuInitialized())
        {
            if(bOK)
            {
                if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
                {
                    cout << "RESETING FRAME!!!" << endl;
                }
                else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
                    mLastBias = mCurrentFrame.mImuBias;
            }
        }

#ifdef REGISTER_TIMES
        std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now();

        double timeLMTrack = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLMTrack - time_StartLMTrack).count();
        vdLMTrack_ms.push_back(timeLMTrack);
#endif

        // Update drawer
        mpFrameDrawer->Update(this);
        if(!mCurrentFrame.mTcw.empty())
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
		
		// 如果跟踪成功或者recent_lost,更新恒速模型，计算上一帧到当前帧的位姿变换
        if(bOK || mState==RECENTLY_LOST)
        {
            // Update motion model
            if(!mLastFrame.mTcw.empty() && !mCurrentFrame.mTcw.empty())
            {
                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;
            }
            else
                mVelocity = cv::Mat();

            if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
                mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

            // Clean VO matches 清除恒速跟踪中的临时地图点
            for(int i=0; i<mCurrentFrame.N; i++)
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(pMP)
                    if(pMP->Observations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                    }
            }

            // Delete temporal MapPoints 清除观测不到的地图点
            for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            mlpTemporalPoints.clear();

#ifdef REGISTER_TIMES
            std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now();
#endif
			//判断是否需要插入关键帧
            bool bNeedKF = NeedNewKeyFrame();




            // Check if we need to insert a new keyframe 插入关键帧(对双目或rgb-d会产生新的地图点)
            if(bNeedKF && (bOK|| (mState==RECENTLY_LOST && (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO))))
                CreateNewKeyFrame();

#ifdef REGISTER_TIMES
            std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now();

            double timeNewKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndNewKF - time_StartNewKF).count();
            vdNewKF_ms.push_back(timeNewKF);
#endif

            // 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. Only has effect if lastframe is tracked
            for(int i=0; i<mCurrentFrame.N;i++)
            {
                if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        //若：关键帧小于5个，或使用imu且imu未初始化时：系统重置活跃子图
        //否则 创建新地图
        if(mState==LOST) 
        {
            if(pCurrentMap->KeyFramesInMap()<=5)
            {
                mpSystem->ResetActiveMap();
                return;
            }
            if ((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO))
                if (!pCurrentMap->isImuInitialized())
                {
                    Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
                    mpSystem->ResetActiveMap();
                    return;
                }

            CreateMapInAtlas();
        }

        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;

        mLastFrame = Frame(mCurrentFrame);
    }



	// 保存当前关键帧相对参考帧的位姿，参考kf、当前帧时间戳，当前帧状态
    if(mState==OK || mState==RECENTLY_LOST)
    {
        // Store frame pose information to retrieve the complete camera trajectory afterwards.
        if(!mCurrentFrame.mTcw.empty())
        {
            cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
            mlRelativeFramePoses.push_back(Tcr);
            mlpReferences.push_back(mCurrentFrame.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);
        }

    }
}