VINS 所有知识点梳理（不断根据理解去添加内容，理论和程序的结合）

知乎：零碎知识，写的很好

https://www.zhihu.com/question/53571648/answer/137726592

https://zhuanlan.zhihu.com/c_1121353757664964608

VIO/VSLAM理论与实战 https://zhuanlan.zhihu.com/c_1121353757664964608

很好的解析：https://www.zybuluo.com/Xiaobuyi/note/866099

launch, .yaw 等参数理解，以及整体的细节，跟着程序解读一遍

https://blog.csdn.net/qq_41839222/article/details/85793998

知乎，偏向文字的理解

https://zhuanlan.zhihu.com/p/138632367

https://blog.csdn.net/iwanderu/article/details/104617829（整体细致-首推）

https://cloud.tencent.com/developer/article/1005894 （整体，方向的把我比较好）

经典的VINS-Mono程序，对应系统的4大部分：图像和IMU预处理、初始化、后端滑动窗口优化、闭环检测和优化。

VINS代码主要包含在两个文件中，分别是feature_tracker和vins_estimate，feature_tracker 的作用是接收图像，使用KLT光流算法踪；vins_estimate包含相机和IMU数据的前端预处理（也就是预积分过程）、单目惯性联合初始化（在线的标定过程）、基于滑动窗口的BA联合优化、全局的图优化和回环检测等。

如图所示，第一部分是Measuremen Preprocessing：观测值数据预处理，包含图像数据跟踪IMU数据预积分；第二部分是Initialization：初始化，包含单纯的视觉初始化和视觉惯性联合初始化；第三部分Local Visual-Inertia BA and Relocalization：局部BA联合优化和重定位，包含一个基于滑动窗口的BA优化模型；第四部分Global Pose Graph Optimization：全局图优化，只对全局的位姿进行优化；第五部分Loop detection：回环检测。  

当然，一些预备的数学知识还是需要的，比如：...,  最小二乘，高斯牛顿，SVD等。

          提取图像Harris角点（100-300） -【因为图像本身有几万个像素，但是计算机能力有限，并为了更好的利用图像的特征信息，采用提取图像特征点的方法进行图像识别】，(直接法/特征点法)金字塔光流 （LK2）跟踪相邻帧，RANSAC, RANSAC1 去除异常点-【这一步很关键，因为如果是错误的匹配点，后面的视觉旋转估计，对齐等都会是错误的】，还涉及到关键帧的选取（两种方法记住）；需要注意的是，旋转和平移都会产生视差（不只是平移），当出现纯旋转的时候特征点无法被三角化（因为纯旋转的话，对极几何中的三角形才会出现,对极几何得到的本质矩阵E=t^R, 平移t=0时，本质矩阵也为0，R也没有办法求出，更不会有后面的三角化，因为三角化需要R,t.），无法计算出旋转值，也就无法计算跟踪点间的平均视差，故采用短时的陀螺仪观测值来补偿旋转，从而计算出视差，这一过程只应用到平均视差的计算，不会影响真实的旋转结果；最后将跟踪到的特征点push到图像队列中，并通知后端进行处理。更加细节的程序看这里，可以把理论和代码结合，理解更深刻。

【注意的地方】

1、频率控制，保证每秒钟处理的image不多于FREQ，这里将平率控制在10hz以内。

2、涉及到3个img(prev_img, cur_img, forw_img)和pts(prev_pts,cur_pts, forw_pts)，两者是相似的。cur和forw分别是LK光流跟踪的前后两帧，forw才是真正的“当前”帧，cur实际上是上一帧，而prev是上一次发布的帧，它实际上是光流跟踪以后，prev和forw根据Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函数.

3、特征点跟踪

首先用cv::goodFeaturesToTrack在第一帧图像上面找最强的150个特征点，非极大值抑制（寻找局部最大值的过程）半径为30。新的特征点都有自己的新的对应的id。然后在下一帧过来时，对这些特征点用光流法进行跟踪，在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每对匹配点，基于校正后的位置，用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域，用cv::goodFeaturesToTrack搜索最强的新的特征点，把特征点数量补上150个。

最后，把剩下的这些特征点，把图像点投影回深度归一化平面上，再畸变校正，再投影到球面上，再延伸到深度归一化平面上，得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配，就是前一帧到这一帧的，一帧帧继承下去。或者生成新的特征点。

4、基于特征点的方法，我们计算出特征点的描述子之后会进行匹配找到点对，而我们希望简化计算，于是有了光流法。具体思路为：找到特征点后，我们不再计算描述子，而是通过光流估计特征点在图像上的运动，从而估计出特征点在下一帧上的位置，从而确定点对。所以光流法的核心是如何估计特征点的运动。

IMU预积分的残差、Jacobian和协方差 processIMU（）+intergrationBase类+imu_factor.h

（1）IMU积分，得到PVQ 位置、速度、旋转；

（2）计算在后端优化中将用到的相邻帧的预积分增量；

（3）计算预积分误差的Jacobian矩阵和协方差。

（1） IMU预积分部分

预积分将世界坐标系下的状态转换到IMU的bk帧坐标系下，最初提出预积分的外国大佬是将世界坐标系转换到求状态的变化量，其实两者的原理都一样，预积分求的值都是变化量），两边同时乘以世界到bk帧坐标系的转换即可。

IMU预积分的作用是计算出IMU数据的观测值（就是IMU预积分值）以及残差的协方差矩阵和雅各比矩阵，那就要清楚的明白为什么要计算这三个量？计算出这三个量为什么就可以和视觉观测值进行耦合？ 想一下！关于视觉的这三个量，视觉中观测值是用来计算残差的（也就是误差），残差的雅各比矩阵是优化中下降的方向（也就是梯度），很少提及的协方差矩阵（但很重要）其实是观测值对应的权值（因为有很多观测值），现在是不是很清楚明白了？具体使用来说，这三个量为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。  如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到SLAM中的神作state estimation for robotic，建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。

 需要注意：  雅各比矩阵有两个：一个是预积分值的雅各比，另一个是IMU残差的雅各比矩阵。预积分值的雅各比是用来计算预积分值时用到的， IMU残差的雅各比矩阵是在紧耦合的时候做下降梯度 。在求残差的雅各比之前，再提一下残差是如何计算的吧，预积分相当于测量值（就是真值，因为没有比这个更准确的值了，那当然就是真值了），要估计的状态就是估计值，所以预积分测量值减去状态估计值就是残差，在后面会提到需要估计的IMU估计值有p,v,q,ba,bg。P和q的估计值初始值比较好得到（和视觉相关，可以直接用视觉的初值），而v,ba,bg这三个量的估计值初始值就比较难得到了，因为视觉没有这三个初始量，就会用到下面的联合初始化得到初始的这三个量。  

视觉、惯导联合优化的是利用松耦合的方式进行的，各自求出自身的旋转等，因为物体视为刚体，因此两次旋转可以看作等价，通过把状态量投影到统一的坐标系下，然后对齐，便可求出bg, ba, v, s等变量。

1、SFM纯视觉估计滑动窗所有帧的位姿和3D路标点逆深度； 

2、SFM与IMU预积分松耦合，对齐求解初始化参数（bg,ba,s,v...）；

（1）1.1外参中的旋转

用机器人手眼标定的方法，计算出外参中的旋转。

qb(k+1)_b(k) 是陀螺仪预积分得到的,  qc(k+1)_c是用8点法对前后帧对应的特征点进行计算得到的。

       1.2 SFM 先在关键帧窗口里面，找到第l帧，第l帧与最后一帧有足够的像素位移，并且能用8点法算出旋转和位移。以l帧的姿态为世界坐标系。先从l帧开始与最后一帧进行三角定位，再用pnp估计出(I帧)下一帧的位姿，下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

【PNP的应用场景有两个，一种是世界坐标系3D点及其在相机坐标系投影的2D坐标点，求解出来的位姿是相机坐标系相对于世界坐标系的；另一是上一帧的3D坐标在下一帧上的2D投影，求出的位姿是下一帧相对于上一帧的】

固定住l帧的位置和姿态，固定住最后一帧的位置。因为这时候的图像位姿和点的位置都不太准，所以用ceres统一一起优化图像位姿和三维点位置，优化重投影误差。优化的测量值是，特征点在每帧中被观察到的位置，可以转成重投影误差约束。有关的自变量是，每帧图像的位姿，特征点的三维坐标。

优化完成之后，即用ceres优化出这些关键帧的位姿和地图点后，再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

程序里面没有求雅克比，而是用自动求导的方法。

（2） 基于滑动窗口的纯视觉单目初始化

 讲一讲为什么要初始化？初始化要做什么？以及初始化的作用？初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度（包括大小和方向），对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义（其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉）。所以初始化要做的事 就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起（也就是被重力加速度给吸收掉），重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化 。

         纯视觉初始化首先构建一个滑动窗口，包含一组数据帧。论文中提及使用的是对极几何模型的5点法求解单目相机的相对变换，包括相对旋转和无尺度信息的位移。其实基本上每个单目模型都是使用对极几何在初始化中求解两帧的相对变换，这里需要注意的是旋转是具有尺度不变性的（其实就是单位旋转，不会有尺度信息，你仔细想想是不是？），至于为什么单目没有尺度信息，我想罗嗦一句，但其实很多学习单目视觉SLAM的人都没有真正搞明白过，单目视觉没有尺度的源头是最开始两帧间的对极几何求位姿，再具体点就是求F/E或者H的时候需要将其降参一位，F从6维降到5维，H从9维降到8维，这里所降的维度就是尺度，而且必须要降。然后三角化得到相应的3d点坐标，有这些3d点和滑动窗口中其他的帧的2d点就可以进行PNP求解获得滑动窗口中的所有的位姿和特征点3d坐标，至此，纯视觉初始化就完成了，真正复杂的是视觉惯性联合初始化，也就是我们初始化的重点和难点。  

（3）视觉惯性联合初始化

视觉惯性联合初始化 定义的名字叫Visual-Inertia Alignment，即视觉惯性联合初始化（而在ORBSLAM2+IMU的论文里，作者定义的名称就叫IMU initialization，即IMU初始化），为什么定义这样一个名词，我觉得有两个意义，第一在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，也就是要联合视觉和惯性的数据。第二这里求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。在具体的讲解初始化每个过程的时候，有必要来个总体的概括，初始化在物理意义上的定义其实就是固有参数的标定，在数学模型上的定义其实就是 矩阵方程求解，就是来自于最原始的PVQ积分公式，其中Q旋转对应着陀螺仪，而PV对应着加速度计 。

在视觉约束和IMU约束中，基本思想是找到优化状态向量，将视觉约束、IMU约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑动窗内所有帧的PVQ、bias。---联想一下GPS/INS松组合，其实是一样的道理，只是优化算法不同而已。

（过程的理解很重要，不要一直想达到重点，一步一个脚印）

 视觉惯性紧耦合系统 才是整个系统的重点，前面所有提及的其实都是松耦合方式，目的是给整个系统提供优化初值和状态，紧耦合系统是将视觉和惯性的原始观测量进行有效的组合，也就是在数据处理前就进行数据融合，VINS中是将滑动窗口内的状态量整合到一个状态向量中，如公式21所示：  

第一个式子是滑动窗口内整个状态向量，其中n是帧数（特征点被观测到的帧数，每一帧中包含的IMU信息），m是滑动窗口中的特征点总数，维度是15*n+6+m，第二个式子xk是在第k帧图像捕获到的IMU状态，包括位姿，速度，旋转，加速度计和陀螺仪偏置，第三个式子是相机外参，λ是特征点深度值得逆（大家一定会问，问什么这里只用特征点深度值的逆，也就是逆深度，主要是考虑稳定和满足高斯系统，不懂得可以参考高博的十四讲）。从状态向量就可以看出xk只与IMU项以及Marginalization有关，特征点的深度值只与camera和Marginalization有关，所以下面建立BA紧耦合模型的时候按照这个思路，下面建立一个视觉惯性的BA优化模型以及鲁棒核函数模型：

 BA优化模型被分成了三个部分，分别是1、Marginalization（边缘化-贺大神）残差部分（从滑动窗口中去掉的位姿和特征点的约束，为什么要进行边缘化，怎么进行边缘化？，Marginalization（边缘化），（边缘化程序讲解2） （边缘化讲解3）（贺大神）优化问题中pose的个数, 从而防止pose和特征的个数随时间不断增加, 使得优化问题始终在一个有限的复杂度内, 不会随时间不断增长。然而, 将pose移出windows时, 有些约束会被丢弃掉, 这样势必会导致求解的精度下降, 而且当MAV进行一些退化运动(如: 匀速运动)时, 没有历史信息做约束的话是无法求解的. 所以, 在移出位姿或特征的时候, 需要将相关联的约束转变成一个约束项作为prior放到优化问题中. 这就是marginalization要做的事情。），2、IMU残差部分（滑动窗口中相邻帧间的IMU产生，估计值和测量值之间的差值就是残差）和3、视觉代价误差函数部分（滑动窗口中特征点在相机下的投影产生，与传统的针孔相机模型定义的重投影误差不同，论文中使用的是单位半球体的相机观测残差，是一个宽视野鱼眼或者全方位相机。），其中鲁棒核函数针对代价函数设定（具体作用请参见视觉SLAM十四讲）。

 【理解1】我们根据运动模型和观测模型建立H矩阵（高斯牛顿法中的JJTJJT）的过程其实就是根据概率图模型（多元高斯分布）建立各个节点变量间的信息矩阵（协方差矩阵的逆）的过程，而边缘化则是去掉概率图中的某一个节点后信息矩阵会发生怎样的变换的问题。

【理解2】边缘化操作并不会改变优化变量的值，而仅仅是改变了优化变量之间的关系，而这个关系就是通过信息矩阵体现的。

DBow进行闭环检测，检测成功后重定位，最后对整个相机轨迹进行闭环优化

论文中主要分为两部分：回环检测与重定位、4-DOF的位姿图优化。

第一部分主要是为了通过回环检测找到当前帧和候选帧的联系，并通过简单的紧耦合重定位将局部滑动窗口移动与过去的位姿对齐。第二部分是为了保证基于重定位结果对过去的所有位姿进行全局优化。

vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,紧耦合重定位三个部分

A、回环检测（只对关键帧）

1、采用DBoW2词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。

2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。

3、VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。

4、单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

B、回环候选帧之间的特征匹配

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。

2、本文提出两步几何剔除法：

1）2D-2D：使用RANSAC进行F矩阵测试，

2）3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。

当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。

2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。  

（感谢博友blowballs）

1、回环优化 如果窗口里存在有回环帧的对应帧的话，则先找到对应帧与回环帧的匹配点id和位置，特征点匹配用的是在一定范围内匹配描述子。特征点匹配完之后，然后只保留这些匹配上的id和特征点位置，给front_pose，id用对应帧的id，特征点位置用回环帧的位置。其实，就是把对应帧的id赋给匹配上的回环帧的特征点。然后给front_pose.loop_pose对应帧的姿态作为初值，然后用窗口里面的点的重投影误差来优化front_pose.loop_pose，点的逆深度，点被观察到的第一帧的位姿。

窗口优化之后，比较对应帧与front_pose.loop_pose的yaw角是否相差太大，如果相对yaw角大于30度或相对位置大于20的话，就认为这是错误的回环，删除这个回环。因为这时候，这个对应帧还在窗口里面，还没有滑出去回环闭环。所以，这样也可以及时把DBow找出来的错误的回环，删掉。

其实，就是把回环帧放到窗口里面来优化，优化出回环帧的位姿，然后再算出回环帧和对应帧的相对位姿。然后把这个相对位姿，作为后面的回环4自由度优化的测量值。具体是，优化出回环帧在窗口里面的位置后，算出对应帧相对于回环帧的位姿。

新的关键帧在加入关键帧数据库的时候，作为对应帧，在关键帧数据库里面去找回环帧。如果用DBOW找到回环帧，使用cur_kf->findConnectionWithOldFrame(old_kf, measurements_old, measurements_old_norm, PnP_T_old, PnP_R_old, m_camera);。先用searchByDes 去找匹配点，然后用cv::findFundamentalMat(un_measurements, un_measurements_old, cv::FM_RANSAC, 5.0, 0.99, status);筛选匹配点，再用对应帧自带的地图点结合solvePnPRansac，得到回环帧的位姿PnP_T_old。PnP_T_old作为回环帧位姿loop_pose的初值，结合继承过来的匹配点measurements_old_norm，再传回当前窗口中优化，problem.AddResidualBlock(f, loss_function, para_Pose[start], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]);，得到优化后的loop_pose。如果优化后的loop_pose相对对应帧的位姿relative_yaw小于阈值，则认为回环正确。而优化后的loop_pose相对回环帧原来的位姿，为relocalize_r和relocalize_t。

在vins-mono的新版本中，新增加了relocalize_r、relocalize_t，其作用是，在大回环起作用的间隙，用relocalize_r、relocalize_t来对位姿进行及时的修正，以更好地保证输出位姿的准确性，以及关键帧输入到关键帧数据库里时的位姿的准确性。因为以前是要等回环帧的对应帧滑出窗口，大回环优化后，才对这两个位姿进行校正的，而现在可以更及时地修正这些位姿，如果有地方想要最快速地得到准确的位姿的话。new KeyFrame(estimator.Headers[WINDOW_SIZE - 2], vio_T_w_i, vio_R_w_i, cur_T, cur_R, KeyFrame_image, pattern_file);这里面的vio_T_w_i是回环优化时的计算前后关键帧的相对位置时用的，所以用的还是窗口中的位姿。而cur_T，也就是T_w_i，是表示校正后的位姿，在输入的时候，会根据relocalize_r校正，大回环优化后，还再校正关键帧数据库里的所有的关键帧位姿。relocalize_r、relocalize_t也是很巧妙的方法，因为它们是根据回环帧和对应帧的图像的相对位姿算出来的回环偏移，其实大回环优化之后得到的回环偏移correct_t，和这个回环偏移relocalize_t，应该相差不大。大回环主要是优化环中间的那些关键帧的位姿，大回环的主要目的在于修正关键帧图PoseGraph。所以，relocalize_r、relocalize_t一开始就很接近最终大回环优化后的回环偏移correct_t。relocalize_t是把回环帧放到当前窗口下优化，算出来的与原来的偏移；correct_t是大回环优化后的对应帧位姿与它原来的位姿的偏移。correct_t准确但计算慢，relocalize_t计算快速且较准确。

 2、回环检测

这里的回环检测，是每3个关键帧检测一帧，相当于是跳两帧。这跟回环检测的速度，和实际关键帧生成的速度，对比有关。因为回环检测的速度总是慢于关键帧生成的速度，所以为了保持回环检测的关键帧不落后于时间，只能跳帧检测。ORBSLAM里面也是这样，但ORBSLAM里面的回环检测判断标准是，一段时间内的关键帧都能匹配上回环，所以ORBSLAM的策略是拿一段时间的关键帧来进行检测。ORBSLAM的回环检测程序，Sleep一段时间，在Sleep的这段时间，收集关键帧，然后开始工作，只针对收集的这些关键帧。工作时，不收集新的关键帧，都跳过。处理完这些关键帧后，又Sleep。

因为回环检测的速度总是慢于关键帧生成的速度，所以为了保持回环检测的关键帧不落后于时间，只能跳帧检测。

窗口里，每3个关键帧，送一个到关键帧数据库里。关键帧数据库里面的每一帧都跟之前的进行检测，看是否有回环。回环检测用的是DBow，这个关键帧上面的用FAST找出来的新的特征点和它在之前被光流跟踪到的特征点，提取描述子，与历史描述子匹配。这帧的描述子以及对应的特征点，跟历史上的描述子以及对应的特征点进行匹配，得到匹配上的特征点。

如果有回环，就等这个对应帧从窗口里滑出，再回环闭环。

找出回环最早开始的帧，然后把这帧的位姿设为固定。

回环闭环的约束条件是，与优化前的相对位姿不能差太大，每帧与前5帧的相对位姿。回环帧和对应帧的相对位姿的权重是5。回环闭环里面，优化的都是4自由度的姿态，回环帧与闭环帧，每帧与它的前5帧的相对姿态。

优化完后，在最后一个对应帧那里，再把世界坐标系也转换一下，然后把剩下的关键帧都转换一下。

回环闭环优化部分的测量值是，回环帧与对应帧的基于图像算出的相对4自由度姿态，就是relative_t和relative_yaw，就是回环帧的loop_pose在窗口中优化后，相对窗口中的对应帧的位姿。每帧图像与它前5帧的相对4自由度姿态。约束为，预测值和测量值之间的差。与此有关的自变量是，每帧图像的4自由度位姿。就像一个项链一样，一串地拉过来。

注意的是，回环帧与对应帧的基于图像算出的相对4自由度姿态的权重是5，为了平衡每帧图像与它前5帧的相对4自由度姿态。对应帧的前后对它的拉力要相同。

假设是第0帧和第m帧回环闭环了。  

在这里，究竟是使用还是，我认为是一样的。前者是把下一帧相机的原点转换到上一帧相机的yaw角的水平坐标系下，以此为测量值，预测值与测量值的残差，关于yaw角求导。后者是把把下一帧的相机的原点转换到上一帧的坐标系下，预测值与测量值的残差，关于yaw角求导。但这里的yaw角都是指在水平世界坐标系里面的yaw角。最终优化的结果应该是一样的。像这种，只是把残差从一个直角坐标系转换到另外一个直角坐标系，结果应该是一样的。但是，如果是另外一些转换，那结果就可能不一样了，比如非直角坐标系，各维度的有不同的缩放尺度，或者维度变换，那样子的话，优化结果就会不一样。

这里使用程序里面的表示，。程序里面没有求雅克比，而是用自动求导的方法。  

回环检测之后的处理，非常有创新点。一般的，回环检测之后，是要根据回环检测结果，把窗口关键帧的位姿都转换掉。但实际上，是可以不管窗口里面的关键帧的，可以认为窗口里面的关键帧的世界坐标系相对真实的世界坐标系发生了偏移和转换。只需要在输出位姿的时候，乘以这个转换就可以了，窗口里面的位姿还是原来的位姿。这样子，就可以避免对窗口的位姿进行大幅度调整，也不需要重新计算雅克比，协方差矩阵。输出的结果是应用所需要的，修正过的结果。

为了输出最新的位姿，需要把新缓存进来的IMU数据也都积分起来。更新实时的最新的位姿。然后，每新进来一个IMU数据，就在之前的基础上预测一下。因为IMU数据都是在载体坐标系的，所以，可以先在窗口的坐标系里积分，然后最后再转换到真实的世界坐标系，也可以直接把坐标转换到真实的世界坐标系，然后再积分。最后都是得到在真实的世界坐标系里面的位姿，然后每新进来一个IMU数据，就在之前的基础上预测一下。这两种方法是一样的。  

感谢：https://blog.csdn.net/wangshuailpp/article/details/78461171