VIO Estimator::processIMU 函数

如果获取了一帧IMU数据，就会通过这一帧IMU数据去对位置、速度、加速度等完成积分，并且计算方差等的传递，这些功能就在processIMU函数中完成，本章对该函数进行讲解。

    if (!first_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]};/*cout<<"************************"<<endl;cout<<"pre integrations: "<<frame_count<<endl<<"acc_0: "<<endl<<acc_0<<endl<<"gyr_0: "<<endl<<gyr_0<<endl<<"Bas: "<<Bas[frame_count]<<endl<<"Bgs: "<<Bgs[frame_count]<<endl;cout<<"************************"<<endl;*/}

首先是判断是否为第一帧IMU, 是指整个过程第一帧IMU，而不是一次预积分的第一帧，利用IMU的数据对acc_0和gyr_0进行赋值，然后构建一个预积分的类IntegrationBase类，Bas是加速度的bias，Bgs是角速度的bias，其构建函数如下，构建了以下几个数据：

acc_0:加速度初始值

gyr_0:角速度初始值

linearized_acc:？

linearized_gyr:？

linearized_ba:？

linearized_bg:？

jacobian:雅可比？

covariance:方差

sun_t:总时间

delta_p:增加的位置

delta_q:增加的旋转角度

delta_v:增加的速度

    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()}{noise = Eigen::Matrix<double, 18, 18>::Zero();noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();}

接下来是对传入的加速度、角速度的具体处理，pre_integrations会对时间、加速度、角速度进行具体的处理，见附录一，处理结束后，pre_integrations会获得一个更新后的位置、速度、加速度值，以及雅可比和方法，estimator也会存储他的dt, 加速度和角速度，同时同时用Rs来记录选装，Ps来记录位置，Vs来记录速度，完成了一帧的IMU数据处理。

   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;Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;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;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;}

附录一：pre_integrations的push_back函数如下，dt_buf、acc_buf、gyr_buff几个buff内存储了输入的时间、加速度、角速度值，并且利用propagate函数进行运动积分，主要功能是在midPointInteration函数中，采用的是中值积分

    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);}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;  }

代码如下，计算得到当前时刻的状态量，误差雅可比，以及方差，delta_p, delta_q, delta_v分别是相对于预先积分初始帧的位置、角度、速度的增加量，t时刻值的基础上，利用传入的数据进行中值积分，就获得t+1时刻的值，另外还有两个偏差ba和bg，对于delta_q由于其是单位向量，因此需要normalizeaiton(),对acc_0和gyr_0也分别进行更新。

    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");Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); //去bias，添加旋转获得accVector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; //去bias，获得加速度result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); //获得新的delta_qVector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba); //计算acc_1的去biasVector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); //中值加速度result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt; //累加得到delta_presult_delta_v = delta_v + un_acc * _dt; //累加得到delta_vresult_linearized_ba = linearized_ba; //获得a的biasresult_linearized_bg = linearized_bg; //获得角速度的bias       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;MatrixXd F = MatrixXd::Zero(15, 15);F.block<3, 3>(0, 0) = Matrix3d::Identity();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;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 = F * jacobian; //误差的传递covariance = F * covariance * F.transpose() + V * noise * V.transpose(); //方差的传递}}

VIO Estimator::processIMU 函数相关推荐

VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差）
引言 VINS论文的IV-B. IMU Pre-integration介绍了IMU预积分模型,Foster的两篇论文对IMU预积分理论进行详细分析. 传统传统捷联惯性导航的递推算法,是在已知上一时刻的 ...
VINS-Mono之IMU预积分，预积分误差、协方差及误差对状态量雅克比矩阵的递推方程的推导
文章目录 1. 前言 2. IMU模型 3. 基于世界坐标系下的IMU运动模型 3.1 连续形式下的IMU运动模型 3.2 离散形式下的IMU运动模型 3.2.1 欧拉法离散形式 3.2.2 中值法离 ...
视觉惯性里程计 VIO
视觉惯性里程计 VIO - Visual Inertial Odometry 视觉−惯性导航融合SLAM方案视觉惯性SLAM专栏 VINS技术路线与代码详解 VINS理论与代码详解0--理论基础白话 ...
从零学习VINS-Mono/Fusion源代码（五）：VIO初始化
本节分析VIO初始化部分 VINS-Mono/Fusion代码学习系列: 从零学习VINS-Mono/Fusion源代码(一):主函数从零学习VINS-Mono/Fusion源代码(二):前端图像跟 ...
视觉惯性里程计综述 VIO Visual Inertial Odometry msckf ROVIO ssf msf okvis ORB-VINS VINS-Mono gtsam
视觉惯性里程计 VIO - Visual Inertial Odometry 视觉−惯性导航融合SLAM方案博文末尾支持二维码赞赏哦 _ 视觉惯性SLAM专栏 VINS技术路线与代码详解 VINS理 ...
VINS-Mono代码解读——状态估计器流程 estimator 写在初始化和非线性优化前
前言本文主要介绍VINS的状态估计器模块(estimator),主要在代码中/vins_estimator节点的相关部分实现. 这个模块可以说是VINS的最核心模块,从论文的内容上来说,里面的内容包 ...
VINS fusion软件架构分析（4）---后端处理processMeasurements()功能函数概览
文章目录 1. 本章概述 2. processMeasurements在哪里调用的 3. 信号数据结构 3.1 IMU数据结构 3.2 图像数据结构 3.3 组合图像+IMU信息的数据结构 3.4 特 ...
海思3519 VIO Sample例程讲解
海思VIO Sample例程讲解海思SDK解压出来后,Sample包含各个功能模块的历程,本篇讲解VIO Sample历程. 进入VIO模块可以看到,VIO的main函数文件,先从main函数执行程 ...
VINS-Mono视觉SLAM总体设计框架解读
文章目录写在前面论文解读 vins-mono 概述 1. Measurement Preprocessing 过程 2. Estimator Initialization 过程 3. Tightl ...

VIO Estimator::processIMU 函数

VIO Estimator::processIMU 函数相关推荐

最新文章

热门文章