【深蓝学院】手写VIO第3章--基于优化的 IMU 与视觉信息融合--笔记

news2025/2/23 18:25:25

0. 内容

在这里插入图片描述

1. 基于BA的VIO融合

优化的方法学会之后,滤波的方法也就会了。
在这里插入图片描述

具体的求解BA问题参考的是SBA的论文,使用的是LM算法(里面有个关于权重μ的计算方法,不同人的实现可能不一样,这些都是实现细节)
在这里插入图片描述
camera两个观测之间的相对位置(pq的变化)可以通过两次观测知道,但是IMU在两个时刻的观测量需要进行积分才能知道,而且IMU数据频率一般比Camera频率高,所以需要逐步地积分才能得到运动情况。

在这里插入图片描述

2. 最小二乘问题的求解

牛顿法,将loss function直接Taylor二阶展开,高斯牛顿法是将原函数f(x)一阶展开,再进行求解。
在这里插入图片描述

解法分为直接解和迭代解:
在这里插入图片描述
马鞍面复习:
在这里插入图片描述

2.1 一阶法

下降法(如果方向选择为梯度反方向则就为DL中的梯度下降法,此处叫最速下降法):
在这里插入图片描述
在这里插入图片描述
最速下降法:不易收敛;牛顿法:H计算量大。

阻尼法,在牛顿法基础上增加一个 Δ x \Delta x Δx的正则项,使其不会太大(此处仍是一阶法,不是LM法):
在这里插入图片描述
阻尼法的阻尼是加在H上的 μ \mu μ

为方便起见,可将残差stack起来成向量(也可以携程sum of的形式,但向量可供讨论具体小f的一些性质,相应地,雅克比J和海塞H也要相应地写为向量形式)
在这里插入图片描述
F(x)是cost function,最简单的例子: m i n F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 minF(x)=\frac{1}{2}||f(x)||_2^2 minF(x)=21∣∣f(x)22

2.2 二阶法

2.2.1 Gauss-Newton

在这里插入图片描述
损失函数F(x)对 Δ x \Delta x Δx的一阶导数可记作b,也可直接写F(x) ′ ^\prime
在这里插入图片描述

2.2.2 LM

2.2.2.1 LM阻尼因子 μ \mu μ初值的选取方法

在这里插入图片描述
τ \tau τ的设定是根据经验值而来,当前x距离 x ∗ x^* x较近时,取二阶近似,初值 μ 0 \mu_0 μ0就设的小一些,乘较小的 τ \tau τ,距离 x ∗ x^* x较远时,初值就设的应该较大,保证下降的较快,取一阶近似,乘接近于1的 τ \tau τ

这里高翔补充了一点,:广义的韦达定理保证所有的特征值 λ i \lambda_i λi之和跟矩阵的trace是一样的。

所以这里取 m a x J T J i i max{J^TJ_{ii}} maxJTJii矩阵的对角线的最大元素是有道理的,即可以有这样的先验: J T J i i J^TJ_{ii} JTJii的对角线最大元素跟 J T J i i J^TJ_{ii} JTJii特征值的最大值是在同一个数量级,所以可取对角线最大元素来近似最大特征值。

2.2.2.2 LM阻尼因子 μ \mu μ的更新策略

定性分析,看上面的 Δ x \Delta x Δx的解析形式,
在这里插入图片描述

μ \mu μ处于分母上,增大阻尼 μ \mu μ会减小步长,也符合“阻尼”的直观感受:当某次 Δ x \Delta x Δx使得cost function增大,则应该增大阻尼使 Δ x \Delta x Δx变小,减小步长;反之,如果cost function在下降则应该减小阻尼 μ \mu μ,增大步长,加快收敛速度。
在这里插入图片描述

上述推导说明分母一定是>0的,因为寻找的 Δ x \Delta x Δx会使L每次都使得L下降,但是cost function F不一定会下降(因为L是对F的二次近似,理论保证是L每次都会下降,但是实际F不一定下降),所以当 ρ < 0 \rho<0 ρ<0时表示F上升了。
ρ \rho ρ>1时代表此时方向是正确的,则应减小 μ \mu μ加快下降速度;如果0< ρ \rho ρ<1则代表则代表实际下降比较小,可能已经到最优值附近,应增大 μ \mu μ减慢下降速度。

Δ x \Delta x Δx越小说明近似的越好,越接近最优值。

在这里插入图片描述
发现了14讲关于这个 μ \mu μ更新策略的一个错误, ρ > 3 4 \rho>\frac{3}{4} ρ>43时应该减小阻尼 μ \mu μ ρ < 1 4 \rho<\frac{1}{4} ρ<41时应该增大阻尼 μ \mu μ

在这里插入图片描述

Nielsen采用了新的 ρ \rho ρ的计算策略,分母直接为L,判断 ρ \rho ρ的符号,当F下降时逐渐减小 ρ \rho ρ,当增大时, ρ \rho ρ下降的也很快。
在这里插入图片描述

2.3 鲁棒核函数(或叫M-Estimation)

outlier会使得F变得很大,为了处理outlier,在 f 2 f^2 f2外面增加一层 ρ \rho ρ对f进行处理 ρ ( f 2 ) \rho(f^2) ρ(f2),也有 ρ ( f ) 2 \rho(f)^2 ρ(f)2
在这里插入图片描述
给x微小扰动 Δ x \Delta x Δx计算 Δ s \Delta s Δs,带入(14)可得加上鲁棒核函数之后的展开式
在这里插入图片描述
在这里插入图片描述
鲁棒核函数本质上是对每项误差的重加权(Reweighted),State Estimation中有对Iterated Reweighted Least Square(IRLS)和鲁棒核函数等价的证明。

在这里插入图片描述
95%有效原则,带上outlier和不带时的协方差之比接近95%, 原文较接近数理统计方面的东西。

  • 对正态分布,若鲁棒核是Huber,则控制参数设为1.345可满足95%原则,Cauchy应为2.3849
  • 若非正态分布,则需统计残差协方差,在进行归一化处理(SVO,DVO,PTAM都是如此)变成正态分布,然后在使用正态分布的参数处理方法。
    (但是在实际可能这个参数不会影响很大,可能直接设置为1,大多数情况Cauchy更好,可以证明Cauchy核函数等价于IRLS,但是Huber是线性的,更简单,Cauchy对每个残差要多算个log)

在这里插入图片描述

g2o里面残差的一二阶导数应该可以和前面的对应起来了
在这里插入图片描述

2.4 小结:

在这里插入图片描述

3. VIO残差函数的构建

3.1 IMU预积分

  1. 从归一化残差的角度来考虑信息矩阵 Σ − 1 \Sigma^{-1} Σ1,为了把不通传感器的误差进行归一化,然后叠加使用,中间用信息矩阵进行了重加权。
  2. 从优化的角度来理解信息矩阵,残差f(x)小,协方差小,逆就大,所以应该占的权重更大,更关注这些准确的点。

基于滑动窗口的VIO残差的构建。
在这里插入图片描述
上述
第一项prior代表之前时刻的先验,
第二项代表当前窗口内数据的残差,因为之前之前窗口外时刻的观测数据可能还有用,所以当做先验加入优化项中进行优化,
第三项是我们熟悉的相机的误差(如重投影误差)

关于IMU需要优化的状态量:
在这里插入图片描述
其中 λ \lambda λ是逆深度(不知道为什么要在这里加入到残差中进行优化)

在这里插入图片描述

逆深度实际上是另一种参数化方式,类似于(x,y,z),xyz具有相关性,体现在协方差矩阵上,非对角线元素不为0,而u,v,d,可以近似独立,所以协方差矩阵可为对角阵。实际上d不为正态分布,但是1/d可近似为正态分布。(具体描述见SLAM14将建图的P326)

误差的构建:
基于逆深度的rpj误差
在这里插入图片描述

在这里插入图片描述
加速度积分,速度积分,旋转四元数积分:
在这里插入图片描述
每次前面的数据如果变化的话,后面的所有的值都要跟着积分变化,计算量很大。
把100个积分变成一个就好了,于是出现了IMU预积分:
预积分思想是:把相对于世界坐标系的单领出来,IMU各个时刻之间的相对位置不变(这部分就叫做IMU预积分)
在这里插入图片描述
在这里插入图片描述
将(32)式中的右边移项得到 α ( 位移 ) , β ∗ ( 速度 ) \alpha(位移),\beta*(速度) α(位移)β(速度)的预测值,和观测值作差即得IMU预积分的误差,bias直接两时刻的测量相减即可,四元数旋转需要按照下面的推导来计算。
在这里插入图片描述
回顾第一讲中关于四元数的求导
在这里插入图片描述
虚部取出来 θ 2 \frac{\theta}{2} 2θ ,乘以2就得到了旋转的角度 θ \theta θ

3.2 预积分的离散形式

在这里插入图片描述

3.3 预积分的方差传递

一个IMU数据的协方差和不确定度可以标定出来,但是如何传递呢?
在这里插入图片描述

在这里插入图片描述

3.4 状态误差线性化

两种方法:

  1. 将误差一阶泰勒展开,
  2. 求出误差的一阶导数,进行传递
    在这里插入图片描述
    KF假设马尔科夫性,k时刻的数据只和k-1有关,KF只能用于线性系统,若想拓展到非线性系统,则可以将运动和观测方程在某点一阶泰勒展开,只考虑线性部分,就得到了EKF(见SLAM14讲后端1部分)。下面真值等于后验+误差? 一阶泰勒展开,两边消去真值就得到了状态误差的传递
    在这里插入图片描述
    在这里插入图片描述
    因为已知速度和状态量之间的关系,所以求其误差之间的关系也是可以的,所以会有第二种基于误差的一阶导数的方法,常见于MSCKF这类优化的论文中。
    在这里插入图片描述

基于优化的方法(VISN-mono,预积分等)中大多数使用的是第一种基于一阶泰勒展开的方法:

3.5 预积分误差的雅克比推导

总体思想是:位置,速度,角度,加速度,角速度都会受到各种测量噪声的影响,比如角度测量可能会受到一个旋转的影响,所以加上一个小的旋转的扰动就能求角度的雅克比。

分别是角速度,旋转四元数,加速度,预计分量: α , β \alpha,\beta α,β,以及加计和陀螺的bias
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述
在这里插入图片描述

对各状态量求导:

3.5.1 对速度预计分量的雅克比

在这里插入图片描述
上面的(49)是(48)两边 β \beta β一阶泰勒展开,然后右边对 β b k \beta_{b_k} βbk求偏导,即得对上一时刻速度预计分量( β b k \beta_{b_k} βbk)的雅克比。

3.5.2 对角度预计分量的雅克比

在这里插入图片描述
k时刻旋转会受到一个微小的旋转扰动(可写成se(3)形式),
在这里插入图片描述
对exp进行一阶泰勒展开,再利用反对称矩阵的性质: a ∧ b = − b ∧ a a^{\wedge}b=-b^{\wedge}a ab=ba即可约掉分母,得part1的雅克比。

在这里插入图片描述
所以可得对角度预积分量的雅克比 f 32 f_{32} f32

part2这部分我看不太懂(可能是有公式)
在这里插入图片描述

3.5.3 速度预计分量对角速度bias的雅克比

在这里插入图片描述

在这里插入图片描述
为什么前面的一项跟分母没关系,直接扔掉了?

3.5.4 旋转预积分的递推公式

在这里插入图片描述
只考虑旋转误差,不考虑角速度测量的bias等误差,而且四元数有如下性质:
对一个向量进行旋转R相当于对应的四元数左右四元数相乘

在这里插入图片描述

在这里插入图片描述

这里把 q b i b k + 1 q_{b_ib_{k+1}} qbibk+1左乘个逆移到右边,计算旋转的测量值的误差传递,倒数第二行就是运用上面的四元数的公式,R就是 [ 1 − 1 2 ω δ t ] \begin{bmatrix} 1 \\ -\frac{1}{2} \omega\delta t \\ \end{bmatrix} [121ωδt]对应的旋转矩阵,于是就可以看出,从k到k+1时刻的姿态角的不确定度是通过R矩阵传递的。(还是有些不太明白)

在这里插入图片描述

4. 系统Jacobian的推导(自己推一遍,有R的就右乘一个exp然后再换出来,剩下的逆深度等就是链式求导法则)

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

预计分量对bias求导较为麻烦,预积分假设bias不动,但是实际上bias是动的,所以求他的bias就是比较麻烦的(我理解就是因为预积分有bias不动的先验,然后再去推对bias的导数就有些冲突),所以这里就使用了一阶泰勒展开来近似,丢掉高阶项,

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

4.1 举个例子

姿态对角度求导,IMU预积分得一个 q j i q_{ji} qji,world系下得两个时刻的q,之间可以求出一个 q j i q_{ji} qji,求这两个 q j i q_{ji} qji四元数之间的四元数乘法可得相对位姿,取虚部对角度求导即得Jacobian。方法是对 q w b i q_wb_i qwbi取一个小扰动,使用公式变换到右边去,跟分母消掉(下面*都代表取逆),下面还有关于四元数左乘和右乘的算子,看Joan sola的四元数手册。
(高翔说还能用李代数?四元数连乘会麻烦一些,用李代数会简单点?)

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述
这个下面需要自己推导。

TODO:
预积分误差传递的推导,
整体系统的Jocabian的推导。

5. 作业

详见:我的作业博客。

在这里插入图片描述

5. 待读文献

5.1 SBA论文

5.2 LM改进阻尼因子 ρ \rho ρ论文

μ \mu μ反复震荡(由于F反复增大和减小,导致要不断调整 μ \mu μ)导致浪费了很多计算次数。
最后Nielsen在此文中改进了 ρ \rho ρ的更新策略。
在这里插入图片描述

5.3 关于鲁邦核函数的论文

3是综述,对4种方法进行了讨论
在这里插入图片描述

5.4 鲁棒核函数

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/625360.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

全新 FinClip Studio 现已上线

FinClip IDE &#xff08;FinClip Integrated Development Environment&#xff0c;简称为 FIDE&#xff09;是面向开发者推出的「小程序桌面端集成开发环境」。自 2021 年起就一直陪伴来自不同平台的小程序开发者完成小程序开发、调试、预览、上传等各类功能。 随着 FinClip …

【MySQL】初识数据库

数据库基础知识 一、什么是数据库二、mysql与mysqld三、服务器、数据库、表关系四、数据库的连接、创建与使用4.1 安装地址4.2 连接服务器4.3 数据库的基本使用4.4 数据逻辑存储 五、MySQL架构六、SQL分类七、存储引擎 一、什么是数据库 存储数据用文件就可以了&#xff0c;为…

一文即可了解!Web测试(性能测试 / 界面测试 / 兼容性测试 / 安全性测试)

目录 前言&#xff1a; 一、Web性能测试&#xff1a;&#xff08;压力测试、负载测试、连接速度测试&#xff09; 二、Web界面测试&#xff1a;&#xff08;导航测试、图形测试、内容测试、整体界面测试&#xff09; 三、Web兼容性测试&#xff1a;&#xff08;平台&#x…

Linux搭建Java环境——安装MySQL5.7(CentOS7.6)

一、使用Xftp上传MySQL安装包&#xff0c;并解压 这里可以直接使用安装包也可以使用命令直接在Xshell运行 wget http://dev.mysql.com/get/mysql-5.7.26-1.el7.x86_64.rpm-bundle.tar 当然要提前创建好文件夹/opt/mysql&#xff0c;并cd进去&#xff0c;运行tar -xvf mysql-…

通义千问写的高考作文你觉得怎么样?

目录 一、全国卷-全国甲卷 二、北京卷 三、上海卷 今天带大家使用通义千问来体验一下2023年高考作文&#xff0c;大家一起来一睹为快吧&#xff01; 一、全国卷-全国甲卷 阅读下面的材料&#xff0c;根据要求写作。&#xff08;60分&#xff09; 人们因技术发展得以更好地掌控…

计算机网络管理-使用SNMPc开展网管活动

一、实验目的 全面学习SNMPc网络管理软件业务服务监控功能&#xff0c;了解如何使用网管软件从事网络管理工作 二、实验内容与设计思想 1&#xff09;操作映射数据库。 2&#xff09;查看管理对象的MIB数据。 3&#xff09;创建、保存长期统计数据&#xff08;要求一定时长…

django传统全栈开发一个ChatGPT应用

根据客户需求&#xff0c;开发一个能多人使用的ChatGPT平台&#xff0c;背后使用的是ChatGPT的api_key。 需求 1、可多轮对话 2、可删除对话 3、流式显示对话 4、可多人使用 5、多个api_key均衡使用 技术分析 第一次接触openai的二次开发&#xff0c;看文档、看文章&…

软件测试-金融银行项目怎么测?系统业务测试总结分析...

目录&#xff1a;导读 前言一、Python编程入门到精通二、接口自动化项目实战三、Web自动化项目实战四、App自动化项目实战五、一线大厂简历六、测试开发DevOps体系七、常用自动化测试工具八、JMeter性能测试九、总结&#xff08;尾部小惊喜&#xff09; 前言 银行测试人员的组…

《计算机组成原理》期末考试手写笔记——模块三:运算方法与运算器(变形补码计算+原码补码一位乘法计算+浮点运算)

本课程的期末考试复习笔记与你分享&#xff0c;祝你考试成功~ 目录 &#xff08;一&#xff09;知识点总结 知识点1&#xff1a;计算机中的运算&#xff08;不考&#xff09; 知识点2&#xff1a;定点加减法运算&#xff08;很简单&#xff09; 知识点3&#xff1a;定点乘法…

IDE/在VS2015中集成Qt开发环境

文章目录 概述安装VS2015安装Qt VS Tools 插件Qt相关配置应用到所有项目配置增加配置删除 VS项目属性下Qt工程配置&#xff08;Qt Project Settings&#xff09;Qt VersionsQt Modules编辑器找不到Qt的类 QtDesigner在VS下使用无法打开UI/打开失败找不到UI编译中间文件UI添加的…

package.json与package-lock.json区别需不需要被.gitignore文件忽略

前言 先要搞清楚package.json与package-lock.json文件分别是什么作用 .gitignore文件是干什么的-主页文章有 介绍 package.json 1.项目名称版本号&#xff0c;描述&#xff0c;以及运行命令和一些配置的node环境命令 2.记录那些包会在开发环境中用到&#xff0c;哪些包会…

【项目】从零实现一个高并发内存池

目录 一、项目介绍 1、该项目的原型 2、该项目所涉及到的技术及博主往期参考文章 3、池化技术 4、内存池的内碎片和外碎片 二、先来看一个定长内存池设计 三、高并发内存池的三层框架设计 1、thread cache的实现 1.1thread cache整体框架 1.2哈希桶映射对齐规则 1.3…

【送书福利-第九期】Web安全攻防从入门到精通

大家好&#xff0c;我是洲洲&#xff0c;欢迎关注&#xff0c;一个爱听周杰伦的程序员。关注公众号【程序员洲洲】即可获得10G学习资料、面试笔记、大厂独家学习体系路线等…还可以加入技术交流群欢迎大家在CSDN后台私信我&#xff01; 本文目录 一、前言二、内容介绍三、作者介…

【LeetCode】768. 最多能完成排序的块 II

768. 最多能完成排序的块 II&#xff08;困难&#xff09; 思路 对于已经分好块的数组&#xff0c;若块数大于 1&#xff0c;则可以得到以下结论&#xff1a;「 右边的块的所有数字均大于或等于左边的块的所有数字」。考虑这个问题&#xff1a;对于已经分好块的数组&#xff0…

Linux——多线程互斥

多线程互斥 抢票问题互斥锁锁的接口 理解锁锁的背景概念如何看待锁&#xff1a;加锁和解锁的原理锁的封装 可重入与线程安全死锁死锁的概念与条件 抢票问题 这里用上一篇&#xff1a; https://blog.csdn.net/qq_63580639/article/details/131054847?spm1001.2014.3001.5501 的…

LVGL lv_color_t 像素定义详解

更多源码分析请访问&#xff1a;LVGL 源码分析大全 目录 1、概述2、颜色格式详解2.1、LV_IMG_CF_RAW_X2.2、LV_IMG_CF_TRUE_COLOR_X2.3、LV_IMG_CF_INDEXED_XBIT2.4、LV_IMG_CF_ALPHA_XBIT2.5、LV_IMG_CF_RGBX 3、送显函数&#xff08;flush_cb&#xff09;中的 lv_color_t附录…

【P54】JMeter 生成概要结果(Generate Summary Results)

文章目录 一、生成概要结果&#xff08;Generate Summary Results&#xff09;参数说明二、准备工作三、测试计划设计 一、生成概要结果&#xff08;Generate Summary Results&#xff09;参数说明 可以将测试结果在客户端模式下输出&#xff0c;同时能美化压测输出的结果 使…

申请Let‘s Encrypt免费SSL证书、自动化续签证书

一、环境 安装证书的环境为Centos Nginx&#xff0c;如果没有安装Nginx则需要先安装。 二、申请流程 1、开放80和443端口 firewall-cmd --permanent --add-port80/tcp firewall-cmd --permanent --add-port443/tcp firewall-cmd --reload2、安装 certbot 使用certbot工具能…

Doris动态表使用快速入门实战

1. 动态表构功能概述 半结构化数据&#xff0c;是介于结构化和非结构化之间的数据。和普通纯文本相比&#xff0c;半结构化数据具有一定的结构性。和结构化数据相比&#xff0c;其结构变化复杂&#xff0c;我们又不能方便的使用结构化的方式去描述它。 半结构的数据中通常即包…

Java基础知识总结归纳

0. 入门常识 0.1 Java 特点 0.2 Java 和 C 0.3 JRE 和 JDK 0.4 Java 程序编译过程 1. 数据类型 1.1 基本数据类型 1.2 引用类型 1.3 封装类 1.4 缓存池 2. 字符串 String 2.1 定义 2.2 不可变性的优点 2.3 String vs StringBuffer vs StringBuffer 2.4 字符串常量池…