VINS理论与代码详解3——IMU预积分

VINS理论与代码详解3——IMU预积分

二．Vins_estimate文件夹中

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

         论文内容：

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。

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

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

         如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到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）。

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

可以看出旋转四元数残差包含的状态量只有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计算的当前旋转，位置，速度，作为优化的初值

注：下接视觉惯性联合初始化代码