视觉SLAM14讲学习笔记-ch9

YJD-Climb

于 2024-09-15 16:56:37 发布

阅读量886

点赞数 8

文章标签：笔记

本文链接：https://blog.csdn.net/shuaibidebei/article/details/142253104

版权

本博客主要记录学习高翔老师视觉SLAM14讲的总结笔记，若有总结不对的地方，欢迎大家与我讨论。

一、状态估计

由于传感器存在噪声，所以位姿x和路标点y可看成服从某种概率分布的随机变量，并且误差是逐渐累计的，所以更需要通过真实的观测数据来减小误差。
当前时刻的 $x_{k}$ ，若不仅使用过去的信息更新，也用到了未来的信息更新，则称之为批量方法，若仅用 $x_{k-1}$ 则称为渐进方法。
将当前位姿和路标点位置未知量统一记作 $x_{k}$ ，即 $x_{k}=\left \{ x_{k}, y_{1}, \cdots ,y_{m} \right \}$ ，则状态方程为：

$\left\{\begin{matrix} x_{k}=f(x_{k-1},u_{k})+w_{k} \\ z_{k}=h(x_{k})+v_{k} \end{matrix}\right.$

则k时刻的状态分布可写为：

$P(x_{k}\mid x_{0},u_{1:k},z_{1:k})$

由贝叶斯法则：

$P(x_{k}\mid x_{0},u_{1:k},z_{1:k})\propto P(z_{k}\mid x_{k})P(x_{k}\mid x_{0},u_{1:k},z_{1:k-1})$

$P(x_{k}\mid x_{0},u_{1:k},z_{1:k-1})=\int P(x_{k}\mid x_{k-1}, x_{0},u_{1:k},z_{1:k-1})P(x_{k-1}\mid x_{0},u_{1:k},z_{1:k-1})dx_{k-1}$

二、卡尔曼滤波

由一阶马尔可夫性假设，上式只与k-1时刻有关，所以可以代入到状态方程中，估计当前状态，其核心是由预测-校正两步组成的。

2.1 线性系统与KF滤波

线性高斯系统可由下式表示：

$\left\{\begin{matrix} x_{k} =Ax_{k-1}+u_{k}+w_{k}\\ z_{k}=Cx_{k}+v_{k} \end{matrix}\right.$

KF滤波假设已知k-1时刻的后验状态估计 $\hat{x_{k-1}}$ 和 $\hat{P_{k-1}}$ ，随后经过3步计算：

先验（预测）：

$\left\{\begin{matrix} \check{x_{k}}=A_{k}\hat{x_{k-1}}+u_{k} \\ \check{P_{k}}=A_{k}\hat{P_{k-1}}A_{k}^{T}+R \end{matrix}\right.$

卡尔曼增益：

$K=\check{P_{k}}C_{k}^{T}(C_{k}\check{P_{k}}C_{k}^{T}+Q_{k})^{-1}$

后验（校正）：

$\left\{\begin{matrix} \hat{x_{k}}=\check{x_{k}}+K(z_{k}-C_{k}\check{x_{k}}) \\ \hat{P_{k}}=(I-KC_{k})\check{P_{k}} \end{matrix}\right.$

K与 $Q_{k}$ 成反比，当观测数据不可信时，则K较小，更加相信先验状态。卡尔曼滤波器构成了线性系统的最优无偏估计。

2.2 非线性系统与EKF

SLAM中运动方程和观测方程常常是非线性的，高斯分布经过非线性变换后，往往不再是高斯分布，所以EKF考虑只在某点附近的一阶泰勒展开，只保留一阶线性部分。

一阶泰勒展开：

$x_{k}\approx f(\hat{x_{k-1}},u_{k})+\frac{\partial f}{\partial x_{k-1}}\mid \hat{x_{k-1}}(x_{k-1}-\hat{x_{k-1}})+w_{k}$

$z_{k}\approx h(\check{x_{k}} )+\frac{\partial h}{\partial x_{k}}\mid \check{x_{k}}(x_{k}-\check{x_{k}})+n_{k}$

其中设 $F=\frac{\partial f}{\partial x_{k-1}}\hat{x_{k-1}}$ ， $H=\frac{\partial h}{\partial x_{k}}\check{x_{k}}$

预测：

$\left\{\begin{matrix} \check{x_{k}}=f(\hat{x_{k-1}, u_{k}}) \\ \check{P_{k}}=F\hat{P_{k-1}}F^{T}+R_{k} \end{matrix}\right.$

卡尔曼增益：

$K_{k}=\check{P_{k}}H^{T}(H\check{P_{k}}H^{T}+Q_{k} )^{-1}$

校正：

$\left\{\begin{matrix} \hat{x_{k}}=\check{x_{k}}+K_{k}(z_{k}-h(\check{x_{k}} ))\\ \hat{P_{k}}=(I-K_{k}H)\check{P_{k}} \end{matrix}\right.$

三、BA与图优化

在视觉SLAM中往往没有运动模型，仅有观测模型，其总结如下：

P点从世界坐标系->相机坐标系：

$P'=RP+t$

P'投影至归一化平面，得归一化坐标

$P_{c}=\begin{bmatrix} u_{c} & v_{c} & 1 \end{bmatrix}^{T}=\begin{bmatrix} x'/z' & y'/z' & 1 \end{bmatrix}^{T}$

考虑畸变情况：

$\left\{\begin{matrix} u_{c}'=u_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4}) \\ v_{c}'=v_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4}) \end{matrix}\right.$

根据内参模型，计算像素坐标：

$\left\{\begin{matrix} u_{s}=f_{x}u_{c}'+c_{x}\\ v_{s}=f_{y}v_{c}'+c_{y} \end{matrix}\right.$

观测模型的代价函数：

$\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left \| e_{ij} \right \| ^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left \| z_{ij}-h(T_{i},P_{j}) \right \| ^{2}$

BA就是指求解该最小二乘，同时对位姿和路标点进行优化调整。

3.1 BA求解

将待优化变量放在一起 $x=\begin{bmatrix} T_{1} & \dots & T_{m} & P_{1} & \dots & P_{n} \end{bmatrix}^{T}$ ，利用非线性优化方法求解：

$\frac{1}{2} \left \| f(x+\bigtriangleup x) \right \| ^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left \| e_{ij}+F_{ij}\bigtriangleup \xi_{i}+E_{ij} \bigtriangleup P_{j}\right \| ^{2}$

$F_{ij}$ 代价函数对姿态的偏导数， $E_{ij}$ 代价函数对路标点位置的偏导数。若将求和号转换为矩阵拼接，可将上式转换为下式：

$\frac{1}{2} \left \| f(x+\bigtriangleup x) \right \| ^{2}=\frac{1}{2} \left \| e+F\bigtriangleup x_{c}+E \bigtriangleup x_{p}\right \| ^{2}$

由此也可得出，代价函数的雅克比矩阵为：

$J=\begin{bmatrix} F &E \end{bmatrix}$

增量方程为：

$H\bigtriangleup x =g$

其中H为：

$H = J^{T}J=\begin{bmatrix} F^{T}F &F^{T}E \\ E^{T}F &E^{T}E \end{bmatrix}$

由于H矩阵是一个维数相当大的矩阵，因此需要采用一些技巧才能求解该增量方程。

3.2 稀疏性和边缘化

$J_{ij}=\begin{bmatrix} 0_{2\times 6} & \dots & \frac{\partial e_{ij}}{\partial T_{i}} & \dots & 0_{2 \times 3} & \dots & \frac{\partial e_{ij}}{\partial P_{j}} & \dots \end{bmatrix}$

取J中的某部分，仅在第i处和第j处为非零值，其余全为0，同时也说明误差项只与这两个顶点有关。

$H=\sum_{i,j}J_{ij}^{T}J_{ij}=\begin{bmatrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{bmatrix}$

H11只与相机位姿有关，为对角阵，H22只与路标点有关，也为对角阵，H12和H21需根据观测数据而定。H矩阵称为邻接矩阵，描述了第(i,j)元素，若两个顶点存在联系，则该位置元素不为0，H矩阵中的非对角部分的非零矩阵块可以理解为其对应的两个变量之间存在联系，即约束。

在SLAM中，H矩阵常称为箭头形矩阵，由于路标点数量远远多于相机位姿数量。面对H矩阵，常使用Shur消元，也称为边缘化。

$H\bigtriangleup x = g$

$\Rightarrow \begin{bmatrix} B& E\\ E^{T}&C \end{bmatrix}\begin{bmatrix} \bigtriangleup x_{c}\\ \bigtriangleup x_{p} \end{bmatrix}=\begin{bmatrix} v\\ w \end{bmatrix}$

$\Rightarrow\begin{bmatrix} I &-EC^{-1} \\ 0 &I \end{bmatrix}\begin{bmatrix} B& E\\ E^{T}&C \end{bmatrix}\begin{bmatrix} \bigtriangleup x_{c}\\ \bigtriangleup x_{p} \end{bmatrix}=\begin{bmatrix} I &-EC^{-1} \\ 0 &I \end{bmatrix}\begin{bmatrix} v\\ w \end{bmatrix}$

$\Rightarrow \begin{bmatrix} B-EC^{-1}E^{T} & 0 \\ E^{T} & C \end{bmatrix} \begin{bmatrix} \bigtriangleup x_{c}\\ \bigtriangleup x_{p} \end{bmatrix}= \begin{bmatrix} v-EC^{-1}w\\ w \end{bmatrix}$