slam知识点day7day8|26届暑期实习

等待暑期实习offer的牛油果

已于 2025-04-11 11:56:01 修改

阅读量993

点赞数 22

分类专栏： SLAM十四讲学习文章标签：视觉SLAM十四讲

于 2025-03-22 17:49:12 首次发布

本文链接：https://blog.csdn.net/weixin_52878972/article/details/146410161

版权

SLAM十四讲学习专栏收录该内容

2 篇文章

订阅专栏

21号看完了，但是记下来要写好久呀

SLAM十四讲第二部分

第7讲视觉里程计1

我还想说多看几讲，第一讲看到一半就头大了，一堆的数学推导，先记录一下再继续看吧

在单幅图像提取特征点，多幅图像间匹配特征点；

用对极几何约束，恢复图像之间的相机的三维运动；

PnP问题：3D和2D，用一些3D点和它们在相机的投影位置，估计出相机的运动。

ICP问题：两组3D点，利用点云的匹配关系求解相机的三维运动。

三角化：获得二维图像上对应点的三维结构。

7.1 特征点法

视觉里程计根据相邻图像之间的信息估计出相机的运动提供给后端。

视觉里程计算法分为两大类：特征点法和直接法。

特征点法是主流，稳定、对光照、动态物体不敏感。提取、匹配图像特征点、估计两帧之间的相机运动和场景结构。这类算法也称为两视图几何。

7.1.1特征点

视觉里程计的核心问题是如何根据图像估计相机运动。图像是一个由亮度和色彩组成的矩阵，如果直接从矩阵层面考虑运动估计，很困难，所以需要选取有代表性的点，将这些点称为路标，在相机视角发生少量变化后会保持不变，在视觉SLAM中，路标则是指图像特征。

在图像提取特征的方式：

在不同图像间辨认角点，确认它们的对应关系。角点的提取算法有Harris角点、FAST角点、GFTT角点。
提取人工设计的特征点（关键点+描述子）:因为远处的角点离近了可能就不是角点了、旋转相机的时候，角点的外观发生变换也辨认不出角点了。特征点：SIFT、SURF、ORB。

特征点有：

可重复性:可以在不同图像找到相同特征

可区别性：不同特征不同表达

高效率：特征点数量远小于像素数量

本地性：特征仅与一小片区域相关。

提取SIFT（尺度不变特征变换）特征点=提取SIFT关键点，并计算SIFT描述子。

关键点：指该特征点在图像的位置，有的还包括朝向、大小信息。
描述子：向量，描述关键点周围像素的信息，按某种人为设计的方式。设计原则：外观相似的特征该有相似的描述子。只要两个特征点的描述子在向量空间上的距离相近就可以认为是相同的特征点。

7.1.2 ORB特征

ORB的关键点：改进的FAST角点，相比原版的FAST计算了特征点的主方向，为BRIEF增加了旋转不变性；

ORB的描述子：BRIEF，对提取出的特征点的周围图像区域进行描述，在BRIEF中使用了先前计算的方向信息。

FAST角点：主要检测局部像素灰度变化明显的地方，检测过程：

原始FAST会出现角点扎堆现象，所以还要进行非极大值抑制，在一定区域内仅保留响应极大值的角点。

FAST角点重复性不强、分布不均匀、不具有方向信息、尺度问题：远看是近看不是角点的。

针对不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述：尺度不变性由构建图像金字塔，并在每一层上检测角点实现，特征的旋转由灰度质心法实现。

图像金字塔：底层是原始图像，每往上一层就对图像进行一个固定倍率的缩放，得到不同分辨率的图像，较小的图像看成远处图像，较大的看成近处，在特征匹配算法中，可以匹配不同层的图像，实现尺度不变性。比如相机后退的时候，可以在上一个图像金字塔的上层和下一个图像金字塔的找到匹配。
灰度质心法：计算特征点附近的图像灰度质心，即以图像块灰度值作为权重的中心，连接图像块的几何中心的O与质心C得到方向向量，从而得到特征点的方向，使角点有了旋转的描述。

通过以上两个方法改进了FAST角点，具有了尺度与旋转的描述，提升了鲁棒性。

BRIEF描述子：提取完关键点，对每个点计算描述子，ORB改进BRIEF特征描述。先介绍BRIEF:二进制描述子，描述向量由许多个0和1组成，用于编码关键点附近随机两像素（p、q）的大小，p大于q就取1，否则取0；这种随机选点的速度非常快，适用于实时的图像匹配。但是原始的BRIEF不具有旋转不变性，图像旋转的时候容易丢失。改进的BRIEF因为在提取关键点的时候计算了方向，可以用进来，从而得到旋转不变性。

7.1.3 特征匹配

通过对图像之间或者图像与地图之间的描述子准确匹配，进行数据关联，为后序姿态估计、优化等操作减轻负担，但是因为图像特征的局部特性、场景中经常存在大量重复纹理，导致特征描述非常相似，比如非结构化物体，就会导致误匹配。

正常匹配方法：

暴力匹配：计算每个特征点与下一时刻所有特征点测量描述子的距离，然后排序，取最近的作为匹配点。计算描述子距离表示两个特征之间的相似程度，分情况可以取不同的距离度量范数：浮点型，欧氏距离；二进制，汉明距离（不同位数的个数）。
快速近似最近邻，因为暴力方法计算量太大。

7.2.3 计算相机运动

利用已经匹配好的点对，估计相机的运动。

单目相机，只有2D像素坐标，根据两组2D点利用对极几何估计运动
一组3D一组2D点，即根据一些3D点和它们在相机的投影坐标利用PnP转为3D-3D再估计运动
双目，RGB-D相机，根据两组3D点利用ICP估计运动

7.3 2D-2D: 对极几何

7.3.1 对极约束

一对匹配好的二维点之间的几何关系：第一帧 $I_{1}$ 到第二帧 $I_{2}$ 的运动为 $R,t$ ，相机中心分别为 $O_1,O_2$ ,特征点 $p_1$ 匹配到的 $p_2$ ，如果匹配正确它们就确实是一个点在两个成像平面的投影。

基线：O1O2

极平面：连线 $O_1p_1$ 与 $O_2p_2$ 相交于P，O1 O2 P形成的平面。

极线：极平面与两个像平面的交线

极点：基线与像平面交点

对极约束推导：根据相机投影方程、尺度不变等价关系、坐标归一化推导出。

对极约束：

E为本质矩阵、F为基础矩阵，两者之间只差了内参

简化得： $x_2^T E x_1 = p_2^T F p_1 = 0.$

于是为了估计相机位姿R、t，就需要先根据配对点的像素位置求E或F，求E形式更简单。

7.3.2 本质矩阵

因为对极约束是等式为零的约束，所以E乘任意非零常数仍成立，所以E在不同尺度下是等价的。

本质矩阵E的内在性质：E的奇异值必定是 $[\sigma, \sigma, 0]^T |$ 的形式。

平移旋转各有三个自由度，所以 $t^{\wedge} R$ 共六个，但因为尺度等价性，所以只有五个。

八点法估计E：利用对极约束方程、展开矩阵E、变成线性方程组，解线性方程组得到。具体看书169页。

估计完E后如何求R、t：利用SVD奇异值分解，设E的SVD为 $E = U\Sigma V^T$ ， $\Sigma = \text{diag}(\sigma, \sigma, 0)$

分解后得到4个可能的解，只有其中一个解的两个相机深度都为正，其他都是错误解。

更简单的做法取 $\Sigma = \text{diag}(1, 1, 0)$ ，因为尺度等价性。

7.3.3 单应矩阵

除了E、F，还有单应矩阵H估计场景中的特征点都落在同一平面的特例情况。假设一个平面方程 $n^T P + d = 0$ 、通过单应矩阵对匹配好的特征点变换 $p_2 \simeq H p_1$ ，对H分解、求解线性方程组得到。求出H后，同样也要再分解求R、t，分解方法包括数值法、解析法。

在SLAM中的应用：单应矩阵在特征点共面或相机纯旋转时有重要意义，此时基础矩阵自由度下降、会退化。为避免退化现象的影响，通常同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的一个作为最终的运动估计矩阵。

对于分解的R、t有四种可能性，OPENCV会帮我们检测深度是否为正，选出正确的解。

单目SLAM有一步不可避免的初始化，初始化的两张图像必须有一定程度的平移，不能只有纯旋转，否则t为0,E也为0，无法求解R，初始化之后的轨迹和地图都将以此步的平移为单位。

当可能存在错误匹配的情况时，采用RANSAC随机采样一致性。

7.5 三角测量

2D-2D---对极几何（对极约束+本质/单应矩阵）--->相机运动----三角测量--->深度

用相机的运动通过三角测量得到特征点的空间位置。三角测量是指通过不同位置对同一个路标点进行观察，从观察的位置推断路标点的距离。

$s_2 x_2 = s_1 R x_1 + t.$

$x_1,x_2$ 是归一化图像坐标， $s_1,s_2$ 是深度因子。

$s_2 x_2^\wedge x_2 = 0 = s_1 x_2^\wedge R x_1 + x_2^\wedge t.$

通过左乘反对称阵 $x_2^\wedge$ 为0求得 $s_2$ ,然后就可以再求出 $s_1$ ，得到了两帧的点的深度（肯定是不同的），就可以确定它们的空间坐标了（没误差的话都会投到同一个点），由于噪声的存在两个直线可能不相交，所以常见做法是求最小二乘解。

三角测量的矛盾：平移很小时，像素不确定性导致深度不确定性，三角化精度不够；平移很大时，图像外观会发生明显变化，导致匹配失效。这个问题称为“视差”。

原地旋转而没有平移就会使单目视觉出现追踪失败、尺度不正确等情况。在机器人中常见。

可以定量计算每个特征点的位置及不确定性的曲线，得到深度滤波器。

7.7 3D-2D: PnP

两张图像中的一张的特征点投影位置已知，且对应的3D位置已知，最少只需3个匹配的特征点对就可以估计相机运动，特征点的3D位置可以通过三角化测量等方式得到。所以双目和RGB-D的视觉里程计中可以直接PnP估计，单目得先初始化得到3D才能再PnP。PnP就不需要再对极约束了，也不需要很多点对。

PnP问题的求解方法：P3P、直接线性变换法（DLT）等、非线性优化：光束平差法（BA）。

7.7.1 直接线性变换

通过线性方程组求解，需要6对匹配点实现，匹配点超过6对可以用SVD方法对超定方程求最小二乘解。

7.7.2 P3P

P3P只用3点对求解PnP，3D世界坐标系A,B,C和2D投影像素坐标系a,b,c，验证点对D-d，一但算出了3D相机坐标系下的坐标，就变成3D-3D，ICP问题。

利用三角形相似性、余弦定理、化简、然后求解最多得到四个可能解，用验证点计算最可能的解，从而得到A、B、C在相机坐标系下的3D坐标，根据3D-3D求解R、t利用ICP，会非常容易。

SLAM中最常见的做法是通过P3P等先估计相机位姿，再构建最小二乘优化问题对估计值调整（进行BA）。

7.7.3 BA 最小化重投影误差求解PnP

线性方法往往是先求相机位姿再求空间点位置

非线性优化把它们都看成优化变量，放在一起优化，可以用它对PnP、ICP给出的结果进行优化。把相机和三位点放在一起进行最小化的问题统称为Bundle Adjustment。

求解过程：通过投影模型、把误差求和构建最小二乘问题，误差项是3D点的投影位置与观测位置作差，称为重投影误差。 $\mathbf{T}^* = \arg \min_{\mathbf{T}} \frac{1}{2} \sum_{i=1}^n \left\| \mathbf{u}_i - \frac{1}{s_i} \mathbf{K}\mathbf{T}\mathbf{P}_i \right\|_2^2$

然后求雅可比矩阵，就是计算误差关于相机位姿的导数，通过这些雅可比矩阵可以用列文伯格马尔夸特方法求解，同时优化相机位姿和特征点的三维位置，即Bundle Adjustment优化。

7.9 3D-3D： ICP

激光SLAM也会碰到ICP问题，但是激光数据特征不够丰富，无从得知两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，这个方法称为迭代最近点。

视觉中，因为有匹配关系，就会简单。

ICP的求解方式也分为线性（SVD方法）和非线性（类似于BA）。

7.9.1 SVD方法

求解步骤：首先计算两组点云的质心；然后将两组点云分别减去各自的质心，得到去质心坐标；接着构造矩阵W，它是去质心后点对的外积和；对W进行SVD分解得到旋转矩阵R；最后根据两组点云的质心和旋转矩阵计算平移向量t。

7.9.2 非线性优化方法

将目标函数表示为最小化两组点之间的误差平方和，求解雅可比矩阵，不断迭代找到最小值。

它不会陷入局部最小值，所以可以任意选定初始值。

当我们已知特征点之间的匹配关系时，最小二乘问题有解析解，其实不必使用迭代优化。但在一些场景下，如RGB-D SLAM中，某些像素的深度数据可能缺失，此时可以混合使用PnP和ICP：对于已知深度的特征点构建3D-3D误差，对于未知深度的特征点构建3D-2D重投影误差，将所有误差统一到同一个优化问题中求解。

第8讲视觉里程计2

8.1 直接法的引出

特征点法的缺点：

关键点提取和描述子计算耗时
忽略了特征点以外的所有信息
在特征缺失的地方，可能找不到足够的匹配点计算相机运动

克服方法：

不计算描述子，使用光流法跟踪特征点的运动，估计相机运动是仍使用对极几何、PnP、ICP
不计算描述子，使用直接法计算特征点在下一时刻图像中的位置，根据图像像素灰度信息同时估计相机运动和点的投影，不要求提取到的点必须为角点。只要场景中存在明暗变化就可以，与特征点法只能构建稀疏地图（只有特征点）相比，直接法可以恢复稠密和半稠密结构

8.2 2D光流

光流描述像素随时间在图像中的运动，计算部分像素运动的称为稀疏光流（如LK光流），计算所有像素的称为稠密光流（Horn-Schunck光流）。

Lucas-Kanade 光流

灰度为 $I(x,y,t)$ ，光流基本假设：灰度不变假设，同一个空间点的像素灰度值在各个图像中固定不变。利用灰度不变假设写方程、泰勒展开保留一阶项、化简得到关于速度的方程、假设一个窗口内的像素具有相同运动得到超定线性方程、最小二乘求解、得到像素在图像中的运动速度u、v，从而可以跟踪特征点。

OPENCV中，在第一张图象中提取角点，用光流追踪它们在第二张图像中的位置，然后调用函数提供前后两张图像及对应的特征点，即可得到追踪后的点，以及各点的状态、误差，根据状态是否为1来确定是否正确追踪到。

调用cv::parallel_for并行调用函数计算指定范围内的光流，并行for循环内部是Intel tbb库实现的。

多层光流可以用图像金字塔，从顶层图像开始计算，把上一层（粗糙）的追踪结果作为下一层（精细）光流的初值。实现由粗至精的光流。好处在于，当像素运动较大时，在图像金字塔顶层的图像看来运动仍然在很小的范围内，如原始图像金字塔特征点运动了20个像素，很容易由于图像非凸性导致优化困在极小值里，假设有0.5缩放倍率的金字塔，在上上层看来只运动了5个像素。

可以通过光流跟踪的特征点用对极几何、PnP、ICP估计相机运动，代替了描述子匹配。但是要求相机运动较平滑、对光照稳定性的要求更高。

8.4 直接法

8.4.1 直接法的推导

直接法的基本思路是：根据当前相机位姿估计值预测特征点在第二帧中的位置，然后通过最小化两帧图像中对应点的灰度差异来优化相机位姿参数。这种方法基于灰度不变假设。直接法通过迭代优化来求解相机位姿，最终可以得到相机的运动参数。

8.4.2 直接法的讨论

直接法可以根据空间点P的来源分为三种类型：

稀疏直接法，P来自于稀疏关键点。
半稠密直接法，P来自部分像素，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。
稠密直接法，P来自所有像素。

稀疏法可以快速求解相机位姿，稠密法可以建立完整地图。

单层直接法的实现类似于并行光流，可以并行计算每个像素点的误差和雅可比。

多层，金字塔，金字塔对图像进行缩放时，对应内参也需要乘以相应的倍率。

相比于特征点法，直接法完全依靠优化来求解相机位姿，如要想要得到正确的优化结果，就需要保证大部分像素梯度能够把优化引导到正确方向。但是实际上图像是非凸函数，会陷入局部最小值，只有相机运动很小才不会有很强的非凸性，直接法才能成立。

总结直接法优缺点，优点：

省去计算描述子时间
只要有像素梯度就可以，不需要有特征点。比如渐变的图像提取不出来特征点，就可以用直接法
可以构建稠密、半稠密地图

缺点： $P(\mathbf{x}_k|\mathbf{x}_0, \mathbf{u}_{1:k}, \mathbf{z}_{1:k}).$

非凸性
单个像素没有区分度，需要计算图像块
灰度值不变的假设太强，会有光照影响。实用的直接法会同时估计相机的曝光参数。

第9讲后端1

9.1概述

9.1.1 状态估计的概率解释

状态估计分为两种处理方式：

"批量的"(Batch)：不仅使用过去信息更新状态，也用未来信息
"渐进的"(Incremental)：当前状态只由过去时刻决定

SLAM过程可通过运动方程和观测方程描述：

$\begin{cases} \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k \\ \mathbf{z}_{k,j} = h(\mathbf{y}_j, \mathbf{x}_k) + \mathbf{v}_{k,j} \end{cases} \quad k = 1, \ldots, N, \, j = 1, \ldots, M.$

批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。

将k时刻的所有未知量记为xₖ，它包含当前时刻的相机位姿与m个路标点 $\mathbf{x}_k \stackrel{\text{def}}{=} \{\mathbf{x}_k, \mathbf{y}_1, \ldots, \mathbf{y}_m\}.$

通过过去0到k中的数据来估计当前的状态分布 $P(\mathbf{x}_k|\mathbf{x}_0, \mathbf{u}_{1:k}, \mathbf{z}_{1:k})$

通过贝叶斯法则，将状态估计分解为似然项和先验项。先验部分表示当前状态是基于过去所有状态估计得来的。如果假设马尔可夫性，认为k时刻状态只与k-1时刻状态有关，就会得到扩展卡尔曼滤波(EKF)方法。视觉SLAM的主流方法是非线性优化方法，考虑k时刻状态与之前所有状态的关系。

9.1.2 线性系统和KF

（以下内容为截图给Claude 3.7 Sonnet生成）

马尔可夫性应用：
- 当假设马尔可夫性后，状态预测可以简化为只依赖上一时刻状态和当前输入
- 这使得计算变得更加简单高效
线性高斯系统：
- 卡尔曼滤波假设运动方程和观测方程都是线性的
- 所有的状态和噪声都服从高斯分布
- 噪声服从零均值高斯分布
卡尔曼滤波两个主要步骤：
- 预测步骤：根据运动方程预测当前状态
- 更新步骤：结合观测数据修正预测结果
卡尔曼增益：
- 定义一个权重系数K，用于平衡预测值和观测值
- K的计算考虑了系统状态的不确定性和观测噪声
概率角度理解：
- 整个过程可以看作是如何从k-1时刻的状态分布推导到k时刻
- 在程序运行期间只需维护状态量的均值和协方差

通过这种方法，卡尔曼滤波器为线性高斯系统提供了最优的无偏估计。这种方法之所以强大，是因为它在线性系统中无需进行任何近似，直接得到最优解。

9.1.3 非线性系统和EKF

EKF是卡尔曼滤波器在非线性系统中的推广应用。在SLAM问题中，运动方程和观测方程通常是非线性的，特别是视觉SLAM中的相机模型需要使用内参模型和李代数表示的位姿，这些都不可能构成线性系统。

EKF的基本思路是：

线性化处理：
- 在当前估计点附近对非线性函数进行一阶泰勒展开
- 只保留线性部分，舍弃高阶项
- 将非线性系统近似为线性系统
雅可比矩阵：
- 对运动方程定义雅可比矩阵F
- 对观测方程定义雅可比矩阵H
- 这些矩阵表示函数在当前点的一阶导数
预测步骤：
- 使用完整的非线性函数计算状态的均值
- 使用线性化后的雅可比矩阵计算状态的协方差
更新步骤：
- 定义卡尔曼增益K
- 结合观测数据更新状态估计
- 更新协方差矩阵

在SLAM这种非线性情况下，EKF提供了单次线性近似下的最大后验估计，而不是像在线性系统中那样提供无偏最优估计。

9.1.4 EKF的讨论

EKF的局限性

马尔可夫性假设：假设当前状态只与上一时刻相关、忽略了与更早时刻的关联、不适合处理回环（当前位置与很久之前位置有关联）的情况。
线性化误差、与非线性优化方法区别：EKF只在一个点处进行一次线性化、当线性化点远离真实值时，一阶泰勒展开不足以近似整个函数、线性近似只在很小范围内有效。而优化方法则在每次迭代后状态估计发生变化后，重新对新的估计点进行泰勒展开，而不像EKF只在固定点做一次泰勒展开，所以EKF可以认为仅是优化方法中的一次迭代。
存储问题：需要存储状态量的均值和协方差矩阵，在大规模场景中，特别是当路标数量很大时，存储需求呈平方增长。
异常值敏感性：缺乏异常检测机制，在视觉SLAM中，特征匹配或光流法容易产生错误匹配，会导致系统不稳定。

9.2 BA与图优化、核函数

BA指从视觉图像中提炼出最优的3D模型和相机内外参。从任意特征点发射的几束光线，会在几个相机的成像平面上变成像素或者检测到的特征点，如果调整各个相机的姿态和各特征点的空间位置，可以使这些光线收束到相机的光心。

9.2.1 投影模型和BA代价函数

坐标转换、归一化投影、畸变矫正、像素映射。

BA的核心是最小化重投影误差，代价函数包括多个相机位姿和多个路标点，通过最小化这个二次项，同时优化相机位姿和路标位置，实现Bundle Adjustment。这个优化问题可以使用非线性优化方法求解。

9.2.2 BA的求解

使用非线性优化方法求解具体步骤如下：

将所有优化变量合成一个大的状态向量x = [T₁,...,Tₘ,p₁,...,pₙ]ᵀ，其中包括m个相机位姿和n个空间点坐标，给一个增量Δx，得到目标函数，简化，通过高斯牛顿法或者列文伯格马尔夸特法得到增量线性方程HΔx = g，分块雅可比矩阵，利用H的稀疏结构求解。

9.2.3 稀疏性和边缘化

H矩阵(信息矩阵)具有稀疏结构。这种稀疏结构可以通过图优化来表示。H矩阵的稀疏性由雅可比矩阵J(x)引起，其中误差项 $e_{ij}$ 只关联相机位姿 $T_i$ 和路标点 $p_j$ ，对其他变量的导数为0。

利用雅可比矩阵可以推导出H的稀疏性

对H矩阵中处于非对角线上的矩阵块来说，如果该矩阵块非零，说明这个位置对应的变量之间会在图中存在一条边，说明两个变量之间存在约束（联系）。

考虑更多的相机位姿和路标点，可以看到箭头形矩阵

为了求解这种稀疏结构的H，可以用Schur消元法，在SLAM中也称为边缘化，Marginalization。

对这个矩阵也可以分为四块，左上角的对角块矩阵对应相机位姿维度，右下角对角块矩阵，每个对角块的维度是路标维度（3维..）。非对角块结构与具体观测数据相关。对应的线性方程组HΔx = g，也拆分、化简消元P249...。消元后得到S矩阵，观察非对角线上的非零矩阵块：

最终结果是求出了关于路标位置变量集合 $x_p$ 的边缘分布，所以称为边缘化。

9.2.4 鲁棒核函数

在BA（Bundle Adjustment）优化问题中，传统方法将误差项的二范数平方作为目标函数。这种做法直观，但存在严重问题：当某个误差项由于异常值（如错误匹配）导致数据错误时，优化算法无法识别这是错误数据，会将所有数据视为误差处理。这相当于观测到一个极不可能出现的数据点，导致优化中出现误差很大的边，梯度也很大，严重影响与该点相关的变量估计。

鲁棒核函数的目的是确保每条边的误差不会太大而掩盖其他正确的边的贡献。它将原始二范数度量替换为增长较缓慢的函数，同时保持自身的光滑性（便于求导）。由于这种函数使优化结果更稳健，因此称为鲁棒核函数（Robust Kernel）。如Huber核函数：
当误差e大于阈值δ后，函数增长由二次形式变为一次形式，相当于限制了梯度的最大值。同时，Huber核函数又是光滑的，可以方便地求导。

可以看出，当误差较大时，Huber核函数的增长明显低于二次函数。

g2o和Ceres的一大不同点：使用稀疏优化时，g2o必须手动设置哪些顶点为边缘化顶点。

本讲总结：

在经典模型中SLAM可以看成状态估计问题。如果我们假设了马尔可夫性，只考虑当前状态，则得到以EKF为代表的滤波器模型。也可以选择考虑所有的运动和观测，它们构成一个最小二乘问题。在只有观测方程的情况下，这个问题称为BA，并可利用非线性优化方法求解。讨论了求解过程中的稀疏性问题，指出了该问题与图优化之间的联系。

第10讲后端2

10.1 滑动窗口滤波和优化

从连续的视频中抽一部分作为关键帧来控制计算规模，但是随着时间关键帧数量越来越多，计算效率下降。通过只保留当前时刻最近的N个关键帧，去掉时间上更早的关键帧，即"滑动窗口法"。

更深入地考虑帧与帧之间的结构，可以定义"共视图" (Covisibility graph)，指那些与当前相机存在共同观测的关键帧构成的图。在BA优化时，按照某些原则在共视图内取一些关键帧和路标进行优化，例如：只优化与当前帧有20个以上共视路标的关键帧，其余部分固定不变。

当窗口更新时，需要处理两种情况：新增关键帧和删除旧关键帧

删除旧关键帧时会遇到"填入"问题——边缘化会破坏稀疏结构

解决方法包括同时边缘化旧关键帧观测的路标点，或保留有共视关系的路标点观测

边缘化在概率上相当于构建条件概率分布，保留先验信息

这种方法适合VO系统，但不适合需要闭环的大规模建图系统

10.2 位姿图

位姿图的意义

通过把特征点固定，只将它们作为位姿估计的约束，可以大幅减少计算量
位姿图只保留相机位姿之间的轨迹，不再优化路标点的位置

位姿图的优化

节点表示相机位姿，边表示两个位姿节点间相对运动的估计，可来自特征匹配、GPS或IMU

表示出相对运动、构建误差函数并求导，可得到位姿优化的目标函数、用高斯牛顿法、列文伯格-马夸特方法等求解，也可使用Ceres或g2o库。

SLAM系统通常采用前后端分离的架构。这种架构将系统分为两个独立部分：

跟踪(Tracking)：前端部分，负责实时响应视频数据，通常以每秒30帧的速度处理图像，提取特征并进行匹配
建图：后端部分，主要指优化功能，可以慢速运行，不需要实时处理，只需在优化完成后将结果返回给前端

这种分离设计的优势在于：

前端可以保持高速实时响应
后端可以执行复杂的优化算法而不影响系统的实时性能
两部分可以并行运行，互不干扰

第11讲回环检测

通常被认为是一个独立模块，主流视觉SLAM回环检测方式：词袋模型

11.1 概述

如果SLAM仅考虑相邻时间帧之间的关系，误差会累积导致整体偏移、回环检测能识别相机回到曾经到访过的位置，为位姿图提供额外约束、这些约束能有效消除累积误差，使轨迹和地图保持全局一致性

回环检测的方法

基于里程计的方法：
- 检测相机是否返回到之前位置附近
- 受累积误差影响，往往难以准确工作
基于外观的方法：
- 不依赖位姿估计，仅根据图像相似性判断
- 能够有效避开累积误差问题
- 已成为视觉SLAM主流方法

图像相似性计算

直接用图像差值(||A-B||)不可靠，因为：
- 图像亮度受环境光照影响
- 相机视角变化导致像素位移

准确率和召回率

假阳性，感知偏差，False Positive；假阴性，感知编译，False Negative。

准确率(Precision) = TP/(TP+FP)：判定为回环的结果中真实回环的比例
召回率(Recall) = TP/(TP+FN)：所有真实回环中被成功检测出的比例
在SLAM中，准确率要求更高，因为假阳性回环可能导致地图严重错误
准确率-召回率曲线用于评价算法性能

SLAM对准确率的要求更高（不能有错误的结果），对召回率相对宽容（有一些回环没检测到，但是不需要很多就够了）。

11.2 词袋模型

词袋bag of words BOW模型的核心思想是使用特征点匹配来判断回环，类似于使用"图像上有哪几种特征"来描述图像。

词袋模型的实现步骤：

建立字典：确定"人"、"车"、"狗"等概念作为BoW中的"单词"，将这些单词组成"字典"
将图像转为向量：统计图像中出现的字典中定义的概念，用单词出现情况描述整幅图像，转换为向量表示
比较相似度：计算向量间的相似度，判断图像的相似程度

例如，如果我们的字典包含"人"(w₁)、"车"(w₂)、"狗"(w₃)，一张包含"人"和"车"的图像A可表示为：

A = 1·w₁ + 1·w₂ + 0·w₃ 或 [1,1,0]ᵀ

这种向量描述的是"特征是否出现"的信息，不关心特征的位置和出现顺序，相机发生少量运动，只要物体仍然出现，就认为描述向量不发生变化。这也是为什么称为"Bag-of-Words"而非"List-of-Words"，强调的是特征的有无，而非顺序。

向量间的相似度可以用多种方式计算，例如：

$s(a,b) = 1 - \frac{1}{W} \|a - b\|_1$

该公式使用L₁范数（元素绝对值之和），W是词典大小，用于归一化相似度得分，使结果在0到1之间。当向量完全相同时得到1，完全相反时得到0。

11.3 字典

字典是词袋模型的核心，由多个"单词"组成，每个单词代表一个视觉概念（特征点的集合）。字典的生成本质上是一个聚类(Clustering)问题，属于无监督机器学习的范畴。

K-means聚类算法

K-means是生成字典的常用方法，其基本步骤：

随机选取k个中心点:c1,⋯,ck
对每个样本，计算与各中心点的距离，分配到最近的类
重新计算每类的中心点
如果中心点变化很小，则算法收敛；否则返回第2步

我们把已经提取的大量特征点聚类成一个含有k个单词的字典。现在的问题变成了如何根据图像中某个特征点查找字典中相应的单词，

K叉树字典结构

为了解决大规模字典中特征匹配的效率问题，可以使用K叉树结构：

在根节点，用K-means将所有样本聚成k类（实际中为保证聚类均匀性通常使用K-means++）
对第一层的每个节点，将属于该节点的样本再聚类成k类，得到下一层
依此类推，最后得到叶子层，叶子节点即为所谓的Words（单词）

K叉树的优势在于查找效率。当需要找到某个特征点对应的单词时，只需将其与各中间节点的聚类中心比较（一共d次，d为树深度），就能找到最终的单词，保证了对数级别的查找效率。

这种结构将原本需要O(n)复杂度的暴力搜索优化为O(log n)，大大提高了大规模字典应用的实用性。

11.4 相似度计算

有了字典后，给定特征，我们可以通过查找字典找到对应的单词。当处理一幅含有N个特征的图像时，找到这N个特征对应的单词后，就可以构建该图像的词袋表示或直方图，描述为"这幅图有一个人和一辆汽车"这样的语义。希望对单词的区分性或者重要性加以评估，给它们不同的权值。

TF-IDF是为词袋模型中的单词赋予不同权重的方法：

IDF (逆文档频率): 单词越稀有，权重越高 $\text{IDF}_{one}= \log \frac{n_{sum}}{n_{one}}$ ,其中 $n_{sum}$ 是字典所有特征数量，n one是单词 w one包含的特征数量
TF (词频): 单词在图像中出现频率越高，权重越大 $\text{TF}_i = \frac{n_i}{n}$ ,其中 $n_i$ 是单词 $w_i$ 在图像A中出现的次数，n是图像A中单词总数
最终权重: $\eta_i = \text{TF}_i \times \text{IDF}_i$

图像表示与比较

图像表示为加权单词集合： $A = \{(w_1, \eta_1), (w_2, \eta_2), \ldots, (w_N, \eta_N)\}$
两图像相似度可用多种方式计算，如L1范数： $s(v_A, v_B) = 2 \sum_{i=1}^{N} |v_{Ai}| + |v_{Bi}| - |v_{Ai} - v_{Bi}|$

这种方法通过为不同特征分配权重，提高了图像匹配的精确度，更适合回环检测应用。

仅使用相似性评分的绝对大小并不总是可靠，因为：

某些环境外观天然相似（如办公室的桌椅）
其他环境各处差异很大

为解决这个问题，可采用相对评分策略：

$s(v_t, v_{t_j})' = s(v_t, v_{t_j})/s(v_t, v_{t-\Delta t})$

这里将当前帧与历史帧的相似度除以当前帧与前一关键帧的相似度进行归一化。例如，如果当前帧与某历史关键帧相似度超过与上一关键帧相似度的3倍，则认为可能存在回环。

关键帧选择对回环检测影响很大：

关键帧过近会导致相似性过高，难以检测真正的回环
理想的关键帧应该稀疏分布，彼此差异较大
当检测到第1帧与第n帧构成回环后，n+1和n+2帧也可能与第1帧构成回环，因此通常将"相近"的回环聚为一类

词袋模型完全基于外观而不利用几何信息，易导致误判。因此通常需要验证步骤：

时间一致性验证：认为单次检测的回环不可靠，需要在一段时间内持续检测到才是有效回环
几何一致性验证：对检测到的回环帧进行特征匹配，估计相对运动，检查与位姿图中已有估计的差异

与机器学习的关系

词袋模型是一种无监督学习方法。随着深度学习发展，更先进的方法出现：对机器学习特征进行聚类，而非人工设计特征。

第12讲建图

12.1 概述

地图的功能：

定位:基本功能
导航：需要稠密地图
避障：需要稠密地图
重建：稠密
交互：语义地图

12.2 单目稠密重建

获取像素点的距离

使用单目相机，估计相机运动并三角化计算像素深度
使用双目相机，利用左右目视差计算像素深度
使用RGB-D相机直接获取像素深度

单目和双目方式（立体视觉）计算深度比较"费力不讨好"，但在室外、大场景下仍能通过立体视觉估计深度信息。RGB-D还无法很好地应用这两个场景。

单目深度估计关键步骤包括：

特征提取与匹配，跟踪空间点在不同图像中的位置
通过三角测量计算深度，需要多次观测提高准确性

在稠密深度图估计中，没办法把全部像素都当作特征点计算描述子，那匹配就要用到极线搜索和块匹配技术。同时要深度滤波器技术。

12.2.2 极线搜索与块匹配

当知道一个点在第一幅图像中的位置，需要在第二幅图像的极线上寻找匹配点
为了提高精度，比较的不是单个像素，而是取像素周围的 $w\times w$ 小块区域,从像素灰度不变性变成了图像块灰度不变性
小块间的差异计算方法包括：
- SAD（绝对差之和）
- SSD（平方差之和）
- NCC（归一化互相关）

在匹配过程中，通常会得到沿极线的NCC分布，分布形状可能有多个峰值，需要通过概率分布描述深度值而非单一数值。这种方法被称为深度滤波器，随着对同一区域多次观测，深度估计会从不确定逐渐收敛到稳定值。

12.2.3 高斯分布的深度滤波器

对像素点深度的估计可以建模为状态估计问题，有滤波器和非线性优化两种求解思路。虽然非线性优化效果较好，但考虑到SLAM的实时性要求，通常采用计算量较少的滤波器方式。

完整的深度估计流程：P312

假设所有像素的深度满足某个初始的高斯分布
当新数据产生时，通过极线搜索和块匹配确定投影点位置
根据几何关系计算三角化后的深度及不确定性
将当前观测融合到上一次的估计中，如果收敛则停止计算，否则返回第2步

此处说的深度值是O₁P的长度，与针孔相机模型中提到的"深度"（指像素的z值）有所不同

像素梯度问题

块匹配的准确性依赖于图像块是否具有区分度
对于均匀区域（如一片黑或一片白），因缺乏有效信息，可能导致错误匹配
实例：打印机表面通常是均匀白色，很容易引起误匹配，导致深度估计不准确
具有明显梯度（纹理）的部分，深度信息更加准确，如桌面上的杂志、电话等
两种极端情况：
- 梯度平行于极线：可精确确定匹配位置，不确定性小
- 梯度垂直于极线：各处匹配度相同，无法确定精确位置，不确定性大

结论：立体视觉的重建质量高度依赖于环境纹理

逆深度

深度的正态分布存在问题：
- 实际场景深度通常在5~10米范围，分布不对称，尾部可能较长
- 某些应用中可能存在非常远或无穷远的点
逆深度(Inverse depth)是近年来SLAM研究中的广泛使用技巧
- 假设深度的倒数(1/d)为高斯分布更合理

在实际应用中表现出更好的数值稳定性

图像间的变换

在块匹配前，需要考虑相机运动对图像的影响
假设：图像块在小范围平移中保持不变，但在旋转时难以保持
解决方法：在匹配前计算两帧之间的变换关系
通过相机模型，建立参考帧与当前帧像素之间的映射关系
最终得到一个线性关系，可以计算出在局部范围内参考帧到当前帧图像坐标变换的仿射变换

根据仿射变换矩阵，我们可以将当前帧(或参考帧)的像素进行变换，再进行块匹配，以期获得对旋转更好的效果

效率问题：GPU并行加速，30万个线程，30万个点和1个点速度是一样的

12.4.3 八叉树地图

点云地图的缺点
- 点云地图通常规模很大，占用大量存储空间
- 一幅640×480像素的图像会产生30万个空间点
- 点云地图无法处理运动物体，因为只能添加点，无法移除点
八叉树地图的基本概念
- 八叉树是一种灵活、压缩、可随时更新的地图形式
- 基本思想：将三维空间递归地分为八等份小立方体
- 每个立方体的每个面平均切成两片，形成八个相同大小的小立方体
- 这个过程可以不断重复，直到达到所需的精度
- 可以将此过程理解为"从一个节点展开成八个子节点"

八叉树的空间效率
- 图12-11展示了八叉树的结构，左侧是空间划分，右侧是树结构
- 根节点代表整个大立方体，叶子节点代表最小的立方体
- 在八叉树中，每下降一层节点，体积就能扩大为原来的8倍
八叉树的占用表示方法
- 在八叉树中存储的是节点是否被占据的信息
- 传统方法用0表示空白，1表示被占据
- 更精确的方法是使用概率对数值来描述
- 引入logit变换：y = logit(x) = log(x/(1-x))
- 这种方法可以动态建模地图中的障碍物信息，并且能处理运动结构

通过使用概率对数表示，八叉树地图可以根据RGB-D数据更新，当观测到某个像素带有深度d时，意味着在对应的空间点上观察到一个点，并且从相机光心到这个点的线段上应该没有物体。

ok，写完了，完结！！！感谢Claude 3.7 Sonnet,感谢自己，晚上要参加笔试，ok加油。