VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化

本文主要是介绍VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

预积分和后端优化IMU部分**
https://blog.csdn.net/weixin_44580210/article/details/93377806

本讲是VINS最核心部分了,前面经历了

1)视觉跟踪feature_tracker、IMU预积分integrationBase类;
2)初始化中SFM纯视觉估计滑动窗中所有帧的位姿和3D路标点深度、SFM与IMU预积分松耦合对齐求解初始化参数。

在完成前面的初始化环节后,本节将进行第3部分,基于滑动窗的紧耦合后端非线性优化:将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化,求解出滑动窗口中所有帧的PVQ、bias等。
在视觉约束和IMU约束中,基本思想是找到优化状态向量,然后通过视觉残差和IMU测量残差分别对状态向量求导,获得视觉和IMU预积分的Jacobian和协方差矩阵
阅读本文所需知识储备:
1、最小二乘、梯度下降法、牛顿与高斯-牛顿与LM关系总结
2、《视觉SLAM十四讲精品总结》4:非线性优化g2o(BA基础)
3、IMU预积分integrationBase类

目录

在这里插入图片描述
一、VIO残差函数的构建
1、需要优化的状态向量:
2、目标函数为:
二、视觉约束(细致)
1.视觉重投影误差residual(归一化平面)
2、优化变量
3、Jacobian
4、协方差
三、IMU约束(粗略,祥细见之前)
1、残差:
2、优化变量:
3、IMU测量残差公式推导
4、残差对状态量的Jacobian
5、残差对状态量的协方差
四、基于舒尔补的边缘化
1、论文部分
2、基本公式
3、舒尔补
4、marg后形成的先验
5 具体例子
5.1 原来的信息矩阵H的构成
5.2 舒尔补后形成新的信息矩阵new_H,并构造为先验
5.3 新测量信息和先验构成新的系统
————————————————

一、VIO残差函数的构建

在这里插入图片描述
1、需要优化的状态向量:
滑动窗口内IMU状态(PVQ、加速度bias、陀螺仪bias)、IMU到Camera的外参、m+1个3D路标点逆深度
在这里插入图片描述

第一个式子是滑动窗口内所有状态量,n是关键帧数量,m是滑动窗内所有观测到的路标点总数,维度是15*n+6+m。特征点逆深度为了满足高斯系统。 第二个式子xk是在第k帧图像捕获到的IMU状态,包括位置,速度,旋转(PVQ)和加速度偏置,陀螺仪偏置。 第三个式子是相机外参。
注意: xk只与IMU项和Marg有关;特征点深度也只与camera和Marg有关.
———————————————

2、目标函数为:
在这里插入图片描述
**视觉惯性BA:**这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差;
BA优化模型分为三部分:
1、Marg边缘化残差部分(滑动窗口中去掉位姿和特征点约束)代码中使用Google开源的Ceres solver解决。
2、IMU残差部分(滑动窗口中相邻帧间的IMU产生).
3、视觉误差函数部分(滑动窗口中特征点在相机下视觉重投影残差).

二、视觉约束(细致)

在这里插入图片描述
这部分要拟合的目标可以通过重投影误差约束,求解的是对同一个路标点的观测值和估计值之间的误差,注意是在归一化平面上表示。

1.视觉重投影误差residual(归一化平面)
在这里插入图片描述

相关自变量是:该路标点被两帧观察到的相机帧位姿、Cam和IMU之间相对位姿、路标点在第一帧的逆深度。

当某路标点在第i帧观测到并进行初始化操作得到路标点逆深度,当其在第j帧也被观测到时,估计其在第j帧中的坐标为:
在这里插入图片描述
此时的视觉残差为:(左侧为根据i帧反推估计的位置,右侧为观测值)
在这里插入图片描述
在这里插入图片描述
逆深度参数化
在这里插入图片描述
逆深度作为参数原因:
1)观测到的特征点深度可能非常大,难以进行优化;
2)可以减少实际优化的参数变量;
3)逆深度更加服从高斯分布。这里特征点的逆深度在第i帧初始化操作中得到。
论文中:
上面的公式是传统的针孔相机模型,这里用的是单位半球体的相机观测残差。是一个鱼眼相机。在相机的归一化平面上比较残差,**再将视觉残差投影到单位球面的正切平面上。**由于视觉残差的自由度是2,所以我们将残差向量投影到切平面上。
在这里插入图片描述
在第i帧第一次观测到第l个路标点,在第j帧中对该路标点进行观测的残差为:
在这里插入图片描述
第一个式子就是残差的表达式,第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据,b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。
2、优化变量
在这里插入图片描述
3、Jacobian
在这里插入图片描述
求解Jacobian就是用视觉残差对上述7个状态变量进行求导。
在这里插入图片描述
方法一:崔博直接给出答案
在这里插入图片描述
方法二:深蓝学院详细推导
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
4、协方差
在这里插入图片描述

void Estimator::setParameter()
{for (int i = 0; i < NUM_OF_CAM; i++){tic[i] = TIC[i];ric[i] = RIC[i];}f_manager.setRic(ric);ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();td = TD;
}

三、IMU约束(粗略,祥细见之前)

1、残差:
状态量传播预测与IMU预积分的残差。
2、优化变量:
IMU的第i、j时刻下的p位置,v速度,Q旋转(PVQ),两个偏置ba,bw.
在这里插入图片描述
之前对IMU预积分进行了详细讲解。参考:VINS-Mono 代码详细解读——IMU离散中值预积分
3、IMU测量残差公式推导
简单来说,如果每一帧IMU都要相对于第一帧求位姿,计算量很大;考虑将PVQ积分公式改为相对第i时刻的姿态。
在这里插入图片描述
原PVQ积分公式:
在这里插入图片描述
基于预积分的PVQ积分公式
在这里插入图片描述
预积分量为:
在这里插入图片描述
最新的PVQ积分公式变为:
在这里插入图片描述
IMU测量残差e为:
在这里插入图片描述
在这里插入图片描述
4、残差对状态量的Jacobian
这里直接给出结果,如果想看详细推导,参考:VINS-Mono 代码详细解读——IMU预积分的残差、Jacobian和协方差 processIMU()+intergrationBase类+imu_factor.h
对应位置补充上这个J的矩阵块就行了,其他位置还是0。一共分为四大模块:
在这里插入图片描述
整个残差对某个变量求导;
在这里插入图片描述
在这里插入图片描述
5、残差对状态量的协方差
在这里插入图片描述

四、基于舒尔补的边缘化

为什么进行边缘化操作?
如果仅仅从前后两帧图像计算相机位姿,速度快但是精度低;但是采用全局优化BA,连接所有图像帧,精度高但是复杂度高。
**采用滑动窗,固定数量的帧进行优化,这样能够保证精度和速度。**既然是滑动窗,在滑动的过程中会有新的图像进来,旧的图像离开,所谓边缘化就是为了删除图像,但是把图像信息约束保留下来。

1、论文部分
为了防止pose和特征的个数的复杂度随着时间不断增长,引入边缘。
化,在移除位姿时将关联的约束转化为先验放入优化问题中。
为了限制基于优化的VIO计算复杂度,引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征λ1边缘化,同时将对应于边缘状态的测量值转换为先验。
在这里插入图片描述

分为两种情况, 1、一种是倒数第二帧如果是关键帧的话,将最旧的pose移出Sliding Window,将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化;Margin_Old作为先验值。
2、如果倒数第二帧不是关键帧的话,那么就只剔除倒数第二帧的视觉观测,而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。

为了保持系统的稀疏性,我们不会边缘化非关键帧的所有测量值。
2、基本公式
我们根据运动模型和观测模型建立H矩阵(高斯牛顿法中的JJT)的过程其实就是根据概率图模型(多元高斯分布)建立各个节点变量间的信息矩阵(协方差矩阵的逆)的过程,而边缘化则是去掉概率图中的某一个节点后信息矩阵会发生怎样的变换的问题。
基于高斯牛顿的非线性优化理论可知,Hdelta_x=b可以写成:
在这里插入图片描述
其中,delta_xa和delta_xb分别是希望marg掉的部分和保留部分。
VINS中需要边缘化滑动窗口中的最老帧,目的是希望不再计算这一帧的位姿或者与其相关的路标点,但是希望保留该帧对窗口内其余帧的约束关系。我们基于与移除状态相关的所有边缘化测量值构造一个先验。新的先验项被添加到现有的先验项中。
3、舒尔补
在这里插入图片描述
4、marg后形成的先验
在这里插入图片描述
xa为需要marg的变量,假设为相机pose,**我们更关心如何求解希望保留的xb,**而不再求解xa(即marg的变量改为0,左乘时左上是0),这里是要变化为上三角。
在这里插入图片描述
在这里插入图片描述
**即:new_H
delta_xb=new_b;
形成新的信息矩阵new_H具体流程:**
在这里插入图片描述
注意:去掉了x1,但是之前和x1相连的所有量x2 x3 x4 x5 在marg掉x1后变得两两相连。

5 具体例子

在这里插入图片描述
在这里插入图片描述
5.1 原来的信息矩阵H的构成
上述最小二乘问题,对用的高斯牛顿求解为:
在这里插入图片描述
在这里插入图片描述
矩阵乘法公式写成连加:
在这里插入图片描述
**雅克比J和信息矩阵H的稀疏性:**由于每个残差只和某几个状态量有关,因此,雅克比矩阵求导时,无关项的雅克比为 0。
在这里插入图片描述
将五个残差的信息矩阵加起来,得到样例最终的信息矩阵 Λ, 可视化如下
在这里插入图片描述
5.2 舒尔补后形成新的信息矩阵new_H,并构造为先验
在这里插入图片描述
在这里插入图片描述
5.3 新测量信息和先验构成新的系统
在这里插入图片描述
在这里插入图片描述
参考文章:
VINS-Mono理论学习——后端非线性优化 by Manii
VINS-mono详细解读 by 极品巧克力
VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差,及其对状态量的雅克比矩阵、协方差递推方程的推导) by Hansry
深蓝学院手写VIO课程
https://blog.csdn.net/iwanderu/article/details/104729332

这篇关于VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/815178

相关文章

Oracle查询优化之高效实现仅查询前10条记录的方法与实践

《Oracle查询优化之高效实现仅查询前10条记录的方法与实践》:本文主要介绍Oracle查询优化之高效实现仅查询前10条记录的相关资料,包括使用ROWNUM、ROW_NUMBER()函数、FET... 目录1. 使用 ROWNUM 查询2. 使用 ROW_NUMBER() 函数3. 使用 FETCH FI

C#使用HttpClient进行Post请求出现超时问题的解决及优化

《C#使用HttpClient进行Post请求出现超时问题的解决及优化》最近我的控制台程序发现有时候总是出现请求超时等问题,通常好几分钟最多只有3-4个请求,在使用apipost发现并发10个5分钟也... 目录优化结论单例HttpClient连接池耗尽和并发并发异步最终优化后优化结论我直接上优化结论吧,

Java内存泄漏问题的排查、优化与最佳实践

《Java内存泄漏问题的排查、优化与最佳实践》在Java开发中,内存泄漏是一个常见且令人头疼的问题,内存泄漏指的是程序在运行过程中,已经不再使用的对象没有被及时释放,从而导致内存占用不断增加,最终... 目录引言1. 什么是内存泄漏?常见的内存泄漏情况2. 如何排查 Java 中的内存泄漏?2.1 使用 J

MySQL中时区参数time_zone解读

《MySQL中时区参数time_zone解读》MySQL时区参数time_zone用于控制系统函数和字段的DEFAULTCURRENT_TIMESTAMP属性,修改时区可能会影响timestamp类型... 目录前言1.时区参数影响2.如何设置3.字段类型选择总结前言mysql 时区参数 time_zon

Java操作PDF文件实现签订电子合同详细教程

《Java操作PDF文件实现签订电子合同详细教程》:本文主要介绍如何在PDF中加入电子签章与电子签名的过程,包括编写Word文件、生成PDF、为PDF格式做表单、为表单赋值、生成文档以及上传到OB... 目录前言:先看效果:1.编写word文件1.2然后生成PDF格式进行保存1.3我这里是将文件保存到本地后

windows系统下shutdown重启关机命令超详细教程

《windows系统下shutdown重启关机命令超详细教程》shutdown命令是一个强大的工具,允许你通过命令行快速完成关机、重启或注销操作,本文将为你详细解析shutdown命令的使用方法,并提... 目录一、shutdown 命令简介二、shutdown 命令的基本用法三、远程关机与重启四、实际应用

MySQL中的锁和MVCC机制解读

《MySQL中的锁和MVCC机制解读》MySQL事务、锁和MVCC机制是确保数据库操作原子性、一致性和隔离性的关键,事务必须遵循ACID原则,锁的类型包括表级锁、行级锁和意向锁,MVCC通过非锁定读和... 目录mysql的锁和MVCC机制事务的概念与ACID特性锁的类型及其工作机制锁的粒度与性能影响多版本

Redis过期键删除策略解读

《Redis过期键删除策略解读》Redis通过惰性删除策略和定期删除策略来管理过期键,惰性删除策略在键被访问时检查是否过期并删除,节省CPU开销但可能导致过期键滞留,定期删除策略定期扫描并删除过期键,... 目录1.Redis使用两种不同的策略来删除过期键,分别是惰性删除策略和定期删除策略1.1惰性删除策略

MySQL不使用子查询的原因及优化案例

《MySQL不使用子查询的原因及优化案例》对于mysql,不推荐使用子查询,效率太差,执行子查询时,MYSQL需要创建临时表,查询完毕后再删除这些临时表,所以,子查询的速度会受到一定的影响,本文给大家... 目录不推荐使用子查询和JOIN的原因解决方案优化案例案例1:查询所有有库存的商品信息案例2:使用EX

使用SpringBoot创建一个RESTful API的详细步骤

《使用SpringBoot创建一个RESTfulAPI的详细步骤》使用Java的SpringBoot创建RESTfulAPI可以满足多种开发场景,它提供了快速开发、易于配置、可扩展、可维护的优点,尤... 目录一、创建 Spring Boot 项目二、创建控制器类(Controller Class)三、运行