VSLAM Summary

1. 相机运动参数是描述相机在世界坐标系中的运动状态的向量
2. 每个相机运动参数 $C_i$ 包含了相机的位置和朝向信息，或者包含相机的平移和旋转的信息
3. 通常表达为一个 3×3 的旋转矩阵(或四元数、欧拉角等) $R_i$ 和一个三维位置变量 $p_i$ (或位移向量)。
4. 通过恢复相机运动参数，可以了解相机在场景中的位置和姿态变化，以将不同帧的图像对齐并构建出场景的三维结构

1. 场景的三维结构表示相机看到的场景中每个物体点的三维位置
2. 三维结构向量通常由每个特征点的三维坐标组成，每个特征点代表场景中的一个 关键点 ，它们可以是角点、边缘、纹理等
3. 通过恢复特征点的三维坐标，可以重建场景三维结构

$R_i$ 和 $p_i$ 将世界坐标系下的三维点 $X_j$ 变换到 $C_i$ 的局部坐标系： $$ (\boldsymbol{X}_{ij},\boldsymbol{Y}_{ij},\boldsymbol{Z}_{ij})^T=\boldsymbol{R}_i(\boldsymbol{X}_j-\boldsymbol{p}_i) $$

进而投影到图像中： $$ \boldsymbol{h}_{ij}=(\frac{f_x\boldsymbol{X}_{ij}}{\boldsymbol{Z}_{ij}}+c_x,\frac{f_y\boldsymbol{Y}_{ij}}{\boldsymbol{Z}_{ij}}+c_y)^T $$

其中，$f_x$ 和 $f_y$ 分别为沿图像 $x,y$ 轴的图像焦距，$(c_x,c_y)$ 为镜头光心在图像中的位置，这些参数应当已事先标定且保持不变。

三维点在图像中的投影位置 $\boldsymbol{h}_{ij}$ 可表示为如下函数： $$ \boldsymbol{h}_{ij}=h(C_i,\boldsymbol{X}_j) $$

V-SLAM 需要将不同图像中对应于相同场景点的图像点匹配起来，求解优化函数： $$ { \underset{C_1...C_m,\boldsymbol{X}_1...\boldsymbol{X}_n}{\arg\min}} \sum_{i=1}^{m} \sum_{j=1}^{n} \lVert h(C_i,\boldsymbol{X}_j)-\boldsymbol{\hat{x}}_{ij} \rVert $$

得到一组最优的 $C_1,...,C_m,\boldsymbol{X}_1,...,\boldsymbol{X}_n$ 使得所有的 $\boldsymbol{X}_j$ 在$C_i$ 图像中的投影位置 $h_{ij}$ 与观测到的图像点位置 $X_{ij}$ 尽可能靠近。

以上过程假设图像观测点符合高斯分布

\begin{align} x_{ij} \sim N(\boldsymbol{\hat{x}}_{ij}, \Sigma_{ij}) \notag \\ \lVert e \rVert_{\Sigma} = e^T \Sigma^{-1} e \notag \end{align}

求解优化函数的过程称为集束调整(bundle adjustment,BA)，一般用线性方程的稀疏结构求解

常用的是在 VSLAM 中结合 IMU 数据，形成 VIN(Visual-aided inertial navigation)或 VI-SLAM(Visual-inertial SLAM)

将相邻 2 帧 $(C_i,C_{i+1})$ 间所有 IMU 数据标记为集合 $Z_i={z_1...z_{n_{i}}}$ ，VI-SLAM 求解优化如下目标函数： $$ { \underset{C_1...C_m,\boldsymbol{X}_1...\boldsymbol{X}_n }{\arg\min}} \sum_{i=1}^{m} \sum_{j=1}^{n} \lVert h(C_i,\boldsymbol{X}_j)-\hat{x}_{ij} \rVert_{\Sigma_{ij}} + \sum_{i=1}^{m-1} \lVert f(C_i,Z_i)-C_{i+1} \rVert_{\Gamma_{i}} $$

VI-SLAM 引入了一个运动方程，其中 $f(C_i,Z_i)$ 为 $Z_i$ 作用于 $C_i$ 后的运动参数，$\Gamma_i$ 是运动方程的协方差矩阵。常见的运动方程有：

1. 连续时间系统
2. 预积分方程

通常，VI-SLAM 需要求解每一时刻的运动速度 $\boldsymbol{v_i}$ 和 IMU 数据的偏移量 $\boldsymbol{b_i}$ $$ C_i=(\boldsymbol{R}_i,\boldsymbol{p}_i,\boldsymbol{v}_i,\boldsymbol{b}_i) $$

若再引入一项 GPS 数据 $\boldsymbol{p}_i^{G}$ $$ { \underset{C_1...C_m,\boldsymbol{X}_1...\boldsymbol{X}_n }{\arg\min}} \sum_{i=1}^{m} \sum_{j=1}^{n} \lVert h(C_i,\boldsymbol{X}_j)-\hat{x}_{ij} \rVert_{\Sigma_{ij}} + \sum_{i=1}^{m-1} \lVert f(C_i,Z_i)-C_{i+1} \rVert_{\Gamma_{i}} + \sum_{i=1}^{m-1} \lVert \boldsymbol{p}_i - \boldsymbol{\hat{p}}_i^{G} \rVert_{\Lambda_i} $$

MonoSLAM 的状态 $\boldsymbol{x}_t=[C_1,...,C_m,\boldsymbol{X}_1,...,\boldsymbol{X}_n]$

其每一时刻相机方位均带有一个概率偏差，每个三维点位也带有一个概率偏差，可用三维椭球表示，椭球中心为估计值，椭球体积表示不确定程度，场景点投影至图像的形状为一个投影概率椭圆。

MonoSLAM 选用的是扩展卡尔曼滤波 EKF，预测阶段采用运动方程： $$ C_{t}=f(C_{t-1},\boldsymbol{a}_{\nu}\Delta t,\boldsymbol{a}_{\omega}\Delta t) $$

其中，$\boldsymbol{a}_\nu$ 和 $\boldsymbol{a}_\omega$ 分别为线性和旋转加速度，并假设它们符合高斯分布 $\boldsymbol{a}_\nu \sim N(0,\Gamma_\nu)$ ，$\boldsymbol{a}_\omega \sim N(0,\Gamma_\omega)$ 在更新阶段采用投影方程： $$ \hat{x}_{j}=h(C_{t},\boldsymbol{X}_{j})+\boldsymbol{n}_{j} $$

其中，$\boldsymbol{\hat{x}}_j$ 为当前帧观测到 $\boldsymbol{X}_j$ 的图像点位置，$\boldsymbol{n}_j \sim N(0,\Sigma_j)$

MonoSLAM 每一时刻仅需估计当前时刻的状态 $x_t$ ，之前所有时刻的相机运动参数 $C_1,...,C_{t-1}$ 全部不参与计算，由此简化计算量

EKF 局限性在于：

1. 预测函数和更新函数在非线性条件下不能保证全局最优，与迭代的非线性优化技术相比更容易造成误差累积
2. 若将三维点引入状态变量，每一时刻的计算复杂度为 $O(n^3)$ ，处理代价过大

MSCKF 用于缓解 EKF 方法的计算复杂度问题，它是 VI-SLAM 方法

1. 在预测阶段，使用 IMU 数据传递系统状态
2. 在更新阶段，MSCKF 将邻近的 $l$ 帧相机运动参数包含进状态变量集合 $C=\{C_{t-l+1},...,C_t\}$ 其中 C 集合内的每个 $C_i$ 的估计值均在不断优化，在被移出 $C$ 前已经较为精确，以此缓解误差累积
3. MSCKF 对所有三维点进行消元(Marginalization)，将 $C_i$ 与 $\boldsymbol{X}_j$ 之间的二元约束转换为 $\{C_{t-l+1}...C_t\}$ 间的多元约束，从而将 $O(n^3)$ 的计算复杂度简化为 $O(nl^3)$

通过以上优化，将计算复杂度从立方关系降到了线性关系，大幅降低了计算代价

FAST角点是一种在计算机视觉中常用的角点检测算法，它可以快速而准确地识别出图像中的角点。FAST角点(Features from Accelerated Segment Test加速段测试特征)设计目标是在保持较高的检测速度的同时，尽可能减少误检和漏检。

FAST角点的原理是通过对图像像素进行快速的二进制测试，判断某个像素是否是角点：

1. FAST算法选取一个中心像素，然后选择周围的16个像素点进行测试
2. 如果中心像素与其中连续的n个像素（通常为9个）中有k个像素的亮度大于或小于中心像素的亮度阈值，那么中心像素就被认为是一个候选角点
3. FAST算法会对候选角点进行进一步的验证和优化，以提高角点检测的准确性。

FAST角点算法的优点：

1. 快速性：FAST算法通过设计高效的像素测试和优化策略，能够以非常快速的速度在图像中检测出角点。
2. 鲁棒性：FAST算法对噪声和图像变换具有一定的鲁棒性，能够在一定程度上抵抗图像的干扰和变形。
3. 低计算复杂度：FAST算法的计算复杂度相对较低，适用于实时应用和资源受限的环境。

FAST角点算法存在的问题：

1. 对角点尺寸不敏感：FAST算法在设计时忽略了角点的尺寸信息，因此对于尺寸较小或较大的角点检测效果可能不理想。
2. 对噪声敏感：FAST算法对噪声比较敏感，当图像中存在较多噪声时，可能产生误检或漏检的情况。
3. 不适用于非平面场景：由于FAST算法是基于图像亮度变化的测试，在非平面场景下，可能会导致角点检测不准确。

Harris角点检测算法用于检测图像中的角点。它通过分析像素的亮度变化来确定图像中可能存在的角点位置。

Harris角点检测算法基于以下观察：

1. 在角点附近，图像的任何方向上的移动都会引起亮度的明显变化;
2. 在平坦区域或边缘上，只有沿着边缘方向的移动才会引起明显的亮度变化

基于这一观察，Harris角点检测算法通过计算图像中每个像素的角点响应函数，并根据响应函数的大小确定角点的位置。

Harris角点检测算法的计算过程如下：

1. 计算图像中每个像素的梯度值，通常使用Sobel算子或Prewitt算子等。
2. 根据梯度值计算每个像素的自相关矩阵（M矩阵），该矩阵描述了像素周围局部区域的亮度变化。
3. 计算M矩阵的特征值，特征值的大小反映了该像素处的亮度变化情况。
4. 根据特征值的大小确定角点的位置，通常选择特征值较大的像素作为角点。

Harris角点检测具有以下优点：

1. 旋转不变性：Harris角点检测算法可以通过计算像素的梯度和自相关矩阵来确定角点的位置和方向，从而实现一定程度上的旋转不变性。
2. 精确性：Harris角点检测算法可以准确地检测出图像中的角点，并且对噪声的影响相对较小。
3. 相对简单：Harris角点检测算法的实现相对简单，计算复杂度较低，适用于实时应用和资源受限的环境。

Harris角点检测存在以下问题：

1. 尺度不变性：Harris角点检测算法在计算时没有考虑尺度信息，对于尺度变化较大的图像或物体，可能无法准确地检测到相应的角点。
2. 对边缘敏感：Harris角点检测算法在边缘区域可能会产生误检，将边缘也识别为角点。
3. 参数选择：Harris角点检测算法中有一个参数k，用于调整角点响应函数的阈值，不同的图像和场景可能需要不同的参数选择。

BRIEF（Binary Robust Independent Elementary Features）描述符用于对图像或图像中的关键点进行描述和匹配。它通过比较两个像素之间的亮度差异来生成二进制字符串，并将其作为描述符表示关键点。

BRIEF描述符的生成过程如下：

1. 随机选择一组像素对，每个像素对(x,y)定义了一个比较器：如果x的亮度值大于y，则输出1，否则输出0。
2. 通过比较器对关键点周围的像素进行比较，得到一个由二进制数构成的向量，向量的长度等于所选像素对的数量。
3. 通常情况下，BRIEF描述符使用256位二进制字符串表示，即使用256对像素进行比较，生成256位的二进制向量作为描述符。

BRIEF描述符具有以下优点：

1. 快速：BRIEF描述符使用简单的亮度比较操作生成二进制字符串，计算复杂度较低，适用于实时应用和资源受限的环境。
2. 稳定性：BRIEF描述符对图像旋转、缩放和光照变化等具有一定的不变性，可以在不同的环境中进行特征匹配。
3. 低维度：BRIEF描述符使用256位二进制字符串表示，相比于其他描述符来说，它的维度较低，存储和处理都比较方便。

BRIEF描述符存在以下问题：

1. 不可扩展性：由于BRIEF描述符是通过随机选择像素对来生成的，因此无法在后续的应用中增加或减少描述符的长度。
2. 对旋转变化敏感：由于BRIEF描述符没有考虑图像中关键点的旋转信息，因此对于旋转变化较大的图像或物体，可能无法准确地进行匹配。
3. 视点变化敏感：由于BRIEF描述符只考虑了像素之间的亮度变化，没有考虑三维几何信息，因此对于视点变化较大的场景，BRIEF描述符可能无法准确匹配。

ORB（Oriented FAST and Rotated BRIEF）特征结合了FAST角点检测和BRIEF描述符技术，同时加入了旋转不变性和尺度不变性的考虑。

ORB特征的计算过程主要分为三个步骤：角点检测、方向分配和描述符生成。

1. 采用FAST角点检测算法来检测图像中的关键点，FAST算法可以快速而准确地识别图像中的角点。
2. 使用Harris角点检测算法来计算关键点的方向，以保证ORB特征具有旋转不变性。
3. 使用BRIEF描述符提取算法来计算每个关键点的描述符，其中BRIEF算法是一种高效的二进制特征描述算法，可以在短时间内生成高质量的关键点描述符。

ORB特征具有以下几个优点：

1. 快速性：ORB算法通过结合FAST角点检测和BRIEF描述符技术来实现快速的图像特征提取，可以在实时应用和资源受限的环境中使用。
2. 鲁棒性：ORB算法在角点检测时使用了Harris角点检测算法来计算关键点的方向，使得ORB特征可以在一定程度上抵抗图像旋转和变形等干扰。
3. 描述性：ORB算法通过使用BRIEF描述符技术来生成关键点的特征描述符，可以准确地描述图像中的关键信息，具有良好的区分性能。

ORB特征存在以下问题：

1. 尺度不变性：ORB算法在计算时没有考虑尺度信息，因此对于尺度变化较大的图像或物体，可能无法准确地提取出相应的ORB特征。
2. 难以处理遮挡：ORB算法没有考虑图像的遮挡和背景干扰等问题，在这种情况下可能会产生错误的ORB特征。
3. 计算复杂度较高：ORB算法在计算描述符时需要进行比较多的计算，因此可能会占用较高的计算资源。

PTAM 是实时 SFM 系统，也是首个基于关键帧 BA 的单目 VSLAM 系统。

1. 特征：选取 FAST 角点作为特征点

2. 运行：在 2 个线程中并行执行 2 个独立的任务

   1. 相机跟踪(Tracking)
   2. 地图构建(Mapping)

3. 集束调整：

   1. 在地图构建线程中稀疏抽取关键帧及关键帧中可见的三维点进行 BA
   2. 根据 BA 恢复的精确三维结构，相机跟踪线程作为前台线程，仅需优化当前帧运动参数 $C_t$ ，以达到实时的计算效率

前台线程通过一个匀速运动模型预测当前帧方位，以搜索地图中的三维点在当前帧图像中对应的*FAST 角点* ，并根据匹配关系优化当前帧方位 $$ \arg\min\limits_{C_t}\sum\limits_{j=1}^nw_j\left\|h(C_t,X_j)-\hat{x}_j\right\|_{\Sigma_j} $$

其中，$w_j$ 是 Tukey 函数对应的权重,用于缓解误差匹配(Outliers)对结果的影响。

将当前帧与已有关键帧的缩略图进行比较，选择最相似的关键帧作为当前帧方位的预测，重复特征匹配和方位优化步骤，如果跟踪成功，判断 $C_t$ 是否满足关键帧条件，一旦符合，将当前帧作为新关键帧传递给后台构建地图。

如果成功匹配点数不足（如图像模糊、快速运动），则判断跟踪失败，开始重定位。

后台线程沿极线(Epipolar Line) 匹配不同关键帧之间对应于相同场景点的图像特征点，通过三角化(Triangulation) 恢复场景点的三维位置，并对所有关键帧和三维点运行 BA，恢复精确的三维地图。

以下是一些相关内容的说明和注释：

1. 匀速运动模型表示 PTAM 假设相邻帧之间的相机运动是匀速运动，在相邻帧间相机的位移是线性变换

2. Tukey 是一种常见的鲁棒损失函数

   \begin{equation} L(r)= \begin{cases} \frac{1}{2}r^2, & \text{if } |r| \leq c \\ c \left(|r| - \frac{1}{2}c\right), & \text{otherwise} \end{cases} \notag \end{equation}

   其中，$r$ 表示残差，$c$为一个阈值，称为 Tukey's biweight 常数，在残差小于阈值时损失函数为平方误差（二次损失），超过阈值时损失函数线性增加。

3. 角点是图像中具有明显变化或边缘交汇的位置，它具有以下特征：

   1. 强度变化，角点周围的像素强度在不同方向上有显著变化
   2. 边缘交汇，角点是多个边缘相交的位置
   3. 不变性，角点在旋转、平移、尺度变化下具有一定程度的不变性

4. 用于检测角点的算法如下：

   1. FAST 用于快速检测具有显著变化的角点，用此法检测称为 FAST 角点
   2. Harris
   3. Shi-Tomasi

ORB-SLAM 对 PTAM 的组件作了改进：

1. 选用 ORB 特征

   1. 具有更好的视角不变性
   2. 新增三维点的特征匹配效率更高

2. 加入回环检测模块

   1. 检测回路

      1. 采用与重定位相同的方法
      2. 匹配回路两侧关键帧上的公共点

   2. 通过方位图优化来闭合回路

      1. 关键帧作为方位图的节点
      2. 每个关键帧赋予一个相似变换 $\xi_i$ 以矫正方位图的方位
      3. 方位图的边表示关键帧之间存在特征匹配
      4. 2 团匹配的三维点云通过坐标对齐可求解一个相似变换 $\xi_{ij}$

      优化过程如下所示：

      \begin{equation} {\underset{\boldsymbol{\xi}_1,\cdots,\boldsymbol{\xi}_m}{\arg\min}} (\boldsymbol{\xi}_{ij},\boldsymbol{\xi}_i^{-1}\circ\boldsymbol{\xi}_j)^{\mathrm{T}}\boldsymbol{\Sigma}_{ij}^{-1}(\boldsymbol{\xi}_{ij}\circ\boldsymbol{\xi}_i^{-1}\circ\boldsymbol{\xi}_j) \notag \end{equation}

      其中，$\sum_{ij}$ 为 $\xi_{ij}$ 的协方差矩阵(文中设为单位阵)，操作符 $\circ$ 按顺序连接两个相似变换。

   3. 采用 g2o 优化方位图，以闭合回路

   与全局 BA 相比，方位图优化极大简化了全局优化的计算量

3. 自动初始化 对于初始化来说，需要选取 2 帧，帧间需要有足够的公共点(相似性)，又需要有足够的平移量(特异性)。通过为公共点提供视差，才能三角化精确的三维位置

   1. PATM 需要手动初始化 用户指定 2 帧用于初始化系统，
   2. ORB-SLAM 自动初始化 通过检测视差自动选择初始化的 2 帧

4. 扩展性强

   1. PTAM 要求新加入的关键帧提供足够的视差

   2. ORB-SLAM 采用更鲁棒的关键帧和三维点的选择机制

      1. 先用宽松的条件判断尽可能及时地加入新的关键帧、三维点(鲁棒跟踪)
      2. 再用严格的条件判断删除冗余的关键帧、不稳定的三维点(提高 BA 过程的精度和效率)

DTAM 最显著的特点是实时恢复场景三维模型。

DTAM 预测一个与当前帧相机方位 $C_t$ 接近的虚拟相机 $C_v$ ,并在 $C_v$ 下绘制场景三维模型，以求解 $C_v$ 和 $C_t$ 之间的相对运动 $\xi_{tv}$

\begin{equation} {\underset{\xi_{tv}}{\arg\min}} \sum_{\chi \in \Omega_{v}} \lVert r(\chi , D_v(\chi),\xi_{tv}) \rVert_2^2 \notag \end{equation}

其中，$r(\cdot)$ 是颜色残差,

\begin{equation} r(\chi,D_v(\chi),\xi_{tv})=I_v(\chi)-I_t(\omega(\chi ,D_v(\chi), \xi_{tv})) \end{equation}

$I_v$ 和 $D_v$ 分别是三维模型在 $C_v$ 下绘制得到的亮度和深度图，$\omega_v$ 为亮度和深度有效像素的集合，函数 $\omega(\chi ,D_v(\chi),\xi_{tv})$ 将虚拟帧 $v$ 中的像素 $\chi$ 投影到当前帧 $t$ 中。

LSD-SLAM 仅恢复半稠密的深度图，每个像素深度独立计算，提高了计算效率。

LSD-SLAM 每个关键帧包含图像 $I_k$ 、逆深度图 $D_k$ 和逆深度的方差 $V_k$ ，系统假设每个像素 $\chi$ 的逆深度值服从高斯分布 $N(D_k(\chi),V_k(\chi))$ ,LSD-SLAM 的前台线程采用直接跟踪法恢复当前帧 $t$ 与关键帧 $k$ 之间相对运动 $\xi_{tk}$ ,即求解优化式

\begin{equation} {\underset{\xi}{\arg\min}} \sum_{\xi \in \Omega_k} \lVert \frac{r^2(\chi ,D_k(\chi), \xi_{tk})}{\sigma_r^2(\chi ,D_k(\chi),\xi_{tk})} \rVert_\delta \end{equation}

其中，$\Omega_k$ 是深度有效像素的集合；$r(\cdot)$ 的求解与上一节相同，$\sigma^2(\chi , \xi)$ 为 $r(\cdot)$ 的方差，用于减小深度误差对结果的影响。

\begin{equation} \sigma_r^2(\chi ,D_k(\chi), \xi)=2\sigma_I^2+\left ( \frac{\partial r(\chi,D_k(\chi),\xi_{tk}) }{\partial D_k(\chi )} \right )^2 V_k(\chi ) \end{equation}