VINS（五）非线性优化与在线标定调整

首先根据最大后验估计（Maximum a posteriori estimation，MAP）构建非线性优化的目标函数。

初始化过程通过线性求解直接会给出一个状态的初值，而非线性优化的过程关键在于求解增量方程，并不断迭代到最优点，需要在初值以及后续的迭代点附近线性化（泰勒展开保留一阶后平方构建高斯牛顿梯度下降的增量方程）：

在初值x附近泰勒展开

$f(x+\Delta x) = f(x) + J\Delta x$

$costFunction = [f(x+\Delta x)]^{2}$

最小化costFunction的过程其实就是迭代求解增量方程

$H\Delta x = b$

其中$H = J^{T}J$，$b = -J^{T}f$

因此，首先需要定义状态的增量，主要因为旋转矩阵没有加法，无法迭代求解，因此在SO(3)流形的正切空间上定义四元数的加法，并表示成最小坐标的李代数（轴转角）形式

因此对应的error-state costFunction定义为：

注意该式求解的是迭代需要使用的增量，而不是初始化过程可以直接线性求解出的初值。这是优化过程和初始化过程中costFunction的主要不同点。

接下来还需要建立对应的观测方程去计算residual及其Jacobian，并且推到误差的传播方程。和初始化过程中对比，增加的有：

IMU中的旋转矩阵；
误差传播考虑旋转矩阵及其与translation之间的相关性；

对比初始化和非线性优化过程：

都是基于Sliding Window，但是初始化中每组Sliding Window只会计算一次，如果没有收敛到可以完成初始化，则扩大窗口大小，或者直接滑动窗口；
Nonlinear Optimization中Sliding Window则是在固定的Sliding Window中求解增量方程迭代优化到满足一定条件（residual收敛或者限制固定迭代次数保证计算量的平衡）；

转载于:https://www.cnblogs.com/shang-slam/p/7081923.html

VINS（五）非线性优化与在线标定调整相关推荐

imu相机标定_解放双手——相机与IMU外参的在线标定
本文作者沈玥伶,公众号:计算机视觉life,编辑部成员一.相机与IMU的融合在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图.这是因为这两个 ...
VINS外参在线标定估计
由于使用的数据相对参数提供的不够精确导致结果精度不高,因此需要研究一下外参的在线估计. VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化. 1.参数配置在参数配置文件ya ...
VINS 外参在线标定
在VINS中相机的外参RicR_{ic}Ric是可以在线动态标定的,实现函数为: /*** @brief 外参在线标定* * @param corres 两帧图像之间的共视特征点* @param d ...
多传感器融合定位十五-多传感器时空标定(综述)
多传感器融合定位十五-多传感器时空标定 1. 多传感器标定简介 1.1 标定内容及方法 1.2 讲解思路 2. 内参标定 2.1 雷达内参标定 2.2 IMU内参标定 2.3 编码器内参标定 2.4 ...
多传感器融合感知 --传感器外参标定及在线标定学习
0. 简介作为无人车以及智能机器人而言,在装配过程中各个传感器之间的外参标定一直是比较头疼的问题.这里作者也系统的学习了一下,传感器的外参标定和在线标定问题.下图是我们常用的几个坐标系,而对于常用的 ...
多传感器融合感知：传感器外参标定及在线标定算法详解
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[全栈算法]技术交流群后台回复[相机标定]获取超详细的单目双目相机模型介绍.内外参标定 ...
基于自动驾驶车辆的激光雷达与摄像头之间的在线标定算法
原文首发于微信公众号「3D视觉工坊」--基于自动驾驶车辆的激光雷达与摄像头之间的在线标定算法本文英文名为<Automatic Online Calibration of Cameras and ...
for根据ID去重_汽车ECU参数标定之配置Overlay RAM实现Qorivva MPC57xx系列MCU参数在线标定和代码重映射原理和方法详解...
内容提要引言 1. MPC5744P的Overlay RAM工作原理介绍 2 MPC5744P的Flash Overlay配置详解 2.1 平台Flash标定区域描述字寄存器配置字0--PFLASH ...
【LCC系列】自动驾驶中激光雷达和相机的自动在线标定（2013）
文章目录 1 动机 2 方法 2.1 提取图像边缘 2.2 提取点云深度不连续 2.3 目标函数 2.4 检测标定误差 2.5 自动在线标定 3 实验 3.1 在线检测标定误差 3.2 在线纠正标定误 ...

VINS（五）非线性优化与在线标定调整

VINS（五）非线性优化与在线标定调整相关推荐

最新文章

热门文章