学习Fast-LIO系列代码中相关概念理解

一、流形和流形空间（姿态）

1.1 定义

1.2 为什么要有流形?

1.3 流形要满足什么性质？

(1) 拓扑同胚

(2) 可微结构

1.4 欧式空间和流形空间的区别和联系?

(1) 区别：

(2) 联系：

1.5 将姿态定义在流形上比定义在欧式空间上有什么好处？

1.6 IMU的状态

二、SO(3)的含义

三、相关名词

3.1 Forward Propagation

3.2 Backword Propagation

3.3 Jacobin matrix

3.4 Prior distribution

3.5 Posteriori distribution

四、KNN & KD-Tree

4.1 前言

4.2 KNN

（1）K值选取

（2）距离计算

（3）算法流程

4.4 KD-Tree

(1) KD-Tree 构建

=== 简易构造过程

1）第一次划分

2）第二次划分

3）第三次划分

===构造依据

4.5 KD-Tree 搜索

(1) 初始化路径

(2) 回溯路径 *

一、流形和流形空间（姿态）

1.1 定义

流形（Manifold）是一种广义的曲面概念，用于描述局部上类似于欧几里德空间的空间。简而言之，流形是一个局部与欧几里德空间同胚（homeomorphic）的空间，但并不一定是全局上同胚的。（局部同胚欧式空间是为了方便处理这种广义的曲面）
流形空间是指一个由流形构成的空间，其中每个点都对应于流形的一个实例。

1.2 为什么要有流形?

非欧式空间难以处理，特别是涉及到曲率和奇异性等概念时，往往难以直接处理。流形的定义允许我们在局部上将复杂的几何问题转化为类似欧式空间的问题。这个局部的类似欧式空间性质为我们提供了一种简化问题的方法，使得我们可以在流形上运用欧式空间中的数学工具，从而更有效地处理问题。

1.3 流形要满足什么性质？

(1) 拓扑同胚

对于流形中的每个点P，都存在一个包含P的开集U，使得U与欧式空间中的开集V存在拓扑同胚。拓扑同胚意味着存在一个双射函数（即一一映射），它将U中的点映射到V中的点，同时保持了它们之间的拓扑关系。这个性质保证了流形的局部结构与欧式空间的局部结构是相似的。

(2) 可微结构

在流形的每个点P，都存在一个坐标图（coordinate chart），它是一个映射函数，将P附近的点映射到欧式空间中的点。这个坐标图应该是可微的，意味着它在流形上的每一点处都具有连续且可导的性质。换句话说，流形上的点应该能够用欧式空间中的坐标来表示，并且这个坐标表示应该具有光滑性。

1.4 欧式空间和流形空间的区别和联系?

(1) 区别：

基本结构：欧式空间是我们熟悉的传统三维空间，其中的点由三个实数（x、y、z）表示，具有直角坐标系。在欧式空间中，可以进行常规的线性运算和加法操作。而流形空间是一种更一般的概念，它在局部上与欧式空间同胚，但在全局范围内可能不是直角坐标系。
维度：欧式空间的维度是固定的，例如三维欧式空间就有三个坐标轴（x、y、z）。而流形空间的维度可以是任意的，取决于流形的定义。例如，SO(3)流形是三维的，而SO(2)流形是二维的。
结构：欧式空间是平直的，它遵循欧几里德几何学的性质。而流形空间通常是曲面的或具有一定的曲率，它遵循非欧几里德几何学的性质。流形空间在局部上与欧式空间类似，但在全局范围内可能有非平直的结构。

(2) 联系：

局部同胚：流形空间在局部上与欧式空间是同胚的，意味着在流形的每一点附近都存在一个局部欧式坐标系，可以将局部的流形映射到欧式空间中。这使得在流形空间上的数学运算和分析可以通过局部欧式空间进行处理。
数学工具：欧式空间中的许多数学工具和方法也可以扩展到流形空间中，尽管可能需要适应流形空间的特殊性质。例如，微积分、线性代数和向量空间等概念在流形空间中也有相应的推广。
总体而言，流形空间和欧式空间是两种不同的数学空间，它们在结构和性质上有所不同，但在一些局部性质和数学工具上存在联系。流形空间的一般性使其成为处理复杂几何问题和高维数据分析的有力工具.

流形多种多样，以下以SO(3)流形为例：
在姿态中，旋转矩阵的李群就是一个SO(3)流形，大概的样子想象为一个三维的球体或球壳。每个球面上的点都对应着一个旋转矩阵，而球体的表面则包含了所有可能的旋转姿态。任意的两点之间都相差一个旋转矩阵。这和欧式空间中定义就完全不一样了。但是SO（3）流形有局部同胚欧式空间，也就是李代数！！！**李代数就是SO(3)流形在原点处同胚的欧式空间！**所以李群上的一些复杂操作可以转到同胚的欧式空间中，也就是李代数中！如果不在原点附近的同胚欧式空间，一般来说不再是李代数的空间。李代数仅仅是单位元处的同胚欧式空间。所以在李群中的操作，都可以使用李群欧式空间中来操作。

1.5 将姿态定义在流形上比定义在欧式空间上有什么好处？

连续性：姿态定义在流形空间中时，旋转操作的组合和插值都保持了流形的连续性。这意味着在流形空间上进行旋转操作时，不会出现突变或不连续性，从一个姿态平滑地过渡到另一个姿态。
不会出现奇异性：在流形空间上定义姿态可以避免一些奇异性问题。在欧式空间中，例如使用欧拉角时，存在万向锁问题，导致某些方向上的旋转变得不稳定。而在流形空间上，使用四元数或旋转矩阵等表示方式，可以避免这些奇异性问题，从而提高了姿态的稳定性。（欧式空间中姿态表示使用欧拉角）
避免过度参数化：姿态定义在流形空间上通常采用最小的参数化方式，例如四元数、旋转矩阵等。相比之下，在欧式空间中使用欧拉角时，可能会存在多种表示方式表示相同的旋转，导致过度参数化，增加了问题的复杂性。
保持结构特性：在流形空间上定义姿态，比如三维旋转群（SO(3)），可以保持旋转矩阵的正交性和行列式等于1的特性。这保证了旋转操作仍然是合法的旋转。

1.6 IMU的状态

IMU中的速度、位置等是定义在欧式空间中的，姿态通常是与其他状态（如速度、位置）一起进行融合。在融合过程中，需要将不同类型的状态（流形空间和欧式空间）统一起来，可能需要使用特定的算法和转换来进行集成。确保在状态融合过程中考虑到流形空间的性质，以保持状态更新的连续性和稳定性是非常重要的。此外，还需要注意数值计算的稳定性和数值误差，以避免在处理复杂状态时产生不良的结果。

二、SO(3)的含义

定义：SO(3)={R| R^转置 R= I,det (R)=±1}；SO(3)是包含旋转矩阵R的一种特殊正交群，我们称之为三维旋转群。

三、相关名词

3.1 Forward Propagation

前向传播：将上一层的输出作为下一层的输入，并计算下一层的输出，一直到运算到输出层为止。

温故知新——前向传播算法和反向传播算法（BP算法）及其推导 - 知乎

3.2 Backword Propagation

反向传播：将激励响应同对应的目标输出求差，获得隐层与输出层的响应误差。

Back Propagation（梯度反向传播）实例讲解 - 知乎
反向传播(Back propagation)算法笔记 - 知乎

3.3 Jacobin matrix

雅克比矩阵：对雅可比矩阵的理解 - 知乎

3.4 Prior distribution

先验分布： https://www.cnblogs.com/tspeaking/p/10856181.html

3.5 Posteriori distribution

后验分布：贝叶斯统计--先验分布与后验分布_东皇太乙的博客-CSDN博客_先验分布

四、KNN & KD-Tree

4.1 前言

FAST-LIO2论文主要内容在于ikd-Tree的介绍，状态估计则与FAST-LIO中的内容差不多。论文中的ikd-Tree是基于kd-tree的，而kd-tree是一种数据结构，能用于储存一系列的点以便对其进行搜索。
百度百科：kd-tree（k-dimensional树的简称），是一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。
一些比较有用的学习视频

通俗易懂：学生视频-KD树 by 一只叫小花的猫
有代码实现举例：[JHML-KNN-02]knn算法系列 by 庸俗武士
考虑更全面：【帅器学习/林木】K最近邻算法（KNN）by 机器学习

4.2 KNN

Kd-tree 是在KNN的基础上优化得到，故有必要先了解一下KNN是个什么东西。
KNN（K-Nearest Neighbor），简称K近邻算法，是最简单的机器学习算法之一。
算法思想：对于一个未分类的样本，可选取其附近最近的K个已分类样本，并认为该样本属于这K个样本中的分类占比最大的类别。
无需严格按照距离远近选取K个样本，也可以引入权重。具体内容看前面推荐的第三个视频，讲解得比较详细。

（1）K值选取

由上面的例子不难发现，K值的选取会直接影响到绿色圆圈的归类类别。一般而言，K值选取需要遵循：

一般从较小值开始
奇数：如果选取偶数，很容易出现“平局”，从而难以进行归类，就比如在上面的例子中，K取4 or 10。
最大不宜超过20 ：K值越大，需要计算和比较的样本数量越大，计算量也随之疯狂增长。

（2）距离计算

前面的例子中，我们是直接通过目测和的中心与中心的距离大小，来判断最近的K个点的。而实际应用时，我们是需要计算出具体的距离，然后进行远近比较的。常用的有两个距离：欧式距离和曼哈顿距离。
在之前学习的路径规划算法中，也用到了这两个距离。
Poao: 常见路径规划算法实现-Matlab
“欧式”、“曼哈顿”看着挺唬人的，但只要下面这一张图就能简单说明他们是个啥了。下图中，求解了(1, 4)与(4, 2)两点之间的距离。

欧式距离也称欧几里得距离，即(欧几里得)空间中两点间的直线距离。
曼哈顿距离，即两点在标准坐标系上的绝对轴距总和。
还有其他一大堆“花里胡哨”的距离：
9种距离度量方法，欧氏距离、切比雪夫距离等

（3）算法流程

KNN算法的大致流程如下：

选择距离公式，进行距离计算（一般选择欧式距离）
对距离进行排序，并选取出最近的K个点
根据这K个点，确定未分类样本点的分类

很明显，使用KNN算法时，针对每一个未分类点，我们都需要计算该店与周围一系列点的距离。当点的数量很多时，计算量也将非常非常大。而使用下面介绍的KD-Tree数据结构，则能够优化搜索操作，有效的减小计算量（无需挨个计算距离比较）。

4.4 KD-Tree

KD-Tree（K-Dimension Tree），即一种将数据点在K维空间中进行划分的数据结构。中心思想：KD树能够将整个空间划分为特定的几个部分，只需要在特定空间中进行搜索操作，能够有效减少计算量。前面的KNN的是排序后一次性搜索出K个最近的点，而这里介绍的KD-Tree则是搜索出最近的一个点后，得到K个最近点。
KD-Tree 能够建立众多数据点之间的联系，借助他们存在的这种联系，就能够进行针对性的搜索操作。
KD-Tree的学习主要包括构造和搜索两个部分。构造即如何搭建这么一个数据结构，搜索则是如何使用搭建好的数据结构进行最近点搜索。

(1) KD-Tree 构建

文章前面推荐的视频中，有KD-Tree的构建过程的详细讲解。各个视频中的构造依据不大相同，我这里会先介绍易理解的构造过程，然后再补充一些构造依据。直接以一个例子为例进行讲解：
直接采用这个视频中的例子： https://www.bilibili.com/video/BV1L4411c7XF?p=5
我们使用下面这6个二维样本点进行KD-Tree的构建：
(2,3)、(5,4)、(9,6)、(4,7)、(8,1)、(7,2)
最终可以得到下图的KD-Tree形式。左侧为二维空间的分割图，右侧为各个节点的关系图（kd树）。

=== 简易构造过程
# 6个二维样本点  （x,y）
(2,3)、(5,4)、(9,6)、(4,7)、(8,1)、(7,2)
前面提到KD-Tree（K-Dimension Tree）是一种将数据点在K维空间中进行划分的数据结构。而上述的6个二维样本点，自然就只需要划分两个维度（X、Y两个维度）。如果是三维样本点，则相应的划分三个维度（X、Y、Z三个维度）。

该例子的简易构造过程：

1）默认选取x维度，以所有样本点在该维度上的数值进行升序排序，选取中位数对应的样本点为根节点，并将剩余样本点分别划分为根节点的左子节点和右子节点；

2）再选取y维度，以左右子节点在该维度上的数值分别进行升序排序，分别选取中位数对应的样本点为根节点，并将剩余样本点分别划分为根节点的左子节点和右子节点；

3）再选取x维度，如此反复进行，直到所有子根节点下都没有子节点了。

1）第一次划分

取n为构造的维度，可取0、1（对应x\y轴）。默认从n=0开始，即默认从x轴开始进行坐标空间上的划分。我们先将六个样本点进行排序如下（n=0，按照x坐标值进行排序）
# n=0，按照x坐标值进行升序排序
(2,3)、(4,7)、(5,4)、(7,2)、(8,1)、(9,6)
目前有6个样本点，取N=6。我们选取中间的一个样本点作为根节点，（N/2+1）= 4，即选取（7,2）作为根节点。同时，我们将其左侧的点作为其左侧子节点，右侧点作为右侧子节点，具体如下图所示。
1）(7,2)为根节点  【中位数】 
2）(2,3)、(4,7)、(5,4)为根节点的左侧子节点 
3）(8,1)、(9,6)为根节点的右侧子节点 
# 划分时，过根节点做垂直于划分维度的线
2）第二次划分

经过上一步的划分后，我们紧接着就得进行子节点的划分了。左右子节点需分别进行划分，这里以左侧子节点为例。第一次划分n=0，这第二次划分则取n=1，即按照y轴坐标进行排序，并选取中位数进行划分。

左右子节点都按照 n=1 进行划分（他们都是同一层的）
# n=1，按照y坐标值进行升序排序
(2,3)、(5,4)、(4,7)
有三个左侧子节点，N=3，取中位数（N/2+1）=2，即选择（5,4）作为左侧子节点的根节点。同时，（2,3）被划分为根节点（5,4）的左侧子节点，（4,7）被划分为根节点的右侧子节点。同理对（7,2）的右侧子节点进行划分，可得下图。

3）第三次划分

经过前面两次的划分，我们已经将如上6个节点的关系划分完成。但左侧的图片中还需要进一步的划分子空间。前面划分时n=1，又因为该6个点处于2维空间，所以此处划分时n=0，即按照x轴进行划分，划分结果如下：

===构造依据

我们回顾一下前面的“简易构造过程”，其维度选择上是先x轴再y轴，两者轮流着来的。那样的选择过于随意，而且按照先y轴后x的顺序，得到的kd树是完全不一样的。而在进行划分维度的选择时，实际上存在着一些依据，如下图所示：

使用样本方差度量各维度数据的分散程度，并优先选择样本方差大维度进行划分。

4.5 KD-Tree 搜索

前面我们成功的构造出了由6个二维样本点构成的KD-tree，接下来，我们就要使用构造的KD-tree进行最近点的搜索啦。

我根据个人理解将KD-Tree的搜索可分两小步：

1）初始化路径：结合KD-tree与待搜索点，初步判断最近点位置。

2）回溯路径：计算距离进行路径回溯，并求得最近点。

(1) 初始化路径

我们一起来寻找上述六个点中与点（4,5）最近的点。首先，我们可根据上图右侧的KD-tree，依次寻找一系列的点。具体步骤如下：

（4,5）先与根节点（7,2）比较，因为按x轴划分，故比较其x轴数值。因4<7，故最近点应该于x=7的左侧子空间寻找。（之后也就将与（5,4）比较）
（4,5）与左节点（5,4）比较，因为按y轴划分，故比较其y轴数值。因4<5，故最近点应该于y=4的上侧子空间寻找。（之后也就将与（4,7）比较）
（4,5）与左节点（4,7）比较，因为按x轴划分，故比较其x轴数值。因4=4，故最近点可能位于x=4的左右两侧子空间。此时会发现，节点（4,7）下已经没有其他子节点了，为此该部分工作结束，准备开始回溯路径。（如果左右两侧还有子节点，那就继续重复上述操作）

(2) 回溯路径 *

回溯路径可能比较难理解，需要多琢磨一下。
我的文字描述终究还是没有视频讲解容易理解，这里看不明白可以看看视频。
https://www.bilibili.com/video/BV1L4411c7XF?p=5

上一步只是初步判断最近点位于节点(4,7)附近，而回溯路径则将通过计算，确定最近点。回溯回溯，自然是从初步确定的最近点(4,7)处逆向进行计算。前面我们确定了一个路径《(7,2), (5,4), (4,7)》，回溯时会用到。

我们计算的距离为两点之间的直线距离，即欧式距离。因为我们前面已经把 二维空间分割图绘制出来了，那 完全可以借助几何的方式判断距离的远近，具体如下：

1）我们先以点（4,5）为圆心，点(4,5)到点(4,7)的距离为半径画圆。可见绘制的⚪与分割线y=4相交（对应节点(5,4)），这个时候就需要回溯，即往前退一步，找到(4,7)的根节点(5,4)。

分割线也可以叫做超平面。
如果这里绘制的⚪与其他分割线都不相交(除 x=4)，那就无需回溯，且可以直接认为点(4,7)即为最近点。

2）我们此时就需要另外考虑“新的子空间”下的几个点，即(5,4)、(2,3)。根据前面绘制的⚪不难发现，点(2,3)在圆圈外，而点(5,4)在圆圈内。为此，可知(5,4)到点(4,5)的距离更近。紧接着，我们需要以点（4,5）为圆心，点(4,5)到点(5,4)的距离为半径画圆。此时，可以发现新的⚪并未和其他的分割线相交，也就不需要回溯了，我们也可以确定点(5,4)即为点(4,5)的最近点。

此时的⚪并未越过x<=7子空间，故无需回溯。并且，直接判断也能得出，没有比(5,4)更近的点啦。