3——IMU预积分
论文内容：
1. 论文第IV点的B部分IMU预积分，
IMU预积分的作用是计算出IMU数据的观测值（就是IMU预积分值）以及残差的协方差矩阵和雅各比矩阵，那就要清楚的明白为什么要计算这三个量？计算出这三个量为什么就可以和视觉观测值进行耦合？如果你现在回答不出来，请好好想一想自己以前学到的知识，关于视觉的这三个量，视觉中观测值是用来计算残差的（也就是误差），残差的雅各比矩阵是优化中下降的方向（也就是梯度），很少提及的协方差矩阵（但很重要）其实是观测值对应的权值（因为有很多观测值），现在是不是很清楚明白了？具体使用来说，这三个量为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。原始陀螺仪和加速度计的观测值数据：
第一个式子等式左边是加速度测量值（你可以从加速度计中读到的值），等式右边是加速度真实值（其实就是准确的值，我们需要得到的是这个真实值）加上加速度计的偏置、重力加速度和加速度噪声项。第二个式子等式左边是陀螺仪测量值，等式右边是陀螺仪真实值加上陀螺仪偏置和陀螺仪噪声项。这里的值都是IMU（body）帧坐标系下的。这里假设噪声是服从高斯正态分布，而偏置服从随机游走模型。
由上面最原始的式子积分就可以计算出下一时刻的p、v和q:
这里等式左边的值都是世界坐标系下（W）bk+1帧的值。从整个式子可以看出来这里的状态传播需要bk帧下的旋转，平移和速度，当这些开始的状态发生改变的时候，就需要重新传播IMU观测值，也就是说状态传播方程要重新计算和修改，我们想要一次性就求出bk和bk+1之间的状态传播，因此选用预积分模型（其实这里我也没有完全搞明白，但是有一点是明白的，这里是在世界坐标系下求解状态，但是由条件里需要世界坐标系下的旋转，明显冲突啊，因此可以使用预积分将世界坐标系下的状态转换到IMU的bk帧坐标系下，最初提出预积分的外国大牛市将世界坐标系转换到求状态的变化量，其实两者的原理都一样，预积分求的值都是变化量），两边同时乘以世界到bk帧坐标系的转换，如下图所示，然后提出等式右边只与加速度和角速度有关的量进行积分，如公式5和6
到这里其实只要求出公式6中的积分值，真的预积分的值就得到了，这里bk是参考帧，从式6中可以看出，在bk到bk+1帧间，这里要求的三个临时状态量只与IMU的偏置有关系，而与其他状态无关，也就是说每个式子相当于一个二元一次方程（f(x,y)=ax+by+c,x相当于加速度计偏置，y相当于陀螺仪偏置），这里就是为了求解这个二元一次方程，当这里的偏置变化特别小的时候，我们可以使用一阶线性展开来调整临时状态量，如下公式12 所示：
所以要想求出这个临时的状态量，就必须求出等式右边的两部分值，第一个部分还是原来的积分形式（就像公式6那样），是预积分的主体，论文中使用的是最简单的欧拉积分法进行展开（取第i时刻值的斜率乘以时间差加上i时刻的初值，就得到i+1时刻的值），但是在代码中作者也提出了采用的是中值积分（顾名思义这里的斜率取得是i和i+1中点（2i+1）/2的时刻斜率）.公式7是采用欧拉积分的结果。这里前面有一定的说明，一开始abkbk，bbkbk等是零，旋转是单位旋转，注意整个过程把噪声设为0
这里解释一下，实际上bk和bk+1间有很多个时刻，这里的计算是先设abkbk，也即abk1为0 然后计算abk2 然后计算abk3 直到abkend
第二部分其实就是对应的一阶偏导（对加速度计偏置和陀螺仪偏置的），一阶偏导的求法在下面进行介绍，到这里我们已经求出了临时状态量的测量值，也就可以求出状态量测量值。
论文到这一步预积分其实已经做了一半了，也就是完成了测量值的求解，还差什么呢？当然是协方差矩阵了，下面重点求解协方差矩阵，顺便把上面没有求出的陀螺仪和加速度计偏置的雅各比矩阵求出来。
如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到SLAM中的神作state estimation for robotic，建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。也就是说还是需要前面求解状态测量值的公式6。注意代码中真正求解公式6使用的是中值法，所以为了和代码中相一致，下面的求解过程我也才用中值法的方式，为求解需求我们先补充点干货：
首先需要将上面的四自由度的旋转转换成三个维度的状态量，这是由于四自由度的旋转存在过参数化的情况，因此将误差看成是一个扰动定义式8：
然后有下面的两张图定义出来离散状态下的预积分过程：
最后得到图中的线性误差状态传播模型，由此得到IMU预积分测量值的协方差矩阵和雅各比矩阵，预积分的雅各比矩阵直接代入到公式12中计算出更加精确的传播状态值，而协方差矩阵自然是在后端优化中使用。
需要格外注意上面求出的雅各比矩阵是预积分值的雅各比，我们还需要求一个IMU残差的雅各比矩阵，WHAT？还有两个雅各比矩阵，惊不惊喜意不意外？但情况确实如此，所以还不赶快从床上爬起来撸一把公式。上面求得的雅各比矩阵是用来计算预积分值时用到的，下面要求的IMU残差的雅各比矩阵是在紧耦合的时候做下降梯度，在最前面已经提到过。在求残差的雅各比之前，再提一下残差是如何计算的吧，预积分相当于测量值（就是真值，因为没有比这个更准确的值了，那当然就是真值了），要估计的状态就是估计值，所以预积分测量值减去状态估计值就是残差，在后面会提到需要估计的IMU估计值有p,v,q,ba,bg。P和q的估计值初始值比较好得到（和视觉相关，可以直接用视觉的初值），而v,ba,bg这三个量的估计值初始值就比较难得到了，因为视觉没有这三个初始量，就会用到下面的联合初始化得到初始的这三个量。下面直接上残差公式和要优化的状态量：
需要求解的残差雅各比矩阵是残差对估计状态量的一阶偏导，残差向量有三个，状态向量有2*5=10个。所以需要计算残差向量对状态向量的一阶偏导。首先需要提出的是对于偏置求偏导是比较复杂的，所以对于预积分的计算采取的是一阶泰勒展开，这样就相对简单了。也就是论文中的公式（12）。
可以看出旋转四元数残差包含的状态量只有qi,qj,big这三个变量，也就是一个三元函数求偏导过程。
（2）速度残差的雅各比矩阵
未完待续，其实可以自己推导试试。
（3）位移残差的雅各比矩阵
未完待续，其实可以自己推到试试。
到这里恭喜你已经完成了数据前端处理的所有步骤，下面直接进入初始化的过程吧！
1. estimator_node.cpp系统入口
首先初始化设置节点vins_estimator，同时读取参数和设置相应的参数，为节点发布相应的话题，为节点订阅三个话题，分别用来接收和保存IMU数据、图像特征数据和原始图像数据，分别是在三个回调函数中imu_callback、feature_callback和raw_image_callback，每当订阅的节点由数据送过来就会进入到相应的回调函数中。
（1） 接收IMU数据
imu_callback函数中首先执行imu_buf.push(imu_msg);将IMU数据保存到imu_buf中，同时执行con.notify_one();唤醒作用于process线程中的获取观测值数据的函数，这里唤醒以及互斥锁的作用很重要到下面真正要使用的时候在详细讨论，然后预测未考虑观测噪声的p、v、q值，同时将发布最新的IMU测量值消息（pvq值），这里计算得到的pvq是估计值，注意是没有观测噪声和偏置的结果，作用是与下面预积分计算得到的pvq（考虑了观测噪声和偏置）做差得到残差。
（2） 接收原始图像和图像特征点数据
feature_callback和raw_image_callback函数中主要是将特征数据和原始图像数据分别保存到feature_buf和image_buf中，在feature_callback也用到了con.notify_one()和互斥锁;。
2. process()处理观测值数据线程
（1） 得到观测值（IMU数据和图像特征点数据）
定义观测值数据类measurements，包含了一组IMU数据和一帧图像数据的组合的容器，这里比较有意思的是使用了互斥锁和条件等待的功能，互斥锁用来锁住当前代码段，条件等待是等待上面两个接收数据完成就会被唤醒，然后从imu_buf和feature_buf中提取观测数据measurements = getMeasurements()，需要注意的是在提取观测值数据的时候用到的互斥锁会锁住imu_buf和feature_buf等到提取完成才释放，也就是说在提取的过程中上面两个回调函数是无法接收数据的，同时上面两个回调函数接收数据的时候也使用了互斥锁，锁住了imu_buf和feature_buf，这里也不能提取imu_buf和feature_buf中的数据。因此整个数据获取的过程是：回调函数接收数据，接收完一组数据唤醒提取数据的线程，提取数据的线程提取完数据后，回调函数就可以继续接收数据，依次往复。这就是线程间通信的曼妙啊！
1） getMeasurements()返回观测值数据
函数的作用顾名思义，就是得到一组IMU数据和图像特征数据组合的容器。首先保证存在IMU数据和图像特征数据，然后还要判断图像特征数据和IMU数据是否对齐。这里使用的是队列数据结构（先进先出front是先进的数据，back是后进的数据），需要满足两个条件就能保证数据对齐，第一是IMU最后一个数据的时间要大于图像特征最开始数据的时间，第二是IMU最开始数据的时间要小于图像特征最开始数据的时间。满足数据对齐就可以数据从队列中按对齐的方式取出来。这里知道把缓存中的图像特征数据或者IMU数据取完，才能够跳出此函数，返回数据。
（2） 处理IMU数据和图像特征数据
步骤1：处理IMU数据
遍历调用send_imu(imu_msg)将单个IMU数据的dt，线加速度值和角加速度值计算出来送给优化器处理，优化器调用estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry,rz));方法。
1）Estimator::processIMU(doubledt, const Vector3d &linear_acceleration, const Vector3d&angular_velocity)处理IMU数据方法
步骤1调用imu的预积分，调用push_back函数，函数中将时间，加速度和角速度分别存入相应的缓存中，同时调用了propagation函数 ,计算对应的状态量、协方差和雅可比矩阵
①propagate(double _dt, const Eigen::Vector3d &_acc_1, constEigen::Vector3d &_gyr_1)
预积分传播方程，在预积分传播方程propagate中使用中点积分方法midPointIntegration计算预积分的测量值，中点积分法中主要包含两个部分，分别是得到状态变化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新协方差矩阵和雅可比矩阵（注意，虽然得到了雅各比矩阵和协方差矩阵，但是还没有求残差和修正偏置一阶项的状态变量），由于使用的是中点积分，所以需要上一个时刻的IMU数据，包括测量值加速度和角速度以及状态变化量，初始值由构造函数提供。需要注意的是这里定义的delta_p等是累积的变化量，也就是说是从i时刻到当前时刻的变化量，这个才是最终要求的结果（为修正偏置一阶项），而result_delta_q等只是一个暂时的变量，最后残差和雅可比矩阵、协方差矩阵保存在pre_integrations中，还有一个函数这里暂时还没有用到，是在优化的时候才被调用的，但是其属于预积分的内容，evaluate函数在这个函数里面进行了状态变化量的偏置一阶修正以及残差的计算。
步骤2预积分公式（3）未考虑误差，提供imu计算的当前旋转，位置，速度，作为优化的初值
步骤2：处理图像特征数据
这里进来的数据不是图像数据，而是前面已经跟踪匹配好的归一化平面坐标。将当前帧的特征存放在image中，image的第一个元素类型是相机的编号，代表是第几帧图像（从0开始），第二个元素是归一化特征点坐标和特征点编号（从1开始），然后直接进入到处理图像特征数据的线程中estimator.processImage(image, img_msg->header)。
1）Estimator::processImage(constmap
首先对进来的图像特征数据根据视差判断是否是关键帧，选择丢弃当前帧（但保留IMU数据）或者丢弃滑动窗口中最老的一帧。
步骤1：将图像数据和时间存到图像帧类中：首先将数据和时间保存到图像帧的对象imageframe中（ImageFrame对象中包含特征点，时间，位姿R，t，预积分对象pre_integration，是否是关键帧）,同时将临时的预积分值保存到此对象中（这里的临时预积分初值就是在前面IMU预积分的时候计算的），然后将图像帧的对象imageframe保存到all_image_frame对象中（imageframe的容器），更新临时预积分初始值。
步骤2：标定相机和IMU的外参数：接着如果没有外部参数就标定外部参数，参数传递有的话就跳过这一步（默认有，如果是自己的设备，可以设置为2对外参进行在线标定）。
步骤3：初始化系统同时进行BA优化：当求解器处于可初始化状态时（初始状态是可初始化，初始化成功就设置为不可初始化状态），判断当前frame_count是否达到WINDOW_SIZE，确保有足够的frame参与初始化，这里的frame_count是滑动窗口中图像帧的数量，一开始被初始化为0，滑动窗口总帧数是10。有外部参数同时当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作。
步骤3.1：initialStructure()系统初始化，首先初始化Vision-only SFM，然后初始化Visual-Inertial Alignment，构成整个初始化过程。
①保证IMU充分运动，通过线加速度判断，一开始通过线加速度的标准差（离散程度）判断保证IMU充分运动，加速度标准差大于0.25则代表imu充分激励，足够初始化。
②纯视觉初始化，对SlidingWindow中的图像帧和相机姿态求解sfm问题，这里解决的是关键帧的位姿和特征点坐标。
步骤1.首先构建SFMFeature对象sfm_f，SFMFeature数组中包含了特征点状态（是否被三角化），id，2d点，3d坐标以及深度，将特征管理器中的特征信息保存到SFMFeature对象sfm_f中sfm_f.push_back(tmp_feature)。
步骤2.接着由对极约束中的F矩阵恢复出R、t，主要调用方法relativePose(relative_R, relative_T, l)。relativePose方法中首先通过FeatureManeger获取（滑动窗口中）第i帧和最后一帧的特征匹配corres，当corres匹配足够大时，考察最新的keyFrame和sliding window中某个keyFrame之间有足够feature匹配和足够大的视差（id为l=i），满足这两个条件，然后这两帧之间通过五点法恢复出R，t并且三角化出3D的特征点feature point，这里是使用solveRelativeRT(corres, relative_R, relative_T)，solveRelativeRT方法定义在solv_5pts.cpp类中，由对极约束中的F矩阵恢复出R、t，直接调用opencv中的方法，没什么好说的，这里值得注意的是，这种relativePose得到的位姿是第l帧的，第l帧的筛选是从第一帧开始到滑动窗口所有帧中一开始满足平均视差足够大的帧，这里的第l帧会作为参考帧到下面的全局SFM使用。到这里就已经得到图像的特征点2d坐标的提取，相机第l帧和最后一帧之间的旋转和平移（注意暂时还没有得到特征的3d点坐标），有了这些信息就可以构建全局的SFM类GlobalSFM sfm，在这里调用sfm.construct(frame_count + 1, Q, T,l,relative_R, relative_T,sfm_f, sfm_tracked_points)，这里以第l帧作为参考帧，在进行PNP求解之前，需要判断当前帧数要大于第l帧，这保证了第l帧直接跳过PNP步骤，首先执行下面的第l帧和最后一帧的三角化，得到共视的特征点，供下面第l+1帧和最后一帧求解PNP，然后利用pnp求解l+1帧到最后一帧的位姿R_initial, P_initial，最后的位姿都保存在Pose中，一次循环，得到l+1，l+2…n-1帧的位姿。跳出步骤2 的循环后，至此得到了l+1，l+2…n-1帧的位姿以及l+1，l+2…帧与n-1帧的特征点三角化。然后再三角化l帧和i帧（在第l帧和最后一帧之间的帧）之间的3d坐标，（这里不明白为什么要做两次，是可以三角化出更多的特征点吗？？？？），接着PNP求解l-1，l-2…0帧和l帧之间的位姿已经三角化相应的特征点坐标，最后三角化其他所有的特征点。至此得到了滑动窗口中所有相机的位姿以及特征点的3d坐标。第6部就是进行BA优化，使用的是ceres优化位姿和特征点，这里可以参考视觉SLAM第十讲中的内容，优化方式相同。
步骤4：visualInitialAlign中调用VisualIMUAlignment方法，真正的视觉惯性联合初始化，imu与视觉对齐，获取绝对尺度等。这个方法定义在initial/initial_alignment.h中。
步骤4.1：solveGyroscopeBias计算陀螺仪偏置，整个方法的计算模型由论文中给出，使用LTLD方法求解最小二乘问题，delta_bg = A.ldlt().solve(b);这里A +=tmp_A.transpose() * tmp_A，b += tmp_A.transpose() * tmp_b，其实就是处理AT*A*x=AT*b问题，一般的最小二乘问题直接处理Ax=b也就是Ax-b=0即可，这里是使用LDLT方法，两边同乘以A矩阵的转置得到的AT*A一定是可逆的，因此就可以直接两边同乘以其逆即可，相应的说明详见LDLT方法。得到陀螺仪偏置之后将其值保存到前面定义的Bgs[]中，最后在重新计算一次预积分。
步骤4.2：LinearAlignment计算尺度，重力加速度和速度。论文中给出的公式是相邻两个速度的模型，映射到整个n+1个速度模型中，A矩阵一定是一个正定矩阵（实对称矩阵），代码中定义的A和b即是最总的H和b，tmp_A和tmp_b相邻速度间的临时变量。最后的求解方法：x = A.ldlt().solve(b)；然后调用RefineGravity重新计算重力加速度方向，得到最优解。
4——初始化
1. 基于滑动窗口的纯视觉单目初始化
在介绍纯视觉初始化前我们首先讲一讲为什么要初始化？初始化要做什么？以及初始化的作用？我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度（包括大小和方向），对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义（其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉）。所以初始化要做的事其实说起来很简单，就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起（也就是被重力加速度给吸收掉），重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化。话不多说，直接进入正题。
纯视觉初始化在第V点的A部分，首先构建一个滑动窗口，包含一组数据帧。论文中提及使用的是对极几何模型的5点法求解单目相机的相对变换，包括相对旋转和无尺度信息的位移。其实基本上每个单目模型都是使用对极几何在初始化中求解两帧的相对变换，这里需要注意的是旋转是具有尺度不变性的（其实就是单位旋转，不会有尺度信息，你仔细想想是不是？）。然后三角化得到相应的3d点坐标，有这些3d点和滑动窗口中其他的帧的2d点就可以进行PNP求解获得滑动窗口中的所有的位姿和特征点3d坐标，至此，纯视觉初始化就完成了。是不是很简单？当然啊，毕竟只是简单的视觉初始化，而真正复杂的是视觉惯性联合初始化，也就是我们初始化的重点和难点
2. 视觉惯性联合初始化
视觉惯性联合初始化在第V点的B部分，这里作者给定义的名字叫Visual-Inertia Alignment，即视觉惯性联合初始化（而在ORBSLAM2+IMU的论文里，作者定义的名称就叫IMU initialization，即IMU初始化），为什么定义这样一个名词，我觉得有两个意义，第一在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，也就是要联合视觉和惯性的数据。第二这里求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。在具体的讲解初始化每个过程的时候，有必要来个总体的概括，初始化在物理意义上的定义其实就是固有参数的标定，在数学模型上的定义其实就是公式（6）的矩阵方程求解，而公式（6）其实就是来自于最原始的PVQ积分公式，其中Q旋转对应着陀螺仪，而PV对应着加速度计
（1） 陀螺仪偏置标定
旋转我们可以通过两种方式求得，一种是陀螺仪测量值，一种就是视觉观测值。按照正常的理解两者的大小一定是相等的（假设没有误差），但实际情况肯定有误差，我们就来看看各自的误差。陀螺仪的误差有两部分测量噪声和陀螺仪偏置，噪声暂时可以忽略（毕竟太小），而视觉的误差就只有观测噪声（也可以忽略不管），因此两者差值的绝对值就是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型：
公式15中第一个式子的第一项和第二项作四元数旋转的广义乘积就可以得到相机从bk到bk+1下的相对旋转（bk+1坐标系下），第三项是陀螺仪从bk+1到bk下的相对旋转（bk坐标系下），两者在做广义乘积，就是首先从bk到bk+1旋转，然后再从bk+1到bk旋转，相当于做差（OA+AO=0），第二个式子就是前面预积分提到的一阶线性近似。然后取最小二乘，当然也可以使用SVD分解等方法求解。注意在求得陀螺仪偏置之后要再次将陀螺仪偏置代入到预积分中再求一次预积分的值，会更加精确。
（1） 速度、重力加速度和尺度标定
作者在这里将这三个状态量统一到一个状态向量中，如公式16所示：
速度的是在bk坐标系下的，重力加速度在初始相机坐标系下，就像前面提到的，求解着几个量是由P、V数学模型求得，在滑动窗口中考虑到两个连续关键帧bk和bk+1，下面进行论文中公式17和19的推导：
公式推导之后就会得到论文中的公式17、18和19，我们重点关注下为什么要这样推导，以及推导得到的运动方程关系。首先为什么要进行这样的推导，这完全取决于状态向量的定义方式，我们最终要得到的方程形式左边一定是以状态向量的形式来表达的，而且还要满足其他量都是已知的（从IMU预积分和视觉跟踪得到），因此就需要将方程进行如此的变化，才能满足这样的关系。然后是最后的形式我们可以看到状态向量最终的形式维度是（n+1）*3+3+1，两个连续帧产生的运动方程的维度是3+3+3+1（vbkbk，vbk+1bk+1，gc0，s），比较维度就可以看到最终得到的H矩阵一定是一个正定对称矩阵，因此可以采用快速的Cholesky分解。
（1） 重力优化
上面其实已经得到了重力加速度的大小和方向，这里为什么还需要对重力进行优化呢？理由很简单，这里计算的重力吸收了重力加速度计的偏置，虽然不需要计算重力加速度计的偏置，但重力还是需要优化的，说到优化重力加速度，肯定包含两个量，大小和方向，也就是三个维度，但是一般来说大小是确定已知的（这里设为9.8），因此其实我们要做的就是优化方向，是一个两维的向量，下图是优化重力的方法以及b1,b2单位向量的方向确定模型