目录
处理imu数据（预积分）：estimator.processIMU
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<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> 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 %f\n",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 %f\n",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<double, 15, 15>::Identity()},
covariance{Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()},
delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
propagate函数
积分计算两个关键帧之间IMU测量的变化量：
- 旋转delta_q
- 速度delta_v
- 位移delta_p
- 加速度的bias linearized_ba
- 陀螺仪的Bias linearized_bg
- 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
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"<<endl<<A<<endl;
// 对V赋值
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(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<<m.block<2,2>(1,1)<<endl<<endl; 提取块大小为（2，2），从元素（1，1）开始提取，需要注意的是，元素是从0开始记得。
return 0;
}
结果为：
6 7
10 11迭代时的noise是什么时候赋值的呢？
中值积分过程： 这里是imu解算（不是预积分量）
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（旋转矩阵）更新：
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();Utility::deltaQ：将角度转换为dq
参考：VINS-MONO代码解读---processIMU()+intergrationBase类+imu_factor.h_xiaojinger_123的博客-CSDN博客
206
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
