VINS代码阅读
非常全局的一个关于VINS-Mono的总结
IMU预积分部分
Estimator是一个类，processIMU是它的一个成员函数；
该函数的作用：使用中值法 求解 当前时刻的PVQ
传入参数：IMU传感器的输出——加速度、角速度；
传出参数：
同时，其中用到的一些数据成员（按使用顺序）列举如下：
bool first_imu; 起一个标志作用，初始值为false；
IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; 是一个数组，里面存放着(WINDOW_SIZE + 1)个指针，指针指向的类型是IntegrationBase；详见PS.1
int frame_count;个人理解为滑窗中图片的帧数；在Estimator::clearState()这个成员函数中被赋初值为0。感觉上限为WINDOW_SIZE。
IntegrationBase *tmp_pre_integration; 和pre_integrations中的一个元素类似；
vector 记录IMU数据之间的间隔时间？是Vector？什么含义？
linear_acceleration_buf和angular_velocity_buf类似；
Matrix3d Rs[(WINDOW_SIZE + 1)]; 一个数组
Bas，Bgs，
PS.
IntegrationBase是一个类型，定义在factor/integration_bash.h中，有待下一步分析；
pre_integrations[frame_count]->push_back是该类型中的一个成员函数，有待分析；
WINDOW_SIZE是在estimator/parameters.h中定义的一个参数，（个人暂时理解为滑窗的大小const int WINDOW_SIZE = 10;）
与使用中值法求解当前时刻PVQ的公式与代码的对应：
ω ^ ˉ i = 1 2 ( ω ^ i + ω ^ i + 1 ) − b ω i \bar{\hat{\omega}}_i=\frac{1}{2}({\hat{\omega}}_i+{\hat{\omega}}_{i+1})-b_{\omega_i} ω^ˉi=21(ω^i+ω^i+1)−bωi
(2) Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
在已知上一时刻姿态矩阵的情况下，使用陀螺仪的角增量（近似旋转矢量），可以求出当前时刻的姿态矩阵：
q b i + 1 w = q b i + 1 w ⊗ [ 1 1 2 ω ^ ˉ i δ t ] q^{w}_{b_{i+1}}=q^{w}_{b_{i+1}}\otimes \begin{bmatrix}1\\ \frac{1}{2}\bar{\hat{\omega}}_i \delta t \end{bmatrix} qbi+1w=qbi+1w⊗[121ω^ˉiδt]
(3) Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
（感觉j=frame_count不变，即在新的图像未到来之前，Rs[j]是在不断根据IMU的输出而刷新的）。
(1)(4)(5)对应的是：
a ^ ˉ i = 1 2 [ q i ( a ^ i − b a i ) − g w + q i + 1 ( a ^ i + 1 − b a i ) − g w ] \bar{\hat{a}}_i=\frac{1}{2}[q_i(\hat{a}_i-b_{a_i})-g^w+q_{i+1}(\hat{a}_{i+1}-b_{a_i})-g^w] a^ˉi=21[qi(a^i−bai)−gw+qi+1(a^i+1−bai)−gw]
(1) Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
(4) Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
最后，解析上公式（2）的前两行
p b i + 1 w = p b i w + v b i w δ t + 1 2 a ^ ˉ i δ t 2 p^w_{b_{i+1}}=p^w_{b_{i}}+v^w_{b_i}\delta t+\frac{1}{2}\bar{\hat{a}}_i \delta t^2 pbi+1w=pbiw+vbiwδt+21a^ˉiδt2
(6) Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
v b i + 1 w = v b i w + a ^ ˉ i δ t v^w_{b_{i+1}}=v^w_{b_i}+\bar{\hat{a}}_i\delta t vbi+1w=vbiw+a^ˉiδt
(7) Vs[j] += dt * un_acc;
（变量上 一巴 代表的是平均，程序里加了前缀un_；变量上 一尖 代表的是测量值；）
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu) // 第一组IMU数据的处理
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
if (!pre_integrations[frame_count]) // 如果为空
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]}; // 构造函数需要四个参数
}
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
(1) Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
(2) Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
(3) Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
(4) Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
(6) Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
(7) Vs[j] += dt * un_acc;
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
=============== 分割线 ===================
前面并不知道在pre_integrations[frame_count]->push_back中发生了什么，下面来分析：
class IntegrationBase{}是一个类型，push_back()是它的一个成员函数；
该类型的数据成员依照函数中出现的先后顺序，依次介绍：
std::vector 类似地，gyr_buf和acc_buf；
push_back()这个成员函数最后调用了另一个成员函数propagate()，且原封不动地把自己的三个输入参数都传递了过去。
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr); // 传播？
}
进入propagate()成员函数，其中数据成员有：
double dt;
Eigen::Vector3d acc_0, gyr_0; 保存上一时刻的值？
Eigen::Vector3d acc_1, gyr_1; 使用当前时刻传入的值赋值
const Eigen::Vector3d linearized_acc, linearized_gyr; const常量
剩余的数据成员，用来保存结果
double sum_dt;
Eigen::Vector3d delta_p;
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_v;
Eigen::Vector3d linearized_ba, linearized_bg;
本函数的核心在于midPointIntegration()，可以和解析里(8)式对应了？
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p; // 几个中间变量，用来保存midPointIntegration的结果
Quaterniond result_delta_q; // 四元数！
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 函数实参中，前面不带result_的几个为输入，后面几个带result_的为输出
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
midPointIntegration()函数
传入参数：
输出参数：
其中，(2)对应公式 ω ^ ˉ i = 1 2 ( ω ^ i + ω ^ i + 1 ) − b ω i \bar{\hat{\omega}}_i=\frac{1}{2}(\hat{\omega}_i+\hat{\omega}_{i+1})-b_{\omega_i} ω^ˉi=21(ω^i+ω^i+1)−bωi
公式符号与变量符号对应：
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
同时，这里使用Eigen::Quaterniond类型的delta_q（上一时刻的姿态四元数？），以及dt时间内陀螺的角增量输出（旋转矢量？）计算了result_delta_q（当前时刻的姿态四元数）。解析上公式（8）的第三个式子：
γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i i + 1 = γ ^ i b k ⊗ [ 1 1 2 ω ^ ˉ i δ t ] \hat{\gamma}^{b_k}_{i+1}=\hat{\gamma}^{b_k}_{i}\otimes \hat{\gamma}^{i+1}_{i}=\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1 \\ \frac{1}{2}\bar{\hat{\omega}}_i \delta t \end{bmatrix} γ^i+1bk=γ^ibk⊗γ^ii+1=γ^ibk⊗[121ω^ˉiδt]
公式和代码的对应：
(3) result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
【注意】：这里有一个奇怪的对应，代码中的同一个量，对应了公式中的两个不同符号？暂时还未搞明白。
(1)(4)(5)构成了公式 a ^ ˉ i = 1 2 [ q i ( a ^ i − b a i ) + q i + 1 ( a ^ i + 1 − b a i ) ] \bar{\hat{a}}_i=\frac{1}{2}[q_i(\hat{a}_i-b_{a_i})+q_{i+1}(\hat{a}_{i+1}-b_{a_i})] a^ˉi=21[qi(a^i−bai)+qi+1(a^i+1−bai)]
公式符号与变量符号对应：
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
(1) Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
(4) Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
【注意】：result_delta_q是Eigen::Quaterniond类型的四元数， ( 4 × 1 ) (4\times 1) (4×1)，与一个 ( 3 × 1 ) (3\times 1) (3×1)的向量相乘时，使用的是重载的乘法（十四讲）！即代码上的q*v表示数学上的 q v q − 1 qvq^{-1} qvq−1，其中为四元数乘， v v v需要先改写成乘第一个元素为0的四元数然后再参与运算。
最后，根据解析上（8）中的第二式和第一式：
β ^ i + 1 b k = β ^ i b k + a ^ ˉ i δ t \hat{\beta}^{b_k}_{i+1}=\hat{\beta}^{b_k}_{i}+\bar{\hat{a}}_i\delta t β^i+1bk=β^ibk+a^ˉiδt
与之对应的代码：
（7）result_delta_v = delta_v + un_acc * _dt;
α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 a ^ ˉ i δ t 2 \hat{\alpha}^{b_k}_{i+1}=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t+\frac{1}{2}\bar{\hat{a}}_i\delta t^2 α^i+1bk=α^ibk+β^ibkδt+21a^ˉiδt2
与之对应的代码：
(6）result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
这样就使用中值积分完成了两帧之间PVQ增量的计算。 下一步，还要具体看一下调用他们的函数。
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
（1）Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
（2）Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
（3）result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
（4）Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
（5）Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
（6）result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
（7）result_delta_v = delta_v + un_acc * _dt;
（8）result_linearized_ba = linearized_ba;
（9）result_linearized_bg = linearized_bg;
if(update_jacobian)
{ // bala 见最后，矩阵的块状赋值，很长但比较简单
}
}
bool update_jacobian是一个布尔型的变量，且一个传入的值。根据实参我们知道，给它的赋值为1,。
解析上（15）式，分别给出了F和V的结构，分别就其中一个小块解读代码：
f 03 = − 1 4 ( R k + R k + 1 ) δ t 2 f_{03}=-\frac{1}{4}(R_{k}+R_{k+1})\delta t^2 f03=−41(Rk+Rk+1)δt2
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
v 00 = − 1 4 R k δ t 2 v_{00}=-\frac{1}{4}R_k\delta t^2 v00=−41Rkδt2
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
解析上（16）式
J k + 1 15 × 15 = F J k {J_{k+1}}^{15\times 15}=FJ_{k} Jk+115×15=FJk
jacobian = F * jacobian;
解析上（17）式
P k + 1 15 × 15 = F P k F T + V Q V T {P_{k+1}}^{15\times 15}=FP_kF^T+VQV^T Pk+115×15=FPkFT+VQVT
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
其中，jacobian和covariance是IntegrationBase类型下的成员函数，在执行构造函数的时候（即实例化一个对象时），二者的初值分别为：
J b k = I J_{b_k}=I Jbk=I和 P b k b k = 0 P^{b_k}_{b_k}=0 Pbkbk=0
对应构造函数中的代码：
jacobian{Eigen::Matrix