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的参与 elsebOK = 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);//上一关键帧到当前帧的预积分mpImuPreintegratedFromLastKFpImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);//上一帧到当前帧的预积分pImuPreintegratedFromLastFrame}
//计算结果加入当前帧
mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;