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

2024-03-16 10:30
文章标签 优化 解读 详细 理论 视觉 耦合 非线性 imu 残差 协方差 mono residual vins 舒尔补 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_Hdelta_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。

相关文章

Vue3 的 shallowRef 和 shallowReactive：优化性能

Vue3 的 shallowRef 和 shallowReactive：优化性能

大家对 Vue3 的 ref 和 reactive 都很熟悉，那么对 shallowRef 和 shallowReactive 是否了解呢？ 在编程和数据结构中，“shallow”（浅层）通常指对数据结构的最外层进行操作，而不递归地处理其内部或嵌套的数据。这种处理方式关注的是数据结构的第一层属性或元素，而忽略更深层次的嵌套内容。 1. 浅层与深层的对比 1.1 浅层（Shallow） 定义
阅读更多...
HDFS—存储优化（纠删码）

HDFS—存储优化（纠删码）

纠删码原理 HDFS 默认情况下，一个文件有3个副本，这样提高了数据的可靠性，但也带来了2倍的冗余开销。 Hadoop3.x 引入了纠删码，采用计算的方式，可以节省约50％左右的存储空间。 此种方式节约了空间，但是会增加 cpu 的计算。 纠删码策略是给具体一个路径设置。所有往此路径下存储的文件，都会执行此策略。 默认只开启对 RS-6-3-1024k
阅读更多...
使用opencv优化图片（画面变清晰）

使用opencv优化图片（画面变清晰）

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化，使其看起来更清晰，同时保持尺寸不变，通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多，主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器：相机传
阅读更多...
2024年流动式起重机司机证模拟考试题库及流动式起重机司机理论考试试题

2024年流动式起重机司机证模拟考试题库及流动式起重机司机理论考试试题

题库来源：安全生产模拟考试一点通公众号小程序 2024年流动式起重机司机证模拟考试题库及流动式起重机司机理论考试试题是由安全生产模拟考试一点通提供，流动式起重机司机证模拟考试题库是根据流动式起重机司机最新版教材，流动式起重机司机大纲整理而成（含2024年流动式起重机司机证模拟考试题库及流动式起重机司机理论考试试题参考答案和部分工种参考解析），掌握本资料和学校方法，考试容易。流动式起重机司机考试技
阅读更多...
MySQL高性能优化规范

MySQL高性能优化规范

前言：      笔者最近上班途中突然想丰富下自己的数据库优化技能。于是在查阅了多篇文章后，总结出了这篇！ 数据库命令规范 所有数据库对象名称必须使用小写字母并用下划线分割 所有数据库对象名称禁止使用mysql保留关键字（如果表名中包含关键字查询时，需要将其用单引号括起来） 数据库对象的命名要能做到见名识意，并且最后不要超过32个字符 临时库表必须以tmp_为前缀并以日期为后缀，备份
阅读更多...
MCU7.keil中build产生的hex文件解读

MCU7.keil中build产生的hex文件解读

1.hex文件大致解读 闲来无事,查看了MCU6.用keil新建项目的hex文件 用FlexHex打开 给我的第一印象是:经过软件的解释之后,发现这些数据排列地十分整齐 :02000F0080FE71:03000000020003F8:0C000300787FE4F6D8FD75810702000F3D:00000001FF 把解释后的数据当作十六进制来观察 1.每一行数据
阅读更多...
Java ArrayList扩容机制 （源码解读）

Java ArrayList扩容机制 （源码解读）

结论：初始长度为10，若所需长度小于1.5倍原长度，则按照1.5倍扩容。若不够用则按照所需长度扩容。 一. 明确类内部重要变量含义         1：数组默认长度         2:这是一个共享的空数组实例，用于明确创建长度为0时的ArrayList ，比如通过 new ArrayList<>(0)，ArrayList 内部的数组 elementData 会指向这个 EMPTY_EL
阅读更多...
系统架构师考试学习笔记第三篇——架构设计高级知识（20）通信系统架构设计理论与实践

系统架构师考试学习笔记第三篇——架构设计高级知识（20）通信系统架构设计理论与实践

本章知识考点：         第20课时主要学习通信系统架构设计的理论和工作中的实践。根据新版考试大纲,本课时知识点会涉及案例分析题(25分),而在历年考试中,案例题对该部分内容的考查并不多,虽在综合知识选择题目中经常考查,但分值也不高。本课时内容侧重于对知识点的记忆和理解,按照以往的出题规律,通信系统架构设计基础知识点多来源于教材内的基础网络设备、网络架构和教材外最新时事热点技术。本课时知识
阅读更多...
SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型，它综合考虑了土壤-水分-大气以及植被间的相互作用；是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程，使其能够精确的模拟土壤中水分的运动，而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者
阅读更多...
从状态管理到性能优化：全面解析 Android Compose

从状态管理到性能优化：全面解析 Android Compose

文章目录 引言一、Android Compose基本概念1.1 什么是Android Compose?1.2 Compose的优势1.3 如何在项目中使用Compose 二、Compose中的状态管理2.1 状态管理的重要性2.2 Compose中的状态和数据流2.3 使用State和MutableState处理状态2.4 通过ViewModel进行状态管理 三、Compose中的列表和滚动
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2