之前有仔细介绍过非线性优化，其中二阶梯度的方法有高斯牛顿法、LM算法、dogleg算法等。前段时间仔细研究了dogleg算法的实现步骤，今天抽出时间总结一下。 dogleg属于二阶梯度法，为了解决高斯牛顿法在使用一阶导数平方近似计算hessian矩阵时可能不准确的问题，它结合了高斯牛顿法与最速下降法，通过定义近似半径来限制近似的可执行范围。本质上与LM算法类似，不过实现细节大有不同。 算法 d

前面已经讲了很多vins里的细节操作，今天来谈谈将整个系统最核心的后端优化部分。 vins的后端优化 一般在vins系统中，视觉与imu数据经过预处理后，此时如果已经完成初始化，则会在滑窗中进行非线性优化操作。整个过程一般会是如下过程： 对滑窗中所有特征点进行三角化操作 针对现有的窗口中的数据，构建非线性优化问题，并为该问题添加优化参数与待优化残差项，然后进行优化； 优化完成后进行滑窗操作（根据

最近这几天在看vins的源码，刚好前天写了初始化中如何确定陀螺仪的偏置，今天顺手将初始化中尺度、速度、重力向量的初始化也总结一下。 初始化 此处初始化的本质其实就是视觉sfm测量与imu预积分的松耦合对齐，也就是寻找最适合的待优化的变量使得sfm与imu预积分结果对的最齐。 首先，我们需要确定待初始化的变量有： \[ \mathcal{X}_{I}^{3(n+1)+3+1}=\left[v_{

vins是一个很好的开源工作，最近一直在熟悉它，其中大部分的内容之前已经或多多少提到过了，这篇博客主要讲一讲vins初始化中的视觉与imu联合初始化工作。 注意：\(T_{WI}\)指的是从I系到W系的变换矩阵。 陀螺仪偏置标定问题 这个工作主要是在vins/vins_estimator/src/initial/initial_aligment.cpp文件中的solveGyroscopeBi

在vio中会经常看到预积分这个词，vins中有，okvis中也有，所以，这个东西到底有什么作用呢？ 预积分的由来 首先，一般imu的频率是比相机高的，所以在两个图像帧之间，会有很多imu采集的数据。如下图： pre-inte 此处假设IMU的真实值是\(\omega, \mathbf{a}\)，测量值为\(\tilde{\boldsymbol{\omega}}, \tilde{\mathbf

随着VSLAM系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量也会越来越大。 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。 滑动窗口算法 流程 增加新的变量进入最小二乘系统优化 如果变量数目达到了一定的维度，则移除老的变量 系统不断循环前面两步 信息矩阵 一般对于基于高斯分布的最大似然估计转化的

在slam中经常会使用滑动窗口法来控制非线性优化的计算量级，此时当从现有的窗口中去除旧的变量时，如何调整信息矩阵就成为了一个棘手的问题。这篇博客就是要介绍如何使用舒尔补来简单的完成这个任务。 舒尔补 给定任意的矩阵块M，如下所示： \[ \mathbf{M}=\left[\begin{array}{ll}{\mathbf{A}} & {\mathbf{B}} \\ {\mathbf{C}}

零均值的多元高斯分布有如下概率形式： \[ p(\mathbf{x}) - \frac{1}{Z} \exp \left(-\frac{1}{2} \mathbf{x}^{\top} \mathbf{\Sigma}^{-1} \mathbf{x}\right) \] 其中\(\mathbf{\Sigma}\)是协方差矩阵，协方差矩阵的逆可以记作\(\mathbf{\Lambda}=\mathbf{

IMU数学模型 加速度计 首先，对于世界坐标系，一般我们会使用最常见的东北天(ENU)坐标系G（无关远点位置，只与姿态有关）。 在这个坐标系中，重力加速度为 \[g^G = (0,0,-9.81)^T\]。 此时，假设IMU坐标系就是ENU坐标系，则\(R_{IB}=I\)，静止时有（其中\(a_{m}\)是测量值）： \[a = 0\] \[a_{m} = -g\] 所以，不静止时：（此处

正交矩阵 正交矩阵的定义如下：如果 \[ AA^T = E \] 其中E为单位矩阵，则称n阶实矩阵A为正交矩阵。 所以正交矩阵的性质如下： 1. 正交矩阵的每一列、行都是单位向量，并且两两正交。最简单的正交矩阵就是单位阵。 正交矩阵的逆等于正交矩阵的转置。由此可以推断出正交矩阵的行列式的值肯定为正负1。 所有的矩阵都可以看成一种变换。正交矩阵的变换可以看成，如果作用在一组空间基向量上，它会将