前言

Vins-Mono预积分相关的公式和代码可以在各个博客中找到。本文将充当一个帮助大家快速理解预积分、中值积分相关概述的角色，带大家理解代码，应对面试。

下面会分成三步，来帮助大家充分理解这个过程。步骤是按照顺序来写的。

中值积分和预积分

imu每进来一帧都会进行一步中值积分和预积分。

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

每进来一帧imu都会push_back一下，push_back内部调用了积分函数propagate
积分函数propagate中调用了midPointIntegration中值积分函数

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

另外还有一个repropagate函数，作用：一个是初始化之后调用，重新积分。另一个是如果零偏更新幅度较大，则调用重新进行积分。这里不做过多讨论。

也就是传入了delta_t可以计算a、w的中值积分，将a、w的中值和之前的进行合并，就是预积分。

协方差和雅可比

在midPointIntegration函数中会对jacobian和convariance进行更新。

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

其中convariance的作用：

Jacobin是预积分后的pvq对bias噪声的Jacobin，意图是如果改变了bias，我们不能每一次又重新积分，所以我们求pvq对噪声的导数，如果更新了bias我们就可以直接通过这个jacobin更新pvq

IMU残差

公式和细节代码在其他博客中都有介绍，这里只简要说明。

IMU残差即为前后两个时刻的状态的差值和这两个时刻之间预积分相减。

前后时刻的bias会通过imu的噪声模型进行更新，而预积分过程中认为bias是不变的，且一直和前一个时刻的bias相等。

所以在优化过程中，如果对bias进行了改变，会通过上面的jacobin对残差进行更新。

vins-mono中使用的是ceres::SizedCostFunction这个功能（是用ceres求残差的其中一种方法，不了解的话可以搜一搜）：

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>

自己的IMUFactor类继承ceres::SizedCostFunction这个类并重写其中的函数。
其中一个很重要的函数：Evaluate要被重写。
Evaluate中调用了：

residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,Pj, Qj, Vj, Baj, Bgj);

对残差进行求解。

【面试克星】【公式少代码少话多】Vins-Mono预积分相关知识点总结与概述相关推荐

通达信指标公式颜色代码的四种写法(COLOR/RGB)
通达信指标公式颜色代码有四种写法,分别为COLOR+颜色的英文.COLOR+十六进制.RGBX+十六进制.RGB(R,G,B).标题有点尴尬,让我想到孔乙己"茴"字的四种写法,哈哈 ...
大学生计算机选购报告怎么写,一份能让你少花钱少走弯路的准大学生电脑选购指南...
距离绝大部分省份的高考放榜时间已经过去了经近一周的时间,许多家庭在大学志愿填报过后也终于能够迎来短暂的休息时间,接下来的时间除了等候录取结果之外,就是时候准备购置上大学的必需品了. 电脑,几乎是每一个 ...
RNN和LSTM的正向/前向传播-图示公式和代码
本文先讲的基础版本的RNN,包含内部结构示意图,公式以及每一步的python代码实现.然后,拓展到LSTM的前向传播网络.结合图片+公式+可运行的代码,清晰充分明白RNN的前向传播网络的具体过程.完整 ...
Interview：算法岗位面试—11.06早上上海某智能驾驶科技公司(创业)笔试+面试之手撕代码、项目考察、比赛考察、图像算法的考察等
Interview:算法岗位面试-11.06早上上海某智能驾驶科技公司(创业)笔试+面试之手撕代码.项目考察.比赛考察.图像算法的考察等导读:该公司是在同济某次大型招聘会上投的,当时和HR聊了半个多 ...
程序员的-多做多错，少做少错，不做不错
#欢迎光临我的随便说记得多年前公司里一个大项目出了重大问题,大领导震怒,于是我们一帮子leader被分配去各个组做check review. 重灾区是一个财务模块,据说这个模块是整个项目最难.最复杂 ...
Word 中设置图、表、公式、代码要与正文之间行间距
一.概述在撰写论文等文档时,常常对图.表.公式.代码要与正文之间行间距有要求.例如: (5)图.表.公式.代码要与正文之间有6磅的行间距. 二.设置方式选中图/表/公式/代码与图题/表头/- ...
Lasso线性回归学习笔记（公式与代码实现）
目录 Lasso线性回归学习笔记(公式与代码实现) 1 为什么要在线性回归中引入正则化项(简介) 2 常见正则化项 3 损失函数图像与正则化之后的图像 3.1损失函数图像 3.2 加了 L~1~ 正则 ...
少打针少吃药 100个宝宝祛病小偏方
少打针少吃药 100个宝宝祛病小偏方从容应对39种宝宝常见病,让宝宝少受罪,早康复. 查看原文下载上一篇:发型打理课程下一篇:养狗全训狗训犬视频教程
前端面试高频手写代码题
前端面试高频手写代码题一.实现一个解析URL参数的方法方法一:String和Array的相关API 方法二: Web API 提供的 URL 方法三:正则表达式+string.replace方法 ...

【面试克星】【公式少代码少话多】Vins-Mono预积分相关知识点总结与概述

前言

中值积分和预积分

协方差和雅可比

IMU残差

【面试克星】【公式少代码少话多】Vins-Mono预积分相关知识点总结与概述相关推荐

最新文章

热门文章