Slam Vslam十四讲笔记 9~14讲

第9讲后端1

9.1 概述

9.1.1 状态估计的概率解释

视觉里程计只有短暂的记忆，后端优化中，考虑一段更长时间内的状态估计问题。

当前状态只由过去时刻决定，称为是 渐进的 incremental，如果当前的状态由过去信息更新后，也会用未来的信息来估计，称为是批量的 batch

每个方程都会收到噪声影响，位姿和路标要看成服从某种概率分布的随机变量，问题为根据运动数据u和观测数据z，如何确定状态量x和y的分布，通常将噪声和状态量假设为服从高斯分布（程序只需要存储均值和协方差即可）

第六讲中提到，批量估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解

令$x_k$为k时刻的所有未知量，包括当前位姿和m个路标点： $\boldsymbol{x}_{k} \stackrel{\text { def }}{=}\left\{\boldsymbol{x}_{k}, \boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{m}\right\}$ 同时，将k时刻的所有观测记作 $z_k$，运动方程和观测方程写成： $\left\{\begin{array}{l} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k} \end{array} \quad k=1, \ldots, N .\right.$ 第k时刻，用过去0到k中的数据估计现在的状态分布： $P(x_k|x_0,u_{1:k},z_{1:k})$ 按照贝叶斯法则： $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right) \propto P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) .$ 第一项为似然，第二项为先验，对于先验部分，当前状态xk是基于所有的状态估计得来的，至少会收到$x_{k-1}$的影响，以$x_{k-1}$时刻为条件概率展开（??）： $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=\int P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}$ 考虑k时刻和k-1时刻之间的关系：

假设马尔可夫性，k时刻状态只和k-1时刻状态有关，得到以 扩展卡尔曼滤波（EKF）为代表的滤波器方法
考虑k时刻和之前所有状态的关系，得到非线性优化为主题的优化框架

9.1.2 线性系统和KF

因为假设了马尔可夫性，则上一个积分中第一部分进一步简化： $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)$ 第二部分，由于k时刻的uk和k-1时刻的状态无关，所以uk可以拿掉： $P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k-1}, \boldsymbol{z}_{1: k-1}\right)$

这一系列方程说明实际在做的是如何把k-1时刻的状态分布推导致k时刻，程序运行期间，只需要维护一个状态量，不断更新迭代即可，如果是高斯分布，维护状态量的均值和协方差即可，实际上，系统输出的信息通常就是自己的位姿和不确定性。

线性高斯系统（线性方程描述）： $\left\{\begin{array}{l} \boldsymbol{x}_{k}=\boldsymbol{A}_{k} \boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=\boldsymbol{C}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k} \end{array} \quad k=1, \ldots, N\right.$ 并假设w和v噪声服从0均值的高斯分布： $\boldsymbol{w}_{k} \sim N(\mathbf{0}, \boldsymbol{R}) . \quad \boldsymbol{v}_{k} \sim N(\mathbf{0}, \boldsymbol{Q}) .$ 记符号 $\hat{x_k}$为后验，$\check{x_k}$为先验，高斯分布的线性变换仍然是高斯分布，下面一步称为预测predict，原理暂略，假设已经知道了k-1时刻的后验状态估计 $\hat{x_{k-1}}$和协方差 $\hat{P_{k-1}}$，根据k时刻的输入和观测来确定xk的后验分布 $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=N\left(\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R}\right)$ 记： $\check{\boldsymbol{x}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \quad \check{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R} .$ …具体推导暂略，得到：

9.1.3 非线性系统和EKF

卡尔曼滤波的结果扩展到非线性系统中，称为扩展卡尔曼滤波器，公式暂略…

9.1.4 EKF的讨论

计算资源有限，或者带估计量比较简单的场合

局限：

…

9.2 BA与图优化

BA bundle adjustment 指从视觉图像中提炼出最优的3D模型和相机参数（内参和外参）。

考虑从任意特征点发射出来的光线 bundles of light rays ，在相机的几个不同成像平面上变成像素或是检测到的特征点，调整 adjustment各个相机的姿态和特征点的空间位置，使得最后光线收束到相机的光心，就称之为BA

流程：

一个图上检测到几个特征点
第二图上也检测到了特征点，进行特征点匹配，进行三角定位，获取到三维空间的点位置
根据计算到的点位置和计算得到的位姿，进行第二次投影，即重投影
误差指的是重投影的位置和后面图上特征点之间的误差

9.2.1 投影模型和BA代价函数

再复习一下投影过程：

将世界坐标转化为相机坐标，需要用到外参： $\boldsymbol{P}^{\prime}=\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}=\left[X^{\prime}, Y^{\prime}, Z^{\prime}\right]^{\mathrm{T}}$
再投至归一化平面，得到归一化平面坐标： $\boldsymbol{P}_{\mathrm{c}}=\left[u_{\mathrm{c}}, v_{\mathrm{c}}, 1\right]^{\mathrm{T}}=\left[X^{\prime} / Z^{\prime}, Y^{\prime} / Z^{\prime}, 1\right]^{\mathrm{T}}$
再考虑畸变坐标，假设只考虑径向畸变： $\left\{\begin{array}{l} u_{\mathrm{c}}^{\prime}=u_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \\ v_{\mathrm{c}}^{\prime}=v_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \end{array}\right.$ 其中， $r_{\mathrm{c}}^{2}=u_{\mathrm{c}}^{2}+v_{\mathrm{c}}^{2}$
根据内参模型，计算像素坐标： $\left\{\begin{array}{l} u_{s}=f_{x} u_{\mathrm{c}}^{\prime}+c_{x} \\ v_{s}=f_{y} v_{\mathrm{c}}^{\prime}+c_{y} \end{array}\right.$

整体的代价函数： $\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|e_{i j}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{z}_{i j}-h\left(\boldsymbol{T}_{i}, \boldsymbol{p}_{j}\right)\right\|^{2}$ 其中T指的是外参的李群，$z_{i,j}$为在位姿$T_i$处观测路标$p_j$产生的数据，对这个最小二乘进行求解，相当于对位姿和路标同时做了调整，即所谓的BA

9.2.2 BA的求解

尽管误差项都是单个位姿和路标点的，但是在BA整体目标函数上，应该把自变量定义成所有待优化的变量： $\boldsymbol{x}=\left[\boldsymbol{T}_{1}, \ldots, \boldsymbol{T}_{m}, \boldsymbol{p}_{1}, \ldots, \boldsymbol{p}_{n}\right]^{\mathrm{T}}$ 当给自变量一个增量时，目标函数变为： $\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}+\boldsymbol{F}_{i j} \Delta \boldsymbol{\xi}_{i}+\boldsymbol{E}_{i j} \Delta \boldsymbol{p}_{j}\right\|^{2}$ Fij是整个代价函数在当前状态下对相机姿态的偏导数，Eij是该函数对路标点的偏导，简化表达，讲相机位姿变量放在一起xc，将空间点的变量也放在一起xp $\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2}=\frac{1}{2}\left\|\boldsymbol{e}+\boldsymbol{F} \Delta \boldsymbol{x}_{c}+\boldsymbol{E} \Delta \boldsymbol{x}_{p}\right\|^{2}$ 这里的E和F必须是整体目标函数对整体变量的导数，将是一个很大块的矩阵，里面的每个小分块，需要由每个误差项的导数Fij和Eij拼凑起来，无论是使用高斯牛顿法还是LM方法，都要面对增量线性方程： $\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ 将雅可比矩阵分块：$\boldsymbol{J}=\left[\begin{array}{ll} \boldsymbol{F} & \boldsymbol{E}] \end{array}\right.$，以高斯牛顿法为例，H矩阵为： $\boldsymbol{H}=\boldsymbol{J}^{\mathrm{T}} \boldsymbol{J}=\left[\begin{array}{ll} \boldsymbol{F}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{F}^{\mathrm{T}} \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{E}^{\mathrm{T}} \boldsymbol{E} \end{array}\right]$ 因为考虑了所有的优化变量，所以这个线性方程的维度将非常大，包含了所有的位姿和路标点。如果直接对H求逆，消耗量极大，但H有特殊结构，来加速求解。

9.2.3 稀疏性和边缘化

H矩阵的稀疏性是由雅可比矩阵J引起的，假设现在只考虑eij，这个误差项只描述了在Ti出看到pj这件事，即只涉及第i个相机位姿与第j个路标点，对其余部分的变量导数都为0，所以J由下面形式： $\boldsymbol{J}_{i j}(\boldsymbol{x})=\left(\mathbf{0}_{2 \times 6}, \ldots \mathbf{0}_{2 \times 6}, \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{T}_{i}}, \mathbf{0}_{2 \times 6}, \ldots \mathbf{0}_{2 \times 3}, \ldots \mathbf{0}_{2 \times 3}, \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{p}_{j}}, \mathbf{0}_{2 \times 3}, \ldots \mathbf{0}_{2 \times 3}\right)$ 除了这两处都为0，体现了误差项与其他路标和轨迹无关的特新，从图优化的角度来说，这条观测边之和两个定点有关。如果J只有两个非零块，那么H将会只有4个非零块。

对整体H而言： $\boldsymbol{H}=\sum_{i, j} \boldsymbol{J}_{i j}^{\mathrm{T}} \boldsymbol{J}_{i j}$ 此时这里的i是所有位姿取值，j是所有路标点中取值。

对H进行分块： $\boldsymbol{H}=\left[\begin{array}{ll} \boldsymbol{H}_{11} & \boldsymbol{H}_{12} \\ \boldsymbol{H}_{21} & \boldsymbol{H}_{22} \end{array}\right]$ H11只和相机位姿有关，H22之和路标点有关，H11和H22都是对角阵，且只在Hxx处有非零块，对于H12和H21，根据观测数据可能是稠密的或者稀疏的。

举例子：

C是相机，P是观测点，能观测到，连线。此时BA目标函数为： $\frac{1}{2}\left(\left\|e_{11}\right\|^{2}+\left\|e_{12}\right\|^{2}+\left\|e_{13}\right\|^{2}+\left\|e_{14}\right\|^{2}+\left\|e_{23}\right\|^{2}+\left\|e_{24}\right\|^{2}+\left\|e_{25}\right\|^{2}+\left\|e_{26}\right\|^{2}\right)$ 以e11而言，令$\boldsymbol{x}=\left(\boldsymbol{\xi}{1}, \boldsymbol{\xi}{2}, \boldsymbol{p}{1}, \cdots, \boldsymbol{p}{2}\right)^{\mathrm{T}}$： $\boldsymbol{J}_{11}=\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{x}}=\left(\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{\xi}_{1}}, \mathbf{0}_{2 \times 6}, \frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{p}_{1}}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}\right)$ 整体的雅可比矩阵和H矩阵的稀疏情况如图：

仔细观察，会发现除了对角元素外，图的邻接矩阵和H矩阵有着一致的结构：

图优化结构与增量方程的稀疏性存在着明显的联系。

考虑更一般的亲口光，通常路标数量远大于相机的数量：

因为形状像箭头或者镐子，又称为箭头形矩阵或者镐形矩阵，利用稀疏性，可以加速计算，最常用的手段是Schur消元，在SLAM研究中也被称为 边缘化 Marginalization

记：

由此： $\left[\begin{array}{ll} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]$ B C都是对角块矩阵，C的每个对角块是3x3，对对角线矩阵求逆，即可得到对角块矩阵的逆，进行高斯消元： $\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \mathbf{0} & \boldsymbol{I} \end{array}\right]\left[\begin{array}{cc} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \boldsymbol{0} & \boldsymbol{I} \end{array}\right]\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]$ 整理后得到： $\left[\begin{array}{cc} \boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{\mathrm{T}} & \mathbf{0} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{c} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w} \\ \boldsymbol{w} \end{array}\right]$ 再根据： $\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{\mathrm{T}}\right] \Delta \boldsymbol{x}_{\mathrm{c}}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}$ C是对角块，所以容以求解，解得xc，代回方程 $\Delta \boldsymbol{x}{p}=\boldsymbol{C}^{-1}\left(\boldsymbol{w}-\boldsymbol{E}^{\mathrm{T}} \Delta \boldsymbol{x}{\mathrm{c}}\right)$

消元后的方程与xp无关，所以叫做边缘化

xc的那个方程的系数记为 S(也是位姿的协方差矩阵)，H矩阵的非对角块处的非零元素对应着相机和路标的关联，这里不加证明地说，S矩阵的非对角线上的非零矩阵块表示了该处对应的两个相机比那辆之间存在着共同观测的路标点，有时被称为共视（co-visibility），反之，如果该块为0，表示这两个相机没有共同观测。

schur消元并不意味着将所有路标消元，将相机变量消元也是SLAM中常用的手段。

9.2.4 鲁棒核函数

由于误匹配的情况存在，邻接图中可能加上不合理的边，如果误差很大，可能会抹平其他正确的边影响，核函数可以保证每条边的误差不会过于大而掩盖其他的边，具体做法是，将二范数度量替换成一个增长没有那么快的函数，同时保证自己的光滑性质，常用的有Huber核： $H(e)=\left\{\begin{array}{ll} \frac{1}{2} e^{2} & \text { 当 }|e| \leqslant \delta \\ \delta\left(|e|-\frac{1}{2} \delta\right) & \text { 其他 } \end{array}\right.$

9.3 实践：Ceres BA

..暂略

第10讲后端2

10.1 滑动窗口滤波和优化

10.1.1 实际环境下的BA结构

BA规模太大，无法保持计算的实时性，通常需要折中：

滑动窗口：BA固定在一个时间窗口，提取关键帧，可以是时间上靠近，也可以是时间上靠近，同时空间上也靠近（防止相机不动的情况）
共视图：“与现在的相机存在共同预测的关键帧构成的图”

10.1.2 滑动窗口法

取窗口N个关键帧，位姿表达为 $x_1,…,x_N$，假设在向量空间内，即用李代数表达，主要关心的是关键帧在哪里，以及不确定度如何，即高斯分布假设下的均值协方差。

设滑动窗口中还有M个路标点yi，与N个关键帧构成局部地图，利用第9讲的BA来处理，边缘化所有路标点来加速器求解，边缘化时，考虑关键帧的位姿，即 $\left[\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right]^{\mathrm{T}} \sim N\left(\left[\boldsymbol{\mu}_{1}, \ldots, \boldsymbol{\mu}_{N}\right]^{\mathrm{T}}, \boldsymbol{\Sigma}\right)$ 其中$\mu _k$为第k个关键帧的位姿均值，$\Sigma$为所有关键帧的协方差矩阵，显然，均值为BA迭代之后的结果，$\Sigma$为整个BA的H矩阵进行边缘化之后的结果，即第9讲中的S矩阵

当窗口结构放生变化后，状态量如何改变？

需要再窗口新增一个关键帧，以及观测到的路标点
删除旧的关键帧以及观测到的路标点

新增关键帧和路标点

问题变成了变量变为N+1个关键帧和更多路标点的集合，同样按照BA流程处理即可。

删除旧的关键帧

当删除就的关键帧x1，x1并不是孤立的，会和其他关键帧观测到同样的路标，将x1边缘化后将导致整个问题不再稀疏

这里是边缘化关键帧，不是第9讲举例的边缘化路标点

假设x1看见y1~4，考虑边缘化x1，此时右下角的路标点矩阵块不再是非对角矩阵，这个过程为边缘化中的填入 fill-in

如果边缘化某个关键帧时，同时边缘化观测到的路标点，可以继续保持右下角的对角块矩阵结构。

SWF中边缘化的直观解释

边缘化在概率上的意义就是指条件概率，即保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率，从数学上来看，相当于是： $p\left(\boldsymbol{x}_{1}, \ldots \boldsymbol{x}_{4}, \boldsymbol{y}_{1}, \ldots \boldsymbol{y}_{6}\right)=p\left(\boldsymbol{x}_{2}, \ldots, \boldsymbol{x}_{4}, \boldsymbol{y}_{1}, \ldots \boldsymbol{y}_{6} \mid \boldsymbol{x}_{1}\right) \underbrace{p\left(\boldsymbol{x}_{1}\right)}_{\text {舍去 }}$

10.2 位姿图

10.2.1 位姿图的意义

经过若干次观测之后，收敛的特征点位置变化较小，发散的外点则已被剔除，再优化，费力不讨好，更倾向于优化几次后，将特征点固定，只把它们当作位姿估计的约束

如果不管路标，只管轨迹，可以得到一个只有轨迹的图优化，位姿节点之间的边由两个关键帧之间的特征匹配之后得到的运动估计来给定初始值，一旦初始化后，路标点相当于是固定了，不再进行优化，只关心所有的相机位姿之间的联系，这个过程就是构建位姿图 pose graph

一个关键帧一般关联几百个关键点，实时的BA最大计算规模，即使利用稀疏性，一般也只有几万个点左右

要么滑动窗口，丢弃历史数据

要么位姿图，舍弃路标点的优化

10.2.2 位姿图的优化

节点表示位姿，Ti来表达，边则是两个位姿节点之间相对运动的估计，假设估计Ti和Tj之间的运动$\Delta T_{ij}$，该运动表达方式可以为： $\Delta \boldsymbol{\xi}_{i j}=\boldsymbol{\xi}_{i}^{-1} \circ \boldsymbol{\xi}_{j}=\ln \left(\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee}$ 或者按李群的写法： $\boldsymbol{T}_{i j}=\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}$ 实际不会精确成立，设立最小二乘误差 $\boldsymbol{e}_{i j}=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee}$ 优化变量为 $\xi _i$和$\xi _j$，求关于这两个变量的导数，按照李代数的求导方式，各给一个左扰动

…emm 推导暂略

10.3 实践：位姿图优化

暂略….

第11讲回环检测

11.1 概述

11.1.1 回环检测的意义

如果只考虑相邻时间上的关键帧，会出现累计误差，无法构建全局一致的轨迹和地图（漂移），回环检测的关键在于检测出相机经过同一个地方这件事

11.1.2 回环检测的方法

最简单的方式，任意两幅图像做一遍特征匹配，根据匹配的数量来判断关联，但计算量太大；随机抽取历史数据进行回环检测，但随着帧数增长，抽中回环的概率将大幅下降。

可以通过基于外观来判断是否可能出现回环（两幅图像的相似性）

11.1.3 准确率和召回率

真阳性：TP true positive
假阳性：FP false positive 又称为感知偏差
假阴性：FN false negative 又称为感知编译
真阴性：TN true negative

定义统计量准确率(所有回环中判断确实为真实回环的概率)和召回率（所有真实回环中被正确检测的概率） $\text { Precision }=\mathrm{TP} /(\mathrm{TP}+\mathrm{FP}), \quad \text { Recall }=\mathrm{TP} /(\mathrm{TP}+\mathrm{FN})$ 因为两者存在矛盾，所以选这两个统计量，通常画PR图，关心曲线偏向右上方的程度。在slam中P一般更为重要，因为判断错了，可能导致建图失败，但如果没检测到回环，但多几次回环可能就会检测到。

11.2 词袋模型

判断两张图，两张图有许多预先定义的各种元素（可以称之为字典），比较两张图出现的同类元素数量，即可判断两张图的相似度

不考虑元素出现的位置，出现的顺序 Bag-of-Words

11.3 字典

11.3.1 jiedian d jiegou

字典的生成类似于聚类问题

首先对大量的图像提取特征点，例如有N，想得到一个k个单词的字典

可以通过经典的K-means算法解决

可以使用一种K叉数来表达字典：逐层使用Kmeans聚类，叶子节点为单词，深度为d的树，可以容纳 $k^d$个单词，每次和每个中间节点的聚类中心比较，可以快速找到

11.3

第12讲建图

12.1 概述

地图的用处：

定位
导航
避障：局部的动态的障碍物处理
重建：不满足于稠密点云等，更希望能够构建带纹理的平面
交互：人与地图之间的互动，比如“拿取桌上的报纸”，需要机器人对地图有更高层面的认知——语义地图

12.2 单目稠密重建

12.2.1 立体视觉

需要获取像素点的距离，解决方案：

单目相机：估计相机运动三角化
双目相机：视差计算距离
RGB-D相机：直接获得像素距离

室内一般RGB-D，室外或者大场景利用立体视觉估计深度信息

单目为例，比较特征点的过程：

对图像提取特征，根据描述子进行特征之间的匹配，相当于是对某一个空间点进行了跟踪，知道了它在各个图像之间的位置
三角测量得到深度

而对于稠密深度图估计中，需要用到极线搜索和块匹配技术来确定一幅图的某像素出现在其他图例的位置

12.2.2 极线搜索与块匹配

单目 d为可能的深度，这条线段在另一个成像平面上投影成一条线，称之为极线，当知道相机之间的运动时，极线也是能够确定的。

直接比较像素的亮度是不可靠的，一个直观的想法是比较像素块，取p1周围的小块，在l2上也取很多同样大小的块进行比较，这就是所谓的块匹配，有若干种不同的计算方法来计算小块之间的差异：

SAD sum of absolute difference $S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{SAD}}=\sum_{i}|\boldsymbol{A}(i, j)-\boldsymbol{B}(i, j)|$
SSD sum of squared distance 平方和 $S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{SSD}}=\sum_{i, j}(\boldsymbol{A}(i, j)-\boldsymbol{B}(i, j))^{2}$
NCC normalized cross correlation 归一化互相关 $S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{NCC}}=\frac{\sum_{i, j} \boldsymbol{A}(i, j) \boldsymbol{B}(i, j)}{\sqrt{\sum_{i, j} \boldsymbol{A}(i, j)^{2} \sum_{i, j} \boldsymbol{B}(i, j)^{2}}}$ 相关性接近0表示不相似，接近1表示相似

有时还先将每个小块的均值去掉，允许“小块B比A整体上亮一些，但仍然很相似”的情况

假设用了NCC，将得到一个沿着极线的NCC分布，会得到一个非凸函数：分布存在许多峰值，真实的对应点必定只有一个。在这种情况下，会倾向于用概率分布来描述深度值，而非用一个单一的数值来描述。问题转化为不断进行不同图像间的极限搜索时，深度分布将发生怎么样的变化——这就是所谓的深度滤波器

12.2.3 高斯分布的深度滤波器

对像素点深度的估计，本身也可以建模为一个状态估计问题，自然存在滤波器与非线性优化两种求解思路，虽然非线性优化效果比较好，但是LSAM实时性要求较高，前端已经占据了不少的计算量，建图方面通常采用计算量较少的滤波器方式

设某个像素点的深度d服从： $P(d)=N\left(\mu, \sigma^{2}\right)$ 每当新的数据到来，观测深度，同样假设观测是一个高斯分布： $P\left(d_{\text {obs }}\right)=N\left(\mu_{\text {obs }}, \sigma_{\text {obs }}^{2}\right)$ 再使用观测的信息来更新原先的d分布，高斯分布的乘积依然是一个高斯分布，根据高斯分布的乘积： $\mu_{\mathrm{fuse}}=\frac{\sigma_{\mathrm{obs}}^{2} \mu+\sigma^{2} \mu_{\mathrm{obs}}}{\sigma^{2}+\sigma_{\mathrm{obs}}^{2}}, \quad \sigma_{\mathrm{fuse}}^{2}=\frac{\sigma^{2} \sigma_{\mathrm{obs}}^{2}}{\sigma^{2}+\sigma_{\mathrm{obs}}^{2}}$ obs的数据的处理方式：

考虑几何不确定性和光度不确定性之和
仅考虑几何不确定性

假设这只考虑几何不确定性，某次极限搜索，找到p1对应点为p2，认为p1对应的三维点为P，令O1P为p，O1O2为平移t；假设存在一个像素大小的误差，p2变为了p2‘，此时的p’和p误差为多大呢？

几何关系： $\begin{array}{l} \boldsymbol{a}=\boldsymbol{p}-\boldsymbol{t} \\ \alpha=\arccos \langle\boldsymbol{p}, \boldsymbol{t}\rangle \\ \beta=\arccos \langle\boldsymbol{a},-\boldsymbol{t}\rangle . \end{array}$ p2扰动一个像素，使$\beta$产生一个变化量： $\begin{aligned} \beta^{\prime} &=\arccos \left\langle\boldsymbol{O}_{2} \boldsymbol{p}_{2}^{\prime},-\boldsymbol{t}\right\rangle \\ \gamma &=\pi-\alpha-\beta^{\prime} . \end{aligned}$ 由正弦定理： $\left\|\boldsymbol{p}^{\prime}\right\|=\|\boldsymbol{t}\| \frac{\sin \beta^{\prime}}{\sin \gamma}$ 如果认为块匹配仅有一个像素的误差，可以设： $\sigma_{\mathrm{obs}}=\|\boldsymbol{p}\|-\left\|\boldsymbol{p}^{\prime}\right\|$ 如果确定性大于一个像素，根据此推导放大这个不确定性