目录

1. 进imu数据的循环

处理imu数据（预积分）：estimator.processIMU 

预积分

IntegrationBase类

propagate函数

中点积分函数 midPointIntegration

中值积分过程：   这里是imu解算（不是预积分量）

imu预积分相关

后端优化线程：void System::ProcessBackEnd() 中for (auto &measurement : measurements)循环部分代码阅读

        for (auto &measurement : measurements){auto img_msg = measurement.second;double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;for (auto &imu_msg : measurement.first)//处理imu数据{...}// TicToc t_s;map>>> image;for (unsigned int i = 0; i < img_msg->points.size(); i++) {...}TicToc t_processImage;estimator.processImage(image, img_msg->header);if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR){...}}

参考：【C++基础编程】#029 for循环中带冒号（:）用法简介_杨小浩浩的博客-CSDN博客_c++ 冒号 循环

1. 进imu数据的循环

参考：VINS-Mono代码精简版代码详解-无ROS依赖(一)_jiweinanyi的博客-CSDN博客

for (auto &imu_msg : measurement.first){double t = imu_msg->header;double img_t = img_msg->header + estimator.td;if (t <= img_t){if (current_time < 0)current_time = t;double dt = t - current_time;assert(dt >= 0);current_time = t;dx = imu_msg->linear_acceleration.x();dy = imu_msg->linear_acceleration.y();dz = imu_msg->linear_acceleration.z();rx = imu_msg->angular_velocity.x();ry = imu_msg->angular_velocity.y();rz = imu_msg->angular_velocity.z();estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));// printf("1 BackEnd imu: dt:%f a: %f %f %f w: %f %f %fn",dt, dx, dy, dz, rx, ry, rz);}else{double dt_1 = img_t - current_time;double dt_2 = t - img_t;current_time = img_t;assert(dt_1 >= 0);assert(dt_2 >= 0);assert(dt_1 + dt_2 > 0);double w1 = dt_2 / (dt_1 + dt_2);double w2 = dt_1 / (dt_1 + dt_2);dx = w1 * dx + w2 * imu_msg->linear_acceleration.x();dy = w1 * dy + w2 * imu_msg->linear_acceleration.y();dz = w1 * dz + w2 * imu_msg->linear_acceleration.z();rx = w1 * rx + w2 * imu_msg->angular_velocity.x();ry = w1 * ry + w2 * imu_msg->angular_velocity.y();rz = w1 * rz + w2 * imu_msg->angular_velocity.z();estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//printf("dimu: dt:%f a: %f %f %f w: %f %f %fn",dt_1, dx, dy, dz, rx, ry, rz);}}

对于每个观测，如果IMU时间戳t小于图像时间戳，将当前时间置为t,直接读取角速度和线加速度，然后执行estimator.processIMU；

否则的话，如果t大于图像时间戳img_t，先前current_time到img_t以及img_t到t的时间差，将img_t置為当前时间，然后将加权后的IMU数据用于后面处理。注意到，img_t到t的时间差越小。

处理imu数据（预积分）：estimator.processIMU 

这里，acc_0和gyr_0是上一帧的传感器测量值，angular_velocity和linear_acceleration是
这一帧的传感器测量值。

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
 {1.判断是不是第一个imu消息，如果是第一个imu消息，则将当前传入的线加速度和角速度作为初始的加速度和角速度if (!first_imu){first_imu = true;acc_0 = linear_acceleration;gyr_0 = angular_velocity;}2.IMU 预积分类对象还没出现，创建一个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){   3.当滑动窗口内有图像帧数据时，进行预积分 frame_count表示滑动窗口中图像数据的个数每新到一个图像帧，就会创建一个IntegrationBase对象存入pre_integrations数组当中pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);4.dt、加速度、角速度加到buf中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; 5.采用的是中值积分的传播方式       Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g; 上一帧的imu加速度，此时Rs未更新Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;  这一帧的imu加速度Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;Vs[j] += dt * un_acc;更新Rs Ps Vs三个向量数组。Rs为旋转向量数组，Ps为位置向量数组，Vs为速度向量数组，数组的大小为WINDOW_SIZE + 1那么，这三个向量数组中每个元素都对应的是每一个window}6.把这一帧的传感器测量值 赋值给acc_0和gyr_0
 滑动窗口中还没有图像帧数据，不需要进行预积分，只进行线加速度和角速度初始值的更新acc_0 = linear_acceleration; gyr_0 = angular_velocity;
 }
 

预积分

预积分操作是在IntegrationBase类中定义的push_back函数进行的：

    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); 预积分}
 

IntegrationBase类

预积分类：加速度计、陀螺仪、线性加速度计ba、陀螺仪bg、雅克比矩阵初始化、协方差矩阵15*15、dt、PVQ
 IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg): acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},jacobian{Eigen::Matrix::Identity()}, covariance{Eigen::Matrix::Zero()},sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
 

propagate函数

积分计算两个关键帧之间IMU测量的变化量：

  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;Quaterniond result_delta_q;Vector3d result_delta_v;Vector3d result_linearized_ba;Vector3d result_linearized_bg;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);//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,//                    linearized_ba, linearized_bg);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

acc_0 上一时刻的传感器测量值（是时刻还是帧）？
 acc_1  当前时刻的传感器测量值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){
 1. 首先是得到状态量Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;result_delta_v = delta_v + un_acc * _dt;result_linearized_ba = linearized_ba;result_linearized_bg = linearized_bg;         2. 计算雅可比矩阵和对应的协方差矩阵if(update_jacobian){Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;Vector3d a_0_x = _acc_0 - linearized_ba;Vector3d a_1_x = _acc_1 - linearized_ba;Matrix3d R_w_x, R_a_0_x, R_a_1_x;//反对称矩阵R_w_x<<0, -w_x(2), w_x(1),w_x(2), 0, -w_x(0),-w_x(1), w_x(0), 0;R_a_0_x<<0, -a_0_x(2), a_0_x(1),a_0_x(2), 0, -a_0_x(0),-a_0_x(1), a_0_x(0), 0;R_a_1_x<<0, -a_1_x(2), a_1_x(1),a_1_x(2), 0, -a_1_x(0),-a_1_x(1), a_1_x(0), 0;// 对F赋值MatrixXd F = MatrixXd::Zero(15, 15);F.block<3, 3>(0, 0) = Matrix3d::Identity();
 f01:        F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;F.block<3, 3>(6, 6) = Matrix3d::Identity();F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;F.block<3, 3>(9, 9) = Matrix3d::Identity();F.block<3, 3>(12, 12) = Matrix3d::Identity();//cout<<"A"<(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;//step_jacobian = F;//step_V = V;// Jacobian和协方差矩阵jacobian = F * jacobian;covariance = F * covariance * F.transpose() + V * noise * V.transpose();}}
 

1. 计算得到状态量

        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;result_delta_v = delta_v + un_acc * _dt;result_linearized_ba = linearized_ba;result_linearized_bg = linearized_bg;解释：
 delta_q，delta_p，delta_v是上一帧的imu预积分结果。
 linearized_ba，linearized_bg是上一帧估计的传感器偏置，此处直接将其等于为此帧的传感器偏置。
 

2. 计算雅可比矩阵

imu预积分是有误差的。

block函数（提取矩阵块）

int main()
 {MatrixXf m(4,4);m<< 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;cout<(1,1)<

Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g; 上一帧的imu加速度，此时Rs未更新
 Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
 Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
 Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;  这一帧的imu加速度
 Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
 Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
 Vs[j] += dt * un_acc;

Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();