SLAM初学-后端(一)

date
Jun 13, 2022
Last edited time
Mar 27, 2023 08:50 AM
status
Published
slug
SLAM初学-后端(一)
tags
SLAM
summary
type
Post
origin
https://www.notion.so/lazurite/b7b15695a55244ed8505bd2a5dc9f18c
Field
Plat
后端要做什么？概述状态估计的概率解释线性系统与KF(卡尔曼滤波)后验的估计后验的更新总结非线性系统和EKF(扩展卡尔曼滤波)线性化预测更新EKF的局限BA 与图优化优化目标

后端要做什么？

前端能够根据相邻的两幅图像判断出此时此刻的位姿，是暂时的；那么后端需要对前端测量以及计算的结果进行矫正，不仅用过去的信息，也用未来的信息更新自己，希望能够得到一个长时间的正确状态。
前面有讲过，在运动方程和观测方程中，如果把位姿和路标看成随机变量，就可以把问题变为已知观测数据和运动数据情况下，如何确定状态量的分布问题，是一个状态估计问题。假设噪声和状态量服从高斯分布，只需要估计它的均值和方差，即可确定状态量的分布。
完成后端优化可以使用滤波器，也可以使用非线性优化，本讲对两种方法都做了推导。

概述

状态估计的概率解释

SLAM过程可以由运动方程与观测方程来描述, 那么假设在 到 的时间段内, 有位姿 到 , 并且有路标 , 那么运动方程与观测方程写为:
其中, 为运动数据, 为观测数据. 注意以下几点:
  1. 观测方程中, 只有 能看到 才有观测数据, 否则就没有. 一般情况下, 由于视觉SLAM的特征点数量 众多, 实际的观测方程数量会远远大于运动方程.
  1. 由于可能没有能够测量运动方程的设备, 因此没有运动方程. 这时候, 我们会假设相机保持静止或者相机保持匀速直线运动.
  1. 当只有运动方程的时候, 误差是逐渐累积的, 我们对位置方差的估计将会越来越大. 而如果有观测方程, 那么位置估计的不确定性会变小.
我们可以将位姿和路标写在一起, 将 记为 , 其为 时刻的所有未知量, 包含了当前时刻的相机位姿与 个路标点. 将 时刻的所有观测记为 . 那么, 原方程可以写为:
在已知 时刻的状态、 时刻的观测下，估计 现在的状态分布 , 则可以得到
证明
记 . 则
假设 之间相互独立, 则 , 带入得:
另外也可以得到 .
即
其中, 第一项为由观测方程确定得似然, 第二项为先验部分.
先验概率中 时刻的状态 受到前面 时刻状态的影响，对先验概率进行以 时刻为条件展开，可以写为
  • 后续的方法
      • 现在没有具体的概率分布形式, 后续有不同的不同的方式进行处理.
      1. 马尔可夫方法
          2. 假设马尔可夫性（k时刻状态只与k-1时刻状态有关，与之前状态无关）。
      1. 非线性优化
          2. 依然考虑 时刻状态与之前所有状态之间的联系, 此时将得到以非线性优化为主题的优化框架.

线性系统与KF(卡尔曼滤波)

由于假设了马尔可夫性, 则当前时刻的状态只与上一个时刻的状态有关，式二第一个概率化简为 , 第二个概率化简为 .
  • 假设
      1. 状态量服从高斯分布
      1. 可以使用线性方程描述运动方程和观测方程
假设状态量服从高斯分布, 那么我们只需要维护状态的均值以及方差即可. 在线性高斯系统中，用线性方程描述运动方程和观测方程，写为
假设这里的噪声服从高斯分布: . 假设我们已知 时刻的状态估计 与协方差 . 记 为后验分布, 为先验分布.

后验的估计

那么, 先验的分布为:
这一步称为预测, 它表示了如何从上一个时刻的状态, 根据有噪声的输入信息推断当前时刻的状态分布. 记为 . 则

后验的更新

后验的分布为:
那么, 根据式二, 我们假设 , 可以得到:
由于等式两侧都是高斯分布, 因此只需要比较等式两边的指数部分.
比较 的二次系数有:
则
令
得到后验方差表达式:
另外, 还可以仅使用先验概率来表示 如下:
证明
Sherman-Morrison-Woodbury，SMW恒等式
Sherman-Morrison-Woodbury(SMW)G-l恒等式（有时也称为矩阵求逆引理(matrix inversion lemma))。实际上，这个等式是从一个恒等式衍生出来的四个不同的等式。 SMW公式以美国统计学家Jack Sherman, Winifred J. Morrison和Max A. Woodbury的名字命名，但是由英国数学家W. Duncan,美国统计学家L. Guttman和M. Bartlett以及其他人独立提出. 分解的本质是对矩阵初等变换成对角形式，然后移到等式右边；也可以直接根据矩阵的乘法和分解格式进行求解，不再赘述，可直接搜索LDU分解。
Sherman-Morrison-Woodbury，SMW恒等式
https://lazurite.vercel.app/SMW-Equation
Sherman-Morrison-Woodbury，SMW恒等式
由 SMW 恒等式, , 令 .
那么, 可以得到:
则
另外, 比较 一次系数可得:
带入式(6), 得到均值方差表达式:

总结

上面的步骤可以归纳为预测以及更新两个步骤.
  1. 预测
  1. 更新: 先计算 ,它又称为卡尔曼增益
      2. 然后计算后验概率分布:
在第一步预测中，使用 时刻的后验分布估计 时刻的先验分布，这一步不确定性变大，第二步使用 时刻的先验分布估计 时刻的后验分布，对于结果进行修正，缩小不确定性。

非线性系统和EKF(扩展卡尔曼滤波)

由于在SLAM中的运动方程以及观测方程一般都是非线性函数, 那么一个符合高斯分布的状态经过非线性变换之后, 往往就不再满足高斯分布. 因此, 我们将卡尔曼滤波器的结果拓展到非线性系统, 通常会在某个点处对方程进行展开，保留一阶项，按照线性系统进行推导。

线性化

对于 时刻的运动方程与观测方程, 我们在 处进行线性化(即一阶泰勒展开).
记 , 上式可以写为:

预测

我们使用 时刻的后验 来得到 时刻的先验 , 因此将 带入得到:
对于方差 只与 的方差 和噪声方差 有关, 则
这里认为 为一个确定的常数, 那么 也是一个常数.
则预测部分为:
其中, 为 的方差.

更新

卡尔曼增益为:
然后计算后验概率分布:
这一步与 KF 一致

EKF的局限

  1. 滤波器方法在一定程度上假设了马尔可夫性，也就是 时刻的状态只与 时刻相关，而与 之前的状态和观测都无关.
  1. 由于 EKF 仅仅做了一阶泰勒展开, 因此存在非线性误差.
  1. EKF 需要存储状态量的均值和方差，并对它们进行维护和更新, 因此不可适用于大型场景.
  1. EKF 等滤波器由于没有异常检测机制, 因此导致系统在存在异常值的时候容易发散.

BA 与图优化

Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。

优化目标

回顾整个投影过程，假设已知相机的外参数 和世界坐标系的 点坐标，求解 点的像素坐标过程。
  1. 世界坐标转化为相机坐标：
  1. 相机坐标投影到归一化平面：
  1. 去畸变（径向畸变）：
  1. 计算像素坐标：
整个过程中，用到了相机的位姿 和路标 的世界坐标，得到了路标的像素坐标，对应到观测方程 中，观测 就是像素坐标  ，相机位姿 用李代数表示 ，路标 用三维点 的坐标。那么通过降低观测到的像素坐标 和估计到的像素坐标 之间的误差，就可以得到最优的 和 的解。用最小二乘表示整体的代价函数为：
采用非线性优化更新增量的方式求解，令  ，  。
引入增量后，目标函数变为：
其中, 表示 对于相机位姿的偏导数，  表示 对于路标点位置的偏导数。
接下来就可以使用非线性优化方法进行求解.
 

© Lazurite 2021 - 2023