FAST-LIVO2重点总结分析

FAST-LIVO2是一种直接法的LIVO框架，可以在实时重建地图的同时，实现快速、准确和鲁棒的状态估计。

FAST-LIVO2能够在严峻的LiDAR和/或视觉退化情况下，仍然保持高精度的定位。

速度的提升归因于在高效的ESIKF内使用原始的LiDAR、惯性和相机测量数据，并进行惯序更新。在图像更新中，采用了逆向求导公式和基于稀疏块的图像对齐，进一步提高了效率。
准确度的提升则得益于使用（甚至精优化）来自LiDAR点的平面先验，以提高图像对齐的精度。此外，使用了一个单一的voxel map来同时管理地图点和观测到的高分辨率图像测量。体素地图结构支持几何构建和更新、视觉地图点的生成和更新以及参考图像块的更新。
鲁棒性的提升则归因于实时估算曝光时间，能够有效应对环境光照变化，同时使用按需体素光线投射来应对LiDAR的近距离盲区。FAST-LIVO2的效率和精度在大量公开数据集上进行了评估，而每个系统模块的鲁棒性和有效性则在私有数据集上进行了测试。

1、顺序更新策略

顺序更新策略是在FAST-LIVO2系统中用于融合不同传感器数据（激光雷达、视觉和惯性测量单元IMU）的关键技术。由于激光雷达和视觉数据的维度和特性不同，直接同时融合这两种数据会带来计算和精度上的挑战，因此该系统采用了顺序更新策略来分别处理这两类数据，以提高系统的鲁棒性和精度。

顺序更新的关键在于将不同传感器的数据分步进行处理。首先更新激光雷达的测量数据，然后再更新视觉测量数据。这种做法允许系统先利用激光雷达的精确深度信息构建几何结构，再利用视觉数据进行进一步的精细对齐和优化。

1、IMU数据传播（状态预测）：

系统首先通过IMU（惯性测量单元）传播来预测状态，IMU提供高频率的加速度和角速度数据，用于估计机器人的位置、姿态和速度。

2、激光雷达数据更新：

在IMU预测得到的状态基础上，系统首先处理激光雷达的测量数据。激光雷达提供精确的深度信息，这些数据被用来更新系统状态。更新的方式是通过点到平面的配准方法（即将激光雷达点云与已知的地图或模型对齐），从而修正IMU传播的状态估计。
激光雷达更新采用点到平面的残差作为误差度量，通过最小化这些残差来优化机器人的位置和姿态。

3、视觉数据更新：

在激光雷达更新完成后，系统再使用视觉测量数据进行更新。视觉数据提供了丰富的颜色和纹理信息，但缺乏直接的深度感知。视觉更新通过直接法进行，即最小化图像的光度误差（即两个图像帧之间的像素强度差异）来进行对齐和优化。
- 直接法通过最小化两帧图像之间的光度误差来实现图像对齐和位姿估计。具体来说，系统通过比较参考帧（已知位姿）和当前帧中对应像素的亮度差异，优化当前帧的相机位姿，使得两帧图像在几何上对齐，从而减少光度误差。这一过程无需像传统的特征法那样提取角点或边缘，而是直接使用每个像素的强度值信息。
视觉更新使用光度误差作为度量，通过将视觉帧与激光雷达构建的几何模型对齐来更新姿态。

4、卡尔曼滤波器的顺序更新机制：

系统使用误差状态迭代卡尔曼滤波器（ESIKF），首先将激光雷达测量数据与IMU传播的状态进行融合（第一个贝叶斯更新），然后在融合后的状态上再进行视觉测量数据的更新（第二个贝叶斯更新）。每一次更新都是通过迭代的方式最小化残差，直到状态收敛。

2、误差状态迭代卡尔曼滤波器（ESIKF）工作原理

1、系统的状态表示

旋转矩阵 R是一个 3×3 的正交矩阵，用于描述物体在三维空间中的姿态。它能够将物体坐标系中的向量转换为全局坐标系中的向量，并能够表示物体绕特定轴的旋转角度。

2、IMU传播（状态预测）

3、激光雷达数据更新（第一个贝叶斯更新）

$\oplus $ 代表在误差状态下的增量操作，它结合了当前的状态估计 $\hat{x}$ 和修正量 $\delta_{x}$ ，使状态估计更加精确。不同的状态变量（如位置、速度、姿态）有不同的增量操作方法，尤其是在姿态估计中，增量操作涉及到非线性空间（如旋转矩阵或四元数）的运算，这使得系统能够更好地处理非线性状态的更新。

4、视觉数据更新（第二个贝叶斯更新）

总结视觉测量模型的关键步骤：

1、选择视觉地图点：从参考帧中选择具有良好光度特性的点作为视觉地图点，并将这些点存储在视觉子地图中。

2、投影到当前帧：通过位姿变换公式，将这些视觉地图点从参考帧投影到当前帧的图像中。

3、计算光度误差：在每个投影位置计算光度误差，即参考帧与当前帧对应像素的光度差异。

4、最小化光度误差：通过最小化光度误差来调整当前帧的位姿，直到残差最小，状态收敛。

5、迭代更新直到收敛

与ChatGPT对话内容：https://chatgpt.com/share/6717070a-54c8-800a-a037-51ba8ee6d1af

3、局部地图更新：

系统在估计位姿的同时，也在更新局部地图。局部地图基于体素结构，通过结合激光雷达的几何信息和视觉图像的纹理信息来进行更新和维护。

体素地图：激光雷达数据用来构建环境的几何结构，视觉数据为这些几何点添加纹理特征。
视觉地图点：在局部地图中选择合适的点，作为视觉地图点，用于后续的视觉图像对齐和位姿估计。

体素（Voxel）结构

体素（Voxel）结构是一种三维空间的网格划分方式，它将空间分割成一系列的小立方体，称为体素，每个体素是一个具有固定体积的空间单元。体素类似于二维图像中的像素，但它们用于表示三维空间中的体积信息。体素结构是一种常见的三维数据表示方法，尤其在地图构建、三维重建、计算机图形学和机器人导航中广泛应用。

1. 体素的基本概念

体素：体素（voxel）是”volume element”（体积元素）的缩写，它是三维空间中一个立方体单元。每个体素可以存储与该立方体对应的某些信息，例如几何信息（是否被占据、占据概率等）、颜色信息或其他属性。
体素网格：体素结构将整个三维空间划分为一个规则的网格，每个体素代表该网格中的一个小立方体单元。体素网格的分辨率决定了每个体素的大小和细致程度，分辨率越高，体素越小，地图越精细。

2. 体素结构的特性

离散化：体素结构通过将连续的三维空间离散化，使得复杂的空间几何能够用有限数量的立方体单元进行描述。这种离散化的表示方法适用于计算机处理，因为它将复杂的三维空间转化为可以存储和计算的规则网格。
三维表示：与二维像素不同，体素结构是三维的，因此可以用于表示三维物体、地图或场景。每个体素不仅有位置，还可以携带属性信息（如占据状态、颜色、纹理等）。
占据网格（Occupancy Grid）：在机器人导航和SLAM中，体素结构通常用于构建占据网格。占据网格表示的是环境中哪些区域被物体占据，哪些区域是空闲的。这对于路径规划、障碍物检测和导航非常重要。

3. 体素结构在SLAM中的应用

在SLAM（同时定位与地图构建）中，体素结构被广泛应用于构建三维地图，尤其是使用激光雷达和视觉数据的系统（如FAST-LIVO2）中。以下是体素结构在SLAM中的具体应用场景：

环境建模：
- 体素网格可以用来描述环境中的几何结构。激光雷达测量的点云数据通过体素化处理，将点云映射到体素网格中，每个体素根据是否被点云覆盖来确定其是否被占据。这样可以得到一个三维的占据网格，表示环境中每个位置是否有物体。
地图更新：
- 随着传感器（如激光雷达或相机）不断收集数据，系统可以动态地更新体素网格。新获取的点云数据会不断融合到现有的体素地图中，逐步完善整个环境的三维表示。
融合视觉和激光雷达数据：
- 在FAST-LIVO2中，体素结构不仅用来存储激光雷达的几何数据，还结合了视觉数据。视觉数据可以为体素赋予颜色或纹理信息，使得地图不仅包含几何形状，还包含丰富的外观信息，这对于系统的精确定位和导航至关重要。
xxxxxxxxxx graph LRA[开始] –> B{判断条件}B –>|条件成立| C[条件成立]B –>|条件不成立| D[条件不成立]C –> E[结束]D –> Emermaid#mermaidChart6{font-family:sans-serif;font-size:16px;fill:#333;}#mermaidChart6 .error-icon{fill:#552222;}#mermaidChart6 .error-text{fill:#552222;stroke:#552222;}#mermaidChart6 .edge-thickness-normal{stroke-width:2px;}#mermaidChart6 .edge-thickness-thick{stroke-width:3.5px;}#mermaidChart6 .edge-pattern-solid{stroke-dasharray:0;}#mermaidChart6 .edge-pattern-dashed{stroke-dasharray:3;}#mermaidChart6 .edge-pattern-dotted{stroke-dasharray:2;}#mermaidChart6 .marker{fill:#333333;stroke:#333333;}#mermaidChart6 .marker.cross{stroke:#333333;}#mermaidChart6 svg{font-family:sans-serif;font-size:16px;}#mermaidChart6 .label{font-family:sans-serif;color:#333;}#mermaidChart6 .cluster-label text{fill:#333;}#mermaidChart6 .cluster-label span,#mermaidChart6 p{color:#333;}#mermaidChart6 .label text,#mermaidChart6 span,#mermaidChart6 p{fill:#333;color:#333;}#mermaidChart6 .node rect,#mermaidChart6 .node circle,#mermaidChart6 .node ellipse,#mermaidChart6 .node polygon,#mermaidChart6 .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaidChart6 .flowchart-label text{text-anchor:middle;}#mermaidChart6 .node .label{text-align:center;}#mermaidChart6 .node.clickable{cursor:pointer;}#mermaidChart6 .arrowheadPath{fill:#333333;}#mermaidChart6 .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaidChart6 .flowchart-link{stroke:#333333;fill:none;}#mermaidChart6 .edgeLabel{background-color:#e8e8e8;text-align:center;}#mermaidChart6 .edgeLabel rect{opacity:0.5;background-color:#e8e8e8;fill:#e8e8e8;}#mermaidChart6 .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaidChart6 .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaidChart6 .cluster text{fill:#333;}#mermaidChart6 .cluster span,#mermaidChart6 p{color:#333;}#mermaidChart6 div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaidChart6 .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaidChart6 :root{–mermaid-alt-font-family:sans-serif;}条件成立条件不成立开始判断条件条件成立条件不成立结束
- 有时为了平衡地图的细节和计算效率，系统会采用多分辨率体素结构。不同区域可以使用不同大小的体素来表示，细节较少的区域使用大体素，而细节复杂的区域使用小体素。这种方法可以减少计算负担，同时保证关键区域的精度。

4. 体素结构的优缺点

优点：

简单而规则：体素结构非常直观，使用规则的立方体单元来划分三维空间，便于计算机存储和处理。
高效表示占据信息：在机器人导航和SLAM中，体素结构有效地表示环境的占据信息，方便路径规划和障碍物检测。
可结合多种传感器数据：体素结构可以轻松结合激光雷达和视觉数据，将几何信息和外观信息整合到同一地图中。

缺点：

高存储需求：由于三维空间的离散化，体素结构可能需要大量的存储空间，特别是在高分辨率的情况下。
细节限制：体素的分辨率有限，体素越大，地图的细节损失越多。为了表示更多细节，可能需要更小的体素，这会增加计算和存储成本。

5. FAST-LIVO2中的体素结构

在FAST-LIVO2中，体素结构用于构建和维护局部地图。激光雷达的数据通过体素结构进行存储，每个体素存储该区域是否被占据的几何信息。与此同时，视觉数据也通过纹理映射的方式与体素网格结合，提供地图的外观信息。这样的融合使得FAST-LIVO2系统能够构建出高精度的三维地图，并且在复杂的环境中保持较高的定位精度和效率。

5.1 自适应体素结构

自适应体素结构是一种基于分层和动态分辨率的空间划分方法。与传统的固定分辨率体素网格不同，自适应体素结构能够根据场景的复杂程度动态调整体素的大小，从而在平衡存储效率和地图细节上表现出色。

动态调整分辨率：自适应体素结构根据环境的复杂性进行自动调整。在细节丰富的区域（例如有很多障碍物或变化多端的区域），系统使用较小的体素来精细表示这些复杂的几何结构。而在细节较少的区域（如空旷区域或大平面区域），系统则使用较大的体素以减少计算和存储的负担。
优势：这种动态调整分辨率的方式能够让系统在保持精细地图的同时，大大减少存储和计算资源的消耗，适合于实时SLAM的应用。

与ChatGPT对话内容：https://chatgpt.com/share/6717070a-54c8-800a-a037-51ba8ee6d1af

4、地图结构

地图采用了自适应体素结构，该结构由哈希表和每个哈希条目的八叉树组成。

哈希表管理根体素，每个根体素的固定尺寸为0.5 × 0.5 × 0.5米。每个根体素封装了一个八叉树结构，用于进一步组织不同大小的叶体素。一个叶体素表示一个局部平面，存储一个平面特征（即平面中心、法向量和不确定性），以及位于该平面上的一组LiDAR原始点。这些点中一部分附有三层图像块（图像块尺寸为8 × 8），称之为视觉地图点。收敛的视觉地图点仅附有参考图像块，而未收敛的点附有参考图像块和其他可见图像块。叶体素的大小可变，使其能够适应不同结构的环境。

图4：局部地图滑动的二维演示。图(a)中，灰色矩形表示初始地图区域，长度为L。

检测区域变化：图(b)中，红色圆圈表示LiDAR的当前检测区域，当LiDAR移动到新位置p1时，检测区域触及地图边界。
地图区域移动：图(c)中，地图区域从初始位置移动到新位置（蓝色矩形），距离为d。内存中存储被移出区域的内容将被重置，以存储新移入的区域。

这种环形缓冲区方法确保局部地图在固定大小的内存内维护。

具体来说，哈希表用于快速定位体素区域，而八叉树则用于对体素内的空间进行进一步细分，以适应不同尺度的环境细节。

4.1 哈希表的作用

哈希表（Hash table）主要用于管理整个三维地图的根体素（Root Voxel），它负责快速查找三维空间中的任意点属于哪个根体素。每个根体素都对应一个固定大小的空间区域，例如 0.5米 × 0.5米 × 0.5米。

具体的工作过程如下：

位置映射：哈希表根据输入的三维坐标（如LiDAR点的位置）通过哈希函数将该坐标映射到具体的体素。例如，假设有一个LiDAR点在 (x,y,z)位置，哈希函数会根据该点的位置计算出其属于哪个根体素。
快速检索：哈希表的优势在于其可以提供常数时间复杂度的检索，也就是说，无论空间有多大，哈希表都能高效地定位该点所在的根体素。如果该根体素已经存在，哈希表会直接返回该体素的引用；如果不存在，则初始化一个新的根体素，并将其插入到哈希表中。

通过哈希表，系统可以在复杂的大范围三维场景中快速找到与新输入数据相关的空间区域，而不需要遍历所有的体素。

4.2 八叉树的作用

八叉树（Octree）是在每个根体素内部使用的空间划分结构。八叉树的主要作用是对体素中的空间进行更精细的划分，以适应不同尺度的细节。具体来说，八叉树允许系统根据需要在一个根体素内创建更小的子体素（Leaf Voxel），以便更精确地表示局部几何结构。

具体细化过程如下：

初始划分：当LiDAR点进入一个根体素后，如果该区域内的所有点不能很好地构成一个平面特征（例如，通过奇异值分解判断），系统会对该根体素进行八叉树的划分。
子体素的生成：一个根体素可以进一步分裂成8个更小的子体素，每个子体素又可以继续细分，直到满足预设的层级深度或该区域的点足够平面化为止。这样，可以动态调整地图的精细程度，使其既能处理大尺度的平坦区域，又能细化复杂的几何区域。
递归更新：当新的LiDAR点添加到体素中时，系统会检查该点是否符合现有体素的几何特征。如果不符合，则该体素会被进一步划分；如果符合，则该体素的几何信息（如平面法向量、中心点等）会被更新。

八叉树的优势在于其能够自适应地在复杂环境中精细划分体素，使得系统可以有效表示复杂的局部结构（如墙面、桌面等），而不必浪费资源在结构简单的区域。

4.3 哈希表与八叉树的结合

哈希表和八叉树的结合使得系统既能够快速定位大尺度的三维空间区域，又能够在局部区域实现高精度的几何描述：

哈希表：在全局范围内，哈希表快速映射三维空间中的坐标点到根体素，确保即使在大场景中，系统也能高效处理新的LiDAR数据。
八叉树：一旦定位到具体的根体素，八叉树负责进一步管理该区域内的空间划分。这种逐层细分的方式确保了体素的灵活性，能够处理不同复杂度的环境细节。

例如，在一个建筑场景中，大多数区域可能是平坦的墙面、地板或天花板，哈希表快速定位这些大区域，而八叉树进一步对这些平面特征进行精确建模。而对于复杂的区域，比如桌子、障碍物等，八叉树可以进行更细致的空间划分，生成更小的子体素来捕捉这些局部细节。

4.4 体素中的数据存储

在每个体素（无论是根体素还是叶体素）中，系统存储以下数据：

平面几何信息：如果体素内的点符合平面特征，则存储该平面的法向量$\text{n}$、中心点 $\text{q}$，以及对应的协方差矩阵$\Sigma_{n,q}$，用来描述平面的几何不确定性。
- 平面的协方差矩阵$\Sigma_{n,q}$ 用于描述平面几何特征的不确定性，具体来说，这个矩阵表征了平面法向量 n和平面中心点 q 的不确定性或误差范围。
- 协方差矩阵 $\Sigma_{n,q}$ 主要包含两个方面的信息：
  1. 平面法向量的不确定性：法向量 n 表示该平面在空间中的方向，协方差矩阵中的相关部分用于描述这个方向的估计误差。例如，当平面上的点分布较均匀时，法向量的估计会比较准确；反之，如果点分布在一个较狭窄的区域，法向量的估计不确定性就会较大。
  2. 平面中心点的不确定性：平面中心点 q 是指平面在三维空间中的位置。协方差矩阵还描述了在 q的估计中，由于LiDAR测量噪声、姿态估计误差等因素引起的空间位置的不确定性。这个部分的协方差反映了中心点的位置误差范围。
- 协方差矩阵的具体作用
- 表征误差分布：协方差矩阵通过量化法向量和中心点的不确定性，能够在算法中为平面几何特征赋予一定的置信度。例如，在计算新的LiDAR点是否属于某个平面时，协方差矩阵可以帮助判断点是否合理地位于该平面上，或者该平面的法向量是否发生了显著偏差。
- 加权更新：当新的LiDAR数据加入平面的更新时，协方差矩阵可以用于加权更新法向量和中心点的位置。具有较小协方差的平面表示更高的置信度，因此新的数据在更新时会受到现有平面特征的较大影响，反之亦然。
- 不确定性传播：在位姿估计或多传感器融合中，协方差矩阵能够传播不确定性信息，从而在融合过程中，考虑到测量误差对最终结果的影响。这对提高系统的鲁棒性和精度非常重要。
LiDAR点：每个体素还会存储附属于该平面的LiDAR点，尤其是最新的LiDAR点，用于后续的更新和参考图像生成。
视觉地图点：在某些体素中，系统还会生成并存储与图像相关的视觉地图点（visual map points），这些点附带有图像补丁信息，用于后续的图像对齐。

4.5 视觉地图点的生成与更新

在 FAST-LIVO2 系统中，视觉地图点的生成与更新是一个重要的过程，主要用于将LiDAR点与图像数据进行有效融合。通过生成和更新视觉地图点，系统可以在位姿估计时将视觉信息和LiDAR信息结合，提升SLAM（同步定位与建图）的精度和鲁棒性。

4.5.1候选LiDAR点的选择

首先，系统会根据当前LiDAR构建的地图来选择候选的LiDAR点，用于生成视觉地图点。这些候选LiDAR点是从已检测到的平面几何信息中选择的，它们必须满足以下条件：

可见性：候选LiDAR点必须位于当前相机视野内。
灰度梯度：候选LiDAR点投影到当前图像后，其对应的像素需要有显著的灰度梯度。这是为了确保图像的局部区域能够为视觉对齐提供足够的特征信息。

候选LiDAR点的选择过程如下：

将LiDAR点投影到相机图像平面上，得到该点在图像中的像素坐标。
系统对每个投影到图像上的LiDAR点进行检查，如果该点位于图像的有效视野内，且其对应的图像像素具有足够的灰度梯度，就将其作为一个候选视觉地图点。

4.5.2 视觉地图点的生成

当候选LiDAR点被选定后，系统会在图像上生成与其对应的视觉地图点。具体过程如下：

图像补丁生成：为每个选定的候选LiDAR点，系统会从对应的图像中提取一个小的图像补丁（patch），该补丁通常大小为11×11或8×8像素，称为“图像补丁金字塔（patch pyramid）”。这个补丁包含了该LiDAR点周围的图像信息，并在多层分辨率下存储，以便后续进行图像对齐。
附加信息：每个生成的视觉地图点不仅仅包含一个图像补丁，还会附加当前帧的相机位姿估计信息、该点所在平面的法向量 n 以及光度信息（如曝光时间等）。

生成的视觉地图点将会在后续的帧中用于图像对齐，以优化系统的位姿估计。

4.5.3 视觉地图点的更新

视觉地图点生成后，系统会根据新的帧数据不断更新这些地图点，以保证它们在系统运行的过程中保持有效和准确。更新过程包括以下几个方面：

新图像帧的投影：当系统接收到新的图像帧时，已经生成的视觉地图点会被投影到新的图像中。如果这些地图点在当前帧的投影位置发生了较大的偏移（例如超过一定数量的像素），或者当前帧的视角与生成该视觉地图点时的视角有较大差异，系统会为该视觉地图点生成一个新的图像补丁。
图像补丁的更新策略：系统会动态选择新的图像补丁来更新视觉地图点。补丁的更新不是每一帧都进行，而是当满足一定条件时才会进行。例如：
- 如果自上次更新以来已经经过了一定数量的帧（例如20帧以上），则该视觉地图点可能需要更新。
- 如果当前帧中该视觉地图点的投影位置与之前的投影位置相比偏移超过一定像素（例如超过40像素），系统也会更新该视觉地图点的图像补丁。
参考补丁的选择：为了提高后续图像对齐的精度，系统还会根据图像的光度信息（如曝光时间、光线变化等）动态选择最佳的图像补丁作为视觉地图点的参考补丁。参考补丁的选择主要基于两个准则：
- 光度相似性：通过计算图像补丁之间的光度差异（例如利用归一化互相关系数 NCC），选择光度差异最小的补丁。
- 视角相似性：通过计算参考帧和当前帧的视角差异，选择与当前帧视角最接近的图像补丁作为参考。

通过这些更新策略，系统能够在环境光线变化、相机姿态变化的情况下，保持视觉地图点的有效性和准确性。

4.5.4 视觉地图点的使用

视觉地图点生成并更新后，会被用于后续的图像对齐和位姿优化过程。具体步骤如下：

图像对齐：系统使用视觉地图点的图像补丁与当前帧进行图像对齐，通过最小化光度误差（photometric error）来优化当前帧的相机位姿估计。
融合LiDAR和视觉信息：视觉地图点的存在允许系统将LiDAR点与视觉信息进行深度融合。在图像对齐过程中，视觉地图点不仅能提供图像信息，还能借助LiDAR的深度信息来提高对齐的精度。这种融合能够在视觉信息缺乏或受到噪声干扰时提供更高的鲁棒性。