ORB-SLAM3：单目+imu 详细代码解读

输入文件

./Examples/Monocular-Inertial/mono_inertial_euroc
./Vocabulary/ORBvoc.txt
./Examples/Monocular-Inertial/EuRoC.yaml 存储相机/imu等初始化参数
./Datasets/EuRoC/MH01
./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt 存相机时间戳
dataset-MH01_monoi

imu0/data.csv中存储imu数据，时间戳，陀螺仪x y z，加速度x y z

#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]
1403636579758555392,-0.099134701513277898,0.14730578886832138,0.02722713633111154,8.1476917083333333,-0.37592158333333331,-2.4026292499999999

cam0/data.csv中存储相机数据，时间戳，图片名字

#timestamp [ns],filename
1403636579763555584,1403636579763555584.png

两个相机的时间戳之间大概会有10个imu数据

入口函数/框架结构

【1】Examples/Monocular-Inertial/mono_inertial_euroc.cc

主要功能：
（1）读入数据，得到时间戳、所有帧、帧间imu数据
（2）新建System函数，执行TrackMonocular()

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

//执行重点，输入图片帧，和对应图片帧的imu数据（10个左右），这两个数据在此文件中处理读出
SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial

【2】system.cc --- TrackMonocular()

主要功能：
（1）执行GrabImageMonocular()
（2）返回相机位姿

   // 如果是单目+imu模式，把IMU数据存储到mlQueueImuData中
   if (mSensor == System::IMU_MONOCULAR)
       for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
           mpTracker->GrabImuData(vImuMeas[i_imu]);
   // 开始跟踪，返回相机位姿
   Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);//mpTracker为三个主线程之一
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;//记录跟踪状态
   mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//当前帧的地图点
   mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//当前帧的去畸变后关键点
   return Tcw;//返回世界坐标系到相机的位姿

【3】tracking.cc --- GrabImageMonocular()

主要功能：
（1）构建Frame表征当前帧，用特这点描述子来表示当前帧
（2）调用了Track()函数

//开始跟踪 单目
Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, string filename)
{
    //将彩色图像转为灰度图像 mImGray

    //构造Frame，同时完成特征点的提取、计算词袋等操作，mCurrentFrame用图片帧里的特征点来表征一帧图像
    //imu模式的Frame构造函数  比纯视觉多了&mLastFrame,*mpImuCalib两项
	mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);

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

    lastID = mCurrentFrame.mnId;
    Track();//最主要的函数

    return mCurrentFrame.GetPose();
}

【4】tracking.cc --- Track() 函数主体部分
下面为我认为的主体部分（还有纯定位部分没有考虑进去）：
（1）imu预积分PreintegrateIMU()
（2）单目imu初始化MonocularInitialization()；
（3）初始化完成进行跟踪，跟踪同步建图CheckReplacedInLastFrame()，两种跟踪模式【纯用关键帧TrackReferenceKeyFrame()，融合imu的跟踪TrackWithMotionModel()】；
（4）跟踪临时丢失用imu复原；
（5）跟踪完全丢失重置地图或新建地图；
（6）跟踪成功，TrackLocalMap()进行优化位姿，增加地图点

();//imu模式下进行imu预积分
if(mState==NOT_INITIALIZED){
	MonocularInitialization();//
}else{//已经初始化完成，正常定位模式
	if(mState==OK){
		CheckReplacedInLastFrame();//检查上一帧被替换的地图点，将地图点替换成新的地图点
			//如果运动模型是空且imu未初始化 || 刚刚完成重定位
		if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
		    bOK = TrackReferenceKeyFrame(); //跟踪参考关键帧  估计参考关键帧和当前帧之间的位姿，都没有IMU的参与 
		else
		    bOK = TrackWithMotionModel(); //否则用恒速模型进行跟踪，IMU,失败则再次跟踪参考关键帧
		    if(!bOK)
		        bOK = TrackReferenceKeyFrame();
		if (!bOK)//如果跟踪失败，bOK==false      
				mState = LOST;
				mState = RECENTLY_LOST;//地图中关键帧的数量>10
	}
	else if(mState == RECENTLY_LOST){
		PredictStateIMU();//利用IMU计算位姿
		//有两种情况会用到此函数：（a）视觉跟丢时用imu预测位姿；（b）imu模式下，恒速模型跟踪时提供位姿初始值
		//imu进行跟踪，最多跟5s，还失败就设置为LOST
	}
	else if(mState == LOST){
		mpSystem->ResetActiveMap();//如果关键帧数量小于10，则重置地图
		CreateMapInAtlas();//否则新建地图
	}
}
TrackLocalMap();//优化当前帧的位姿（跟踪成功，则更新局部地图，寻找更多的匹配）

【5】tracking.cc --- TrackLocalMap() 主体部分
这个函数主要是利用局部窗口的关键帧和地图点，为当前帧找到更多的匹配地图点，再进行位姿优化，使得位姿更加准确。
以下三个优化函数都在optimizer.cc中

UpdateLocalMap();//更新局部地图，共视关键帧，共视地图点
SearchLocalPoints();
//imu没有初始化，或者刚刚重定位
PoseOptimization(&mCurrentFrame);
//地图未更新时（与上一帧距离近、误差小）用PoseInertialOptimizationLastFrame()
PoseInertialOptimizationLastFrame(&mCurrentFrame); 
//地图更新时用（关键帧优化了，和上一阵相比误差更小）PoseInertialOptimizationLastKeyFrame()
PoseInertialOptimizationLastKeyFrame(&mCurrentFrame);

【6】tracking.cc --- PreintegrateIMU()
IMU预积分，主要实现在IMUType.cc中，IntegrateNewMeasurement(acc,angVel,tstep)
？？目前不明白IntegrateNewMeasurement的具体操作，好像只积分了一个imu数据，怎么就能表示到上一关键帧的呢？？

//在IMUType.cc中有Preintegrated构造函数
IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
//对于n个imu数据，要进行n-1次计算得到两帧之间的预积分量
for(int i=0; i<n; i++){
 	 //开始计算预积分 IntegrateNewMeasurement()
	 mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);//上一关键帧到当前帧的预积分mpImuPreintegratedFromLastKF
	 pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);//上一帧到当前帧的预积分pImuPreintegratedFromLastFrame
	 }
//计算结果加入当前帧
mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;