ORB_SLAM3：单目+IMU初始化流程梳理

单目+IMU模式下，前面的一些配置完成后，处理第一帧图像时：

1、每帧图像都会调用该函数：

clahe->apply(im,im);

目的：使灰度直方图分布较为均匀，从而使整体对比度更强，便于后面特征点的提取等工作；

2、第一帧图像（ni=0）时无IMU数据（vImuMeas容器为空），进入下面的这个函数：

SLAM.TrackMonocular(im, tframe, vImuMeas);

该函数先是对一些状态进行判断，做出相应的处理。当有IMU数据时，将IMU数据存储到队列mlQueueImuData中，其主要任务是计算当前帧相机的位姿，通过调用下面这个函数来完成；

Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);

3、先是将彩色图像转为单通道灰度图像，然后按照是否有IMU和跟踪状态（mState）来构造Frame类，此时跟踪状态mState==NO_IMAGES_YET。当前帧构造Frame类时需要提取5*1500个特征点，初始化成功后只需要提取1500个特征点；

4、Frame构造函数中先是完成帧的ID赋值：

mnId = nNextId++;    // 先赋值再自增1

然后将金字塔的相关参数赋值给成员变量，接下来是对图像进行特征点提取，这时应用的是仿函数，运行的函数为：

int ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints,
                                OutputArray _descriptors, std::vector &vLappingArea)

上面仿函数一些注意的点是：

1）使用四叉树的方式计算每层图像的特征点并进行均匀分配，均匀的特征点可以提高位姿计算精度；2）在计算特征点的描述子之前先对图像进行高斯模糊，主要是为了避免图像噪声的影响；

3）最后需要将金字塔中非0层的特征点坐标恢复到原图像（第0层）的坐标系下，至于界限的意思我没有看懂，后面也不清楚有什么用。

5、提取完特征点后先用OpenCV的矫正函数对特征点去畸变，然后计算去畸变后图像边界（此过程一般是在第一帧或者是相机标定参数发生变化之后进行），最后将特征点分配到图像网格中（每个固定大小的网格记录其里面特征点的索引），目的是加快后期的特征点匹配。

当第一帧图像进Tracking::Track()时：

1、此时mpLastKeyFrame=NULL、mbCreatedMap=false，将跟踪状态mState更新为NOT_INITIALIZED；

2、进行单目初始化，此时mbReadyToInitializate=false，创建初始化关键帧要求当前帧的特征点数多于100个；

3、当前帧满足创建初始化关键帧条件后，更新mInitialFrame、mLastFrame和用于记录"当前帧"所有特征点的mvbPrevMatched，将容器mvIniMatches的所有值都设置为-1，该容器的长度为当前帧特征点的个数；

4、mpImuPreintegratedFromLastKF的Bias设置为0，*mpImuCalib是在读取IMU的yaml文件时设置的。初始化当前帧预积分mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF，最后将mbReadyToInitializate设为true，意为已经创建初始化参考帧，当前帧的变换矩阵Tcw为单位矩阵。

当第二帧图像进Tracking::Track()时：

1、此时有IMU数据，mState=NOT_INITIALIZED、mpLastKeyFrame=NULL，当有异常的时间戳时，需要进行处理；

2、对IMU数据进行预积分，对每帧IMU数据有两种预积分，一种是相对于上一帧，另一种是相对于上一个关键帧；

注：在Frame构造函数中将当前帧的mpPrevFrame=*mLastFrame，陀螺仪输出角速度（rad/s）,加速度计输出加速度（m/s2）。

void Tracking::PreintegrateIMU()  // 要求当前帧的上一帧（mpPrevFrame）不为NULL且mlQueueImuData里有数据

该函数主要进行的工作是：

1）进入while(true)中，将符合下面这2个条件的IMU数据存入mvImuFromLastFrame容器中；

条件：a.当前IMU时间戳 ≥ 上一帧时间戳 - mImuPer（0.001s）

b.当前IMU时间戳 ＜ 当前帧时间戳 - mImuPer（0.001s）

结束while(true)的条件：当前IMU时间戳不满足上面的2个条件或mlQueueImuData容器为空；

2）构造IMU预积分处理器（pImuPreintegratedFromLastFrame）,偏置使用上一帧的偏置，每帧的mImuCalib都相同（其是在读取IMU的yaml文件时设置的）；

3）第一帧IMU数据和最后一帧IMU数据需要单独处理，中间的IMU数据直接处理，主要就是计算前后2帧IMU的平均加速度（acc）、平均角速度（angVel）和两帧之间的时间差（tstep0）；

4）mpImuPreintegratedFromLastKF是在Tracking::ParseIMUParamFile(cv::FileStorage &fSettings)中设置的，其中偏置都设置为0；

5）最开始时（即当前帧的上一帧）的dR为单位矩阵，dP、dV均为0。主要计算当前帧相对于上一帧和上一关键帧之间的dP、dV和dR，计算JPa、JPg、JVa、JVg和JRg，最后计算出该时间段的协方差矩阵C；

mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc, angVel, tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);

6）更新当前帧的mpImuPreintegratedFrame（基于上一帧的IMU预积分处理器指针）、mpImuPreintegrated（基于上一关键帧的IMU预积分处理器指针）和mpLastKeyFrame（上一个关键帧指针），最后将当前帧的mbImuPreintegrated设为true，意为当前帧完成了预积分。

当第二帧图像进Tracking::MonocularInitialization()时：

1、此时单目初始化器（mInitialFrame）已创建，mbReadyToInitializate=true，要求当前帧特征点超过100个或mSensor == System::IMU_MONOCULAR且当前帧上一帧的时间戳比单目初始化参考帧的时间戳大于1秒；

2、满足上面的条件后对当前帧与初始化参考帧进行特征点匹配，mvIniMatches容器的索引为初始化参考帧的特征点序号，若初始化参考帧中的该特征点与当前帧有匹配，则该容器对应的值为当前帧特征点序号，否则为-1；

3、先是对两帧中的特征点进行校正，然后通过RANSAC（循环200次）双线程计算基础矩阵F和单应矩阵H，分别记录得分最高的SH和SF对应的H和F矩阵，最后通过条件来判断是通过基础矩阵F还是单应矩阵H进行重建；

4、重建的意思是通过F或H恢复R、t以及满足相应条件恢复出的三维点的个数，最后输出满足相关条件最好的Tcw、重建的地图点和初始化参考帧中哪些特征点成功重建了地图点（存入容器vbTriangulated中）；

5、将初始化参考帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵，最后设置当前帧位姿（Tcw）。

Tracking::CreateInitialMapMonocular()函数：

1、将初始化参考帧和当前帧都设置为关键帧，将初始化参考帧的mpImuPreintegrated（基于上一关键帧的IMU预处理器指针）设置为NULL；

2、将初始化参考关帧和当前关键帧的描述子转为BoW，主要是完成mBowVec和mFeatVec容器的赋值，其中mBowVec容器的数据类型为map，第一个数据为单词id，第二个值为权重（weight），如果图像中多个特征点对应同一个单词，则weight会叠加。mFeatVec容器的数据类型为map，第一个数据为节点id（L=2时），第二个数据类型为vector容器，其里面的内容为该节点id包含的描述子索引，该容器的目的是为后面特征点匹配进行加速；注：只有关键帧才会把其的描述子转为Bow

3、用初始化得到的3D点来生成地图点MapPoints，其中mvIniP3D容器中的数据为世界坐标系下点的坐标 ：

1）用3D点（世界坐标系下）来构造地图点；

2）为该地图点添加属性：

a.更新观测到该MapPoint的关键帧，容器mObservations中第一个元素为关键帧指针，第二个元素为该地图点对应此关键帧中的特征点索引，同时更新该地图点的观测数目（nObs），双目和RGB相机：nObs=nObs+2，单目相机：nObs=nObs+1；

b.从众多关键帧中能观测到该MapPoint的特征点中挑选最有代表性的描述子，地图点最好的描述子与其它能观测到该地图点的特征点的描述子应该具有最小的距离中值；注：由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前MapPoint最有代表性的描述子。

c.更新该MapPoint平均观测方向以及观测距离的范围。

3）更新关键帧间的连接关系，每个边有一个权重，边的权重是该关键帧与当前关键帧公共3D点的个数；

4）全局BA优化，优化当前关键帧的位姿和MapPoint；

单目初始化之后，得到的初始地图中的所有点都是局部地图点。

5）mState = OK，此时单目初始化全部完成。