2024年C C++最新VINS技术路线与代码详解(1)，C C++框架体系架构的知识

本文链接：https://blog.csdn.net/2401_84975459/article/details/138781307

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新

具体代码实现：主要负责图像角点提取和光流跟踪，只有一个主线程。主要是三个源程序，分别是feature_tracker、feature_tracker_node以及parameters。feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

1. feature_tracker_node.cpp系统入口

（1） main()函数

步骤1：readParameters(n);读取参数，是config->euroc->euroc_config.yaml中的一些配置参数。

步骤2: trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);在这里NUM_OF_CAM设置成常量1，只有一个摄像头（单目），读取相机内参。

步骤3:判断是否加入鱼眼mask来去除边缘噪声

步骤4: ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100,img_callback); 订阅话题和发布话题，监听IMAGE_TOPIC（/cam0/image_raw），有图像发布到这个话题上的时候，执行回调函数，这里直接进入到img_callback函数中接收图像，前端视觉的算法基本在这个回调函数中。

1） img_callback(const sensor_msgs::ImageConstPtr &img_msg)接收图像

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

步骤2: 处理单目相机

步骤2.1: trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW *(i + 1)));读取到的图像数据存储到trackerData中，读取完之后如果图像太亮或太黑（EQUALIZE=1），使用createCLAHE对图像进行自适应直方图均衡化，如果图像正常，设置成当前图像。在读取图像的时候进行光流跟踪和特征点的提取。FeatureTracker类中处理的主要函数就是readImage()，这里涉及到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()函数. readImage()的处理流程为:

①先调用cv::CLAHE对图像做直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

PS：CLAHE是一种直方图均衡算法，能有效的增强或改善图像（局部）对比度，从而获取更多图像相关边缘信息有利于分割，比如在书架识别系统的书脊切割中，使用CLAHE可以比传统的直方图增强方法达到更好的增强书脊边界直线的效果，从而有利于后续的书脊边界直线的检测和提取。还能够有效改善AHE中放大噪声的问题，虽然在实际中应用不多，但是效果确实不错。

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

整体来说需要注意的是：光流跟踪在②中完成，角点提取在⑥中完成

步骤2.2:判断是否需要显示畸变。

步骤2.3:将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型的feature_points实例中；将图像封装到cv_bridge::CvImageConstPtr类型的ptr实例中

步骤3: 发布消息的数据

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

将处理完的图像信息用PointCloud实例feature_points和Image的实例ptr消息类型，发布到"feature"和"feature_img"的topic（此步骤在main函数中完成）

至此，已经将图像数据包装成特征点数据和图像数据发布出来了，下面就是在开一个线程，发布一个话题，接收这两种消息，也就是下面的vins_esitimate文件中做的事。

下面是具体的流程图：

二．Vins_estimate文件夹中

下面到第二部分了，也是整个框架的重点和难点，估计要花很长的时间学习和总计理论知识和代码逻辑，这一部分还是从论文的理论知识讲起，然后再附上代码消化理解。

论文内容：

1. 论文第IV点的B部分IMU预积分，

IMU预积分的作用是计算出IMU数据的观测值（就是IMU预积分值）以及残差的协方差矩阵和雅各比矩阵，那就要清楚的明白为什么要计算这三个量？计算出这三个量为什么就可以和视觉观测值进行耦合？如果你现在回答不出来，请好好想一想自己以前学到的知识，关于视觉的这三个量，视觉中观测值是用来计算残差的（也就是误差），残差的雅各比矩阵是优化中下降的方向（也就是梯度），很少提及的协方差矩阵（但很重要）其实是观测值对应的权值（因为有很多观测值），现在是不是很清楚明白了？具体使用来说，这三个量为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。原始陀螺仪和加速度计的观测值数据：

第一个式子等式左边是加速度测量值（你可以从加速度计中读到的值，带尖括号的是带误差的数据），等式右边是加速度真实值（其实就是准确的值，我们需要得到的是这个真实值）加上加速度计的偏置、重力加速度（注意是世界坐标系下）和加速度噪声项。第二个式子等式左边是陀螺仪测量值，等式右边是陀螺仪真实值加上陀螺仪偏置和陀螺仪噪声项。这里的值都是IMU（body）帧坐标系下的。这里假设噪声是服从高斯正态分布，而偏置服从随机游走模型。

有上面最原始的式子积分就可以计算出下一时刻的p、v和q:

状态量上标代表的是所处的坐标系，下标是具体哪一帧数据。这里等式左边的值都是世界坐标系下（W）bk+1帧的值。有必要解释下公式的推导，P和V的公式比较简单，q的公式需要解释下，一般情况下q_w_bk+1=q_w_bk*q_bk_bk+1，注意这里的乘法是四元数的乘法，和一般的旋转矩阵直接相乘方式不同，而q_bk_bk+1就是后面的积分公式，积分可以理解成q_bk_t*q_t_t1*q_t1_t2…，单独看q_bk_t*q_t_t1，将四元数的乘法转换成一般的乘法，论文这里使用四元数的右乘法则，将q_bk_t*q_t_t1转换成了1/2*Ω(w)*dt*q_bk_t。q推倒公式如下，公式中的()R是汉密尔顿右乘

从整个式子可以看出来这里的状态传播需要bk帧下的旋转，平移和速度，当这些开始的状态发生改变的时候，就需要重新传播IMU观测值，也就是说状态传播方程要重新计算和修改（而在slam的优化过程中，bk帧下的旋转，平移和速度是实时优化更新的状态量）。我们想要一次性就求出bk和bk+1之间的状态传播，因此选用预积分模型（最初提出预积分的外国大佬是将世界坐标系转换到求状态的变化量，其实两者的原理都一样，预积分求的值都是变化量），两边同时乘以世界到bk帧坐标系的转换，如下图所示，然后提出等式右边只与加速度和角速度有关的量进行积分，如公式6：

IMU预计分真实的意义在于其结果是一段短时间内（tk～tk+1）IMU真实旋转，位移和速度的变化量，而IMU短时间的变化量是非常准确的。到这里其实只要求出公式6中的积分值，真的预积分的值就得到了，这里bk是参考帧，从式6中可以看出，在bk到bk+1帧间，这里要求的三个预计分状态量只与IMU的偏置状态量有关系，而与其他状态无关，（状态量就是我们一直在优化的量，会一直变化），由于IMU的bias会一直修改，那么上面的预积分也会不断的重新积分（这很麻烦，也很耗时），所以我们希望bias变化的时候，我们可以很简单的就重新计算得到真实的预积分，而不需要再麻烦的重复积分计算预积分。我们的做法是，首先根据当前已知的bias通过公式（6）计算当前的预积分，当这里的偏置变化特别小的时候，我们可以使用一阶线性展开来调整预积分，这里提一下雅可比矩阵就是一阶偏导（我想大家应该都是知道的），如下公式12 所示：

所以要想求出这个临时的状态量，就必须求出等式右边的两部分值，第一个部分还是原来的积分形式（就像公式6那样），是预积分的主体，论文中使用的是最简单的欧拉积分法进行展开（取第i时刻值的斜率乘以时间差加上i时刻的初值，就得到i+1时刻的值），但是在代码中作者也提出了采用的是中值积分（顾名思义这里的斜率取得是i和i+1中点（2i+1）/2的时刻斜率）.公式7是采用欧拉积分的结果。这里前面有一定的说明，一开始abkbk，bbkbk等是零，旋转是单位旋转，注意整个过程把噪声设为0。

第二部分其实就是对应的一阶偏导（对加速度计偏置和陀螺仪偏置的），一阶偏导的求法在下面进行介绍，到这里我们已经求出了临时状态量的测量值，也就可以求出状态量测量值。

论文到这一步预积分其实已经做了一半了，也就是完成了测量值的求解，还差什么呢？当然是协方差矩阵了，下面重点求解协方差矩阵，顺便把上面没有求出的陀螺仪和加速度计偏置的雅各比矩阵求出来。

如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到SLAM中的神作state estimation for robotic，建立一个**线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。**也就是说还是需要前面求解状态测量值的公式6。注意代码中真正求解公式6使用的是中值法，所以为了和代码中相一致，下面的求解过程我也才用中值法的方式，为求解需求我们先补充点干货：

首先需要将上面的四自由度的旋转转换成三个维度的状态量，这是由于四自由度的旋转存在过参数数化的情况，过参数化求解会需要引入额外的约束（单位四元数性质），因此将误差看成是一个扰动定义式8：

然后有下面的两张图定义出来离散状态下的预积分过程：

最后得到图中的线性误差状态传播模型，由此得到IMU预积分测量值的协方差矩阵和雅各比矩阵，预积分的雅各比矩阵直接代入到公式12中计算出更加精确的传播状态值，而IMU误差运动方程的协方差矩阵自然是在后面优化中使用，后面优化使用的有IMU误差的运动方程，即 $\begin{aligned}Err_{imu}=\hat{Z}{b_k}_{b_{k+1}}-{Z}{b_k}_{b_{k+1}} \ \delta{Z^{b_k}_{b_{k+1}}} = \hat{Z}{b_k}_{b_{k+1}}-{Z}{b_k}_{b_{k+1}}\end{aligned}$ 。这里 $\hat{Z}^{b_k}_{b_{k+1}}$ 是预积分的值， ${Z}^{b_k}_{b_{k+1}}$ 是估计出来的积分值。上面求出的 $\delta{z_{k+1}}=F\delta{z_k}+VQ$ 可以表示成 $\delta{Z^{b_k}_{b_{k}+\delta t}}=F\delta{Z^{b_k}_{b_{k}}}+VQ_{b_k}$ ,我们希望一步步的迭代计算出来 $\delta{Z^{b_k}_{b_{k+1}}}$ 的协方差矩阵和jacobian矩阵，根据该公式预测k+1时刻的IMU误差方程的预积分和jacobian（这部分可以参考视觉slam十四讲的10.1.2的IMU运动方程的预测先验），如上面图片的公式。

需要格外注意上面求出的雅各比矩阵是预积分值的雅各比，我们还需要求一个IMU残差的雅各比矩阵，WHAT？还有两个雅各比矩阵，惊不惊喜意不意外？但情况确实如此，所以还不赶快从床上爬起来继续撸一把公式。上面求得的雅各比矩阵是用来计算预积分值时用到的，下面要求的IMU残差的雅各比矩阵是在紧耦合的时候做下降梯度，在最前面已经提到过。在求残差的雅各比之前，再提一下残差是如何计算的吧，预积分相当于测量值（就是真值，因为没有比这个更准确的值了，那当然就是真值了），要估计的状态就是估计值，所以预积分测量值减去状态估计值就是残差，在后面会提到需要估计的IMU估计值有p,v,q,ba,bg。P和q的估计值初始值比较好得到（和视觉相关，可以直接用视觉的初值），而v,ba,bg这三个量的估计值初始值就比较难得到了，因为视觉没有这三个初始量，就会用到下面的联合初始化得到初始的这三个量。下面直接上残差公式和要优化的状态量：

需要求解的残差雅各比矩阵是残差对估计状态量的一阶偏导，残差向量有三个，状态向量有2*5=10个。所以需要计算残差向量对状态向量的一阶偏导。首先需要提出的是对于偏置求偏导是比较复杂的，所以对于预积分的计算采取的是一阶泰勒展开，这样就相对简单了。也就是论文中的公式（12）。

需要解释下为什么旋转向量的误差前面会有一个系数2，这和四元数转换成旋转向量之间存在一个2倍的关系有关

（1）旋转四元数残差的雅各比矩阵

可以看出旋转四元数残差包含的状态量只有qi,qj,big这三个变量，也就是一个三元函数求偏导过程。

（2）速度残差的雅各比矩阵

未完待续，其实可以自己推导试试。

（3）位移残差的雅各比矩阵

未完待续，其实可以自己推到试试。

到这里恭喜你已经完成了数据前端处理的所有步骤，下面直接进入初始化的过程吧！

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

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

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

3. 视觉惯性联合初始化

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

（1）陀螺仪偏置标定

旋转我们可以通过两种方式求得，一种是陀螺仪测量值，一种就是视觉观测值。按照正常的理解两者的大小一定是相等的（假设没有误差），但实际情况肯定有误差，我们就来看看各自的误差。陀螺仪的误差有两部分测量噪声和陀螺仪偏置，噪声暂时可以忽略（毕竟太小），而视觉的误差就只有观测噪声（也可以忽略不管），因此两者差值的绝对值就是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型：

公式15中第一个式子的第一项和第二项作四元数旋转的广义乘积就可以得到bk+1坐标系下相机从bk+1到bk下的相对旋转（相对bk+1），第三项是bk坐标系下陀螺仪从b到bk k+1下的相对旋转（相对bk），两者在做广义乘积，就是首先从bk到bk+1旋转，然后再从bk+1到bk旋转，相当于做差（OA+AO=0），第二个式子就是前面预积分提到的一阶线性近似。然后取最小二乘，当然也可以使用SVD分解等方法求解。注意在求得陀螺仪偏置之后要再次将陀螺仪偏置代入到预积分中再求一次预积分的值，会更加精确。

（2）速度、重力加速度和尺度标定

作者在这里将这三个状态量统一到一个状态向量中，如公式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），gc0是第一个相机坐标系下的重力加速度，剩下的就是解最小二乘问题了，论文中采用的是快速的Cholesky分解。对于最小二乘求解这类方程如果有不懂的可以翻看我的另一篇博客，希望可以帮到你：

视觉SLAM常见的QR分解SVD分解等矩阵分解方式求解满秩和亏秩最小二乘问题（最全的方法分析总结）https://blog.csdn.net/wangshuailpp/article/details/80209863

（3）重力优化

上面其实已经得到了重力加速度的大小和方向，这里为什么还需要对重力进行优化呢？理由很简单，这里计算的重力吸收了重力加速度计的偏置，虽然不需要计算重力加速度计的偏置，但重力还是需要优化的，说到优化重力加速度，肯定包含两个量，大小和方向，也就是三个维度，但是一般来说大小是确定已知的（这里设为9.8），因此其实我们要做的就是优化方向，是一个两维的向量，下图是优化重力的方法以及b1,b2单位向量的方向确定模型。正切平面优化适合于对重力加速度的微调，而且只是微调方向。

4. 基于滑动窗口的单目视觉紧耦合后端优化模型

终于讲到了真正的视觉惯性紧耦合系统了，（在这里插一句，算是写博客和学习一个SLAM系统的心得吧，一开始我们可能抱着一个必胜的心态打算征服所有的知识点和难点，但到了整个过程的中期，总会出现或多或少的问题，阻碍着我们前进的步伐，你可能想放松警惕，不想太深究具体的公式和内容，急功心切的想看到终点到底是什么，但是我要提醒大家，也包括自己，最美好的以及最有价值的永远是过程，是在过程中学到的可以为一生所用的经验，只是一味着冲到终点，而忘记了过程，你最终会发现得到的只有失望。如果你不想失望的话，那就再接再厉，给自己继续加油吧！）到这里才是整个系统的重点，前面所有提及的其实都是松耦合方式，目的是给整个系统提供优化初值和状态。视觉惯性紧耦合优化模型在第VI部分，我理解的紧耦合系统是将视觉和惯性的原始观测量进行有效的组合，也就是在数据处理前就进行数据融合，VINS中是将滑动窗口内的状态量整合到一个状态向量中，如公式21所示：

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

从公式中可以明显的看出来，BA优化模型被分成了三个部分，分别是Marginalization（边缘化）残差部分（从滑动窗口中去掉的位姿和特征点的约束），IMU残差部分（滑动窗口中相邻帧间的IMU产生）和视觉代价误差函数部分（滑动窗口中特征点在相机下的投影产生），其中鲁棒核函数针对代价函数设定（具体作用请参见视觉SLAM十四讲），鲁棒核函数的作用是去除outlier，视觉残差使用了鲁棒核函数，而imu残差没有使用，这是因为IMU数据不存在outlier。下面具体介绍着三个部分。

（1） IMU观测值残差

考虑到的是两个连续帧bk和bk+1之间的观测值，与前面的预积分模型相同，如公式24所示，还记得在IMU预积分的时候求得到协方差矩阵和观测值吗？那里求得的观测值就是测量值，协方差矩阵就是这里的协方差矩阵，而公式24前面的项就是预测的值，也称估计值，估计值和测量值之间的差值就是残差，其实这里的公式24是由公式5的右边左移得到，你发现了吗？

（2）视觉观测值残差

与传统的针孔相机模型定义的重投影误差不同，论文中使用的是单位半球体的相机观测残差，是一个宽视野鱼眼或者全方位相机。相机的残差模型如下公式25所示：

第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。这里有一个细节提一下，我觉得还蛮重要的，视觉残差的公式是归一化相机平面坐标下的误差（而不是直接使用像素误差，你发现了吗？），主要的原因还是精度问题，像素的刻度是一个像素（也就是说误差最小是1个像素变化），而归一化相机平面的刻度没有具体限制，精度更高。

（3） Marginalization

这一部分借鉴博客：http://blog.csdn.net/q597967420/article/details/76099443

sliding windowsbounding边界化了优化问题中pose的个数, 从而防止pose和特征的个数随时间不断增加, 使得优化问题始终在一个有限的复杂度内, 不会随时间不断增长。然而, 将pose移出windows时, 有些约束会被丢弃掉, 这样势必会导致求解的精度下降, 而且当MAV进行一些退化运动(如: 匀速运动)时, 没有历史信息做约束的话是无法求解的. 所以, 在移出位姿或特征的时候, 需要将相关联的约束转变成一个约束项作为prior放到优化问题中. 这就是marginalization要做的事情。

VINS-MONO中,为了处理一些悬停的case,引入了一个two-way marginalization, 简单来说就是:如果倒数第二帧是关键帧, 则将最旧的pose移出sliding window, 将最旧帧相关联的视觉和惯性数据边缘化掉，也就是MARGIN_OLD，作为一部分先验值,如果倒数第二帧不是关键帧, 则将倒数第二帧pose移出sliding window,将倒数第二帧的视觉观测值直接舍弃，保留相关联的IMU数据，也就是MARGIN_NEW。选取关键帧的策略是视差足够大，在悬停等运动较小的情况下, 会频繁的MARGIN_NEW, 这样也就保留了那些比较旧但是视差比较大的pose. 这种情况如果一直MARGIN_OLD的话, 视觉约束不够强, 状态估计会受IMU积分误差影响, 具有较大的累积误差。

边缘化部分可以理解成是系统的先验，也就是滑窗之前的约束，这部分的约束会越来越多，矩阵的维度和耗时会越来越大。

到这里整体VINS理论的框架基本上算是介绍完毕了，对于后面的重定位，全局位姿优化和回环检测等有时间再做下讨论，现在有没有一个比较清晰的思路？如果还比较困惑的话，那就赶紧进行下面的代码实战环节吧。下面就从代码角度来分析VINS的整个第二部分。

（4）单目视觉雅可比计算（参考贺大佬）

视觉SLAM十四讲中第162页有提到，将空间坐标系下的3D点投影到像素坐标系下做最小二乘，求出该最小二乘模型的雅各比矩阵既可以得到高斯牛顿或者LM方法的下降梯度，从而优化位姿和路标点，具体的内容请参照书本。需要注意的是VINS在计算视觉雅各比时与书上有三点不同：

①VINS中由feature_tracker传过来的像素点坐标吸收了内参K且做了归一化，因此是吸收内参的归一化图像坐标。

②VINS中使用Q旋转四元数来优化位姿，因此在计算位姿对应的雅各比矩阵时和书上的公式不完全相同。

③VINS是基于滑动窗口，优化的位姿包含滑动窗口中的11帧位姿，但同时是依靠特征点来确定所优化哪两帧的位姿，每次传入视觉残差模型是遍历特征点来确定的，设定i帧为最开始观测到次特征点的相机，则j（不等于i）就为其他共视关键帧。

4.1视觉j帧残差计算（其实也可以计算i帧残差，过程基本相似）

明白上面三个不同点就可以进行下面的公式推导了，首先下图是残差计算模型，VINS选取的是j帧图像的残差计算模型。