常见传感器与SLAM搭配

只有多样性才能吸收多样性。一个有效的控制系统，其自身必须具有与被控对象干扰或复杂性相匹配的多样性与灵活性。 —W. Ross Ashby
理论来源于工程，理论高于工程，理论指导工程 —钱学森
笔者是一个名不见经传的感知算法爱好者，对于目前很多算法的所谓部署的局限性，我在这里提出一点一面之词也是自己从高中开始学习的的感悟。或有思考，或有暴论，权当笑料公众作为茶语便后的谈资罢了。
首先我们必须要知道以SLAM在内的算法是为了什么，我们都知道SLAM是为了获取一个地图与相对位置，那么我们首先需要知道SLAM算法的输入与输出。很明显SLAM的输入是各个传感器数据：无论是单目相机，深度相机，Lidar，IMU，光流检测，激光传感还是什么，他们反馈的数据都可以看作为真值加噪声，再经过映射与滤波等预处理步骤，得到一个“干净”的输入数据并送到SLAM进行加权平均，弥补各个传感器的不足（卡尔曼滤波与他的贤子孝孙）或者是对各个传感器的数据压成一个个节点内的环境变量矩阵迭代视作约束并进行联合优化（图优化）。那么我在这里就可以浅浅的下个定义了：SLAM的本质就是为了对各个传感器的输出数据进行的处理并输出在当前情况最佳的相对位置。用稍微高大上的语言就是：SLAM的本质，是一个通过多传感器融合进行联合状态估计的过程，旨在实时、同步地解算移动主体在未知环境中的运动状态和该环境的空间结构。那么我们就不得不回到传感器的领域，好好看看各个传感器的作用。

1.常见的传感器

我相信各位已经很熟悉单目相机，深度相机这种的了，那我们就看看别的的传感器吧。

1. IMU（惯性测量单元）：他可以测量物体三轴姿态角以及加速度的装置。IMU通常包含陀螺仪、加速度计 ，有的还包含磁力计。陀螺仪用来测量三轴的角度/角速度，加速度计用来测量三轴的加速度，磁力计提供磁场朝向信息。但是它存在结构性缺陷：加速度计的任何微小零偏误差都会在二次积分后导致位置误差的发散，而陀螺仪的微小零偏误差在积分后会导致角度误差的线性发散。因此，在惯性导航中，IMU无法独立长时间工作，必须依赖外部参考进行周期性修正，这就是组合导航系统的基本原理。
2. 激光传感器：通过发射单色性强、方向性好的激光束，利用飞行时间法（TOF）或相位差法测量反射信号的时间差或相位偏移，实现毫米级精度的距离、速度与轮廓测量。具有高精度（厘米级）、高速度、抗环境光干扰强；激光的光斑更小，受距离影响较小，适合复杂环境下高精度检测（这玩意的升级版就是Lidar了）。缺陷是环境影响大，受限于物理环境限制，但是在一般场景下，激光传感器的精度还是很客观的。
3. 光流传感器（鼠标那玩意）：通过计算两个连续帧之间的光流，来判断物体运动方向与速度。光流检测算法主要光流方程法。光流方程法通过像素位置变化方程，来判断物体运动方向与速度。光流检测算法的优点是计算简单，且对环境影响小，缺点是计算效率较低，且对光流方程法来说，计算方程的精度有限。
4. Lidar（激光雷达）：自动驾驶与机器人感知中常见的高精度传感器之一，目前的Lidar传感器已经可以达到厘米级精度，并且具有高精度、高速度、抗环境光干扰的特点。但是还是受限于物理环境限制，雨雪天气和高反射率的材质，Lidar的精度会下降，而且受限于光束的密度，他反馈回来的数据是稀疏的，数据量远不如相机拍摄的稠密矩阵，因此在生产生活中常把他和相机放在一起中，形成一个深度图做进一步处理。
5. 压力传感器：通过压力传感器，可以判断当前机器人碰撞状态，从而进行避障或者建图识别。一般来说结构简单且作用在一般的机器人的避障程序比较多，可以作为异常状态的最后底线处理方法使用。
6. 红外传感器：与激光测距类似，但是他能进行热源感应，因此可以简单判别生物或者危险热源，适合在避障中使用。
7. 编码器：编码器是一种将物理信号（如旋转角度或线性位移）转换为电信号的设备，广泛应用于自动化、机器人和数控机床等领域。他能输出角度与速度信息，可用于关节处的运动与位置控制，按照类型信号可以分为增量和绝对两个类。
8. 霍尔传感器：霍尔传感器在感知方面的核心作用是将磁学量（磁场的有无、强弱、方向、变化）转换为电信号，从而实现对物理世界的非接触式、高可靠性感知。换句话说就是利用转动或者位姿变化中磁强度等的周期性变化来输出判断位姿信息，正因如此，霍尔传感器是磁性编码器的核心组成部分之一。相对于其他同等类型编码器来说，在中低分辨率应用中，通常比同等级光学编码器成本更低。
9. 光电传感器；与霍尔类似，他是光电编码的一个核心部件，轴转动时，码盘的光栅交替透光/遮光，使光敏元件接收到明暗交替的光信号，并转换为电脉冲，以他为基础组成的编码器分辨率和精度是所有编码器类型中最高的，但是极怕污染，比磁感的环境稳定性差得多，因此一般来说工业机器人还是用霍尔传感器为主的磁感应编码器。

2.常见的传感器算法

光流算法

光流算法就跟我们在之前说的光流传感器的作用一样，本身这个传感器就是集成了光流算法的摄像头加后端处理单片机。对于帧间速度测算主要是分为稀疏和稠密两种，稀疏是指只计算某些像素点，稠密则计算所有像素点。在这里光流算法有两个假设

1. 目标亮度恒定不变。即同一目标在不同帧间运动时，其亮度不会发生改变。这是基本光流法的假定（所有光流法变种都必须满足），用于得到光流法基本方程。
2. 时间连续或运动是 “小运动”。即时间的变化不会引起目标位置的剧烈变化，相邻帧之间位移要比较小。同样也是光流法不可或缺的假定。
   由于篇幅，这里只介绍稀疏光流算法。

稀疏光流算法

首先摄像头会对快速拍摄的帧进行图像预处理，包括图像裁剪，去噪，图像校正与灰度图转换（从RGB三通道转换成黑白单通道）来减少噪声和计算量。在经过一定处理之后我们把他送入光流的特征检测器中寻找角点，现在常见的特征检测器有Shi-Tomasi角点检测器，FAST 角点检测器，ORB|定向FAST和旋转BRIEF（应用较为广泛）。这里把ORB进行解释，他也是ORB-SLAM的核心部分（opencv中有他的函数cv2.ORB()），其他有兴趣的可以去自行查阅。
ORB 基本上是通过融合FAST关键点检测器和BRIEF描述符，并进行多项修改来增强了性能。它首先使用FAST来查找关键点，然后应用Harris角点度量来查找其中的前N点。它还使用金字塔来生成多尺度特征。首先先用FAST检测关键点：对特征点周围划定一个区域。在这像素中，有连续N个的亮度都同时高于或同时低于中心点p的亮度加上一个阈值t，则该点p被认为是角点。但是这样很明显是一个粗略的（帧间大了容易目标丢失，而且容易被噪声污染，例如突然地强光照射部分候选点），所以他们选用Harris角点响应筛选:对于每一个FAST检测出的候选点，在其周围取一个小的图像块,进行这个角点的局域梯度计算按照R值从大到小对所有候选点进行排序。在这里为了防止尺度噪声问题，他们还选择使用图像金字塔，对图像卷积后形成多尺度特征，然后对不同尺度的图像块进行角点检测。


概述说完就是公式.他的作用公式为：对于FAST筛选出的点在其周围取一个小的图像块，这个窗口内的所有像素计算其梯度的二阶矩，并求和形成矩阵：
首先fast点检测的公式为：

$$m_{pq} = \sum_{(x, y) \in \text{Patch}} x^p y^q I(x, y)\tag{1-1}$$

其中 $I(x, y)$ 是像素强度，坐标以关键点为中心
然后对其计算总强度和一阶矩（并非梯度）：

$$m_{00} = \sum_{x, y} I(x, y), \quad m_{10} = \sum_{x, y} x \cdot I(x, y), \quad m_{01} = \sum_{x, y} y \cdot I(x, y) \tag{1-2}$$

这里$ \cdot I(x, y)$ 表示像素强度的乘积。后面两个是一阶矩。物理意义：计算整个图像块内所有像素的“质心”位置。如果图像块是完全均匀的，质心就在中心。如果图像块有亮度分布（如一个角点或边缘），质心就会偏离中心。
关键点的主方向 θ由图像块的质心相对于关键点的方向决定：

$$C = \left( \frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}} \right) \theta = \arctan2(m_{01}, m_{10}) \tag{1-3}$$

也就是从你的质心计算出来之后给你的关键点拉一个向量方向出来，然后计算这个向量的角度，这个角度就是这个关键点的主方向。
FAST检测完成后，就对角点进行一个筛选，这个筛选就是Harris角点响应：

$$M = \sum_{\text{window}} \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix} = \begin{bmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{bmatrix} \tag{1-4}$$

这个M矩阵被称为结构张量，他是为了计算点的变化率。理论上说一下推导：图像梯度$∇I=(I_x, I_y)$是一个向量,它指向该点处亮度上升最快的方向，其大小表示在这个方向上的最大变化率。但梯度只告诉我们最优方向上的变化。如果我们想知道在任意单位方向向量,因此用方向导数给他导一下就可以得到M矩阵。
Harris 响应值 $R$ 为：
其中$\lambda_1$和$\lambda_2$是矩阵的特征值，这两个方向分别对应了变化最快和最慢的方向。

$$R = \det(M) - k \cdot [\operatorname{trace}(M)]^2 \tag{1-5}$$

其中 $\det(M) = \lambda_1 \lambda_2$，$\operatorname{trace}(M) = \lambda_1 + \lambda_2$，$k \approx 0.04\text{—}0.06$。
但是很明显，算特征值太慢了，所以他们很多是显式计算形式：

$$R = \left( \sum I_x^2 \right) \left( \sum I_y^2 \right) - \left( \sum I_x I_y \right)^2 - k \left( \sum I_x^2 + \sum I_y^2 \right)^2 \tag{1-6}$$

这里如果又想去的同学可能会好奇为什么两个特征值对应了图像梯度的变快最快与最慢，这里进行一个推导：
1.考虑图像中点 $(x_0, y_0)$ 的一个局部窗口 $\Omega$。定义图像 $I(x, y)$ 的梯度为 $\nabla I = (I_x, I_y)$，其中 $I_x = \frac{\partial I}{\partial x}, I_y = \frac{\partial I}{\partial y}$,这个是高数知识我相信你们还是可以看懂的。
2.入窗口函数 $w(x, y) \geq 0$（如高斯函数,其实就是高斯分布的方程），用于给窗口内不同位置赋予权重。则沿任意单位方向向量 $\mathbf{u} = (u, v)$ 的平均亮度变化强度 $E(\mathbf{u})$ 定义为：

$$E(\mathbf{u}) = \sum_{(x,y) \in \Omega} w(x, y) \cdot [\nabla I(x, y) \cdot \mathbf{u}]^2 \tag{1-7}$$

这里的权重赋值存在一个先验：靠近窗口中心的像素对计算贡献更大，远离中心的像素贡献较小。这符合”局部特征”的直觉——离中心越近，相关性越强。
3.定义结构张量（自相关矩阵 $M$ 为）：

$$M = \begin{bmatrix} M_{00} & M_{01} \\ M_{01} & M_{11} \end{bmatrix} = \begin{bmatrix} \sum_{\Omega} w I_x^2 & \sum_{\Omega} w I_x I_y \\ \sum_{\Omega} w I_x I_y & \sum_{\Omega} w I_y^2 \end{bmatrix} \tag{1-8}$$

于是 $E(\mathbf{u})$ 可简洁地写为二次型：

$$E(\mathbf{u}) = \mathbf{u}^T M \mathbf{u} \tag{1-9}$$

其实这个二次型就是$E(\mathbf{u}) = \sum_{(x,y) \in \Omega} w(x, y) \cdot [\nabla I(x, y) \cdot \mathbf{u}]^2$这个式子的展开，你自己把后面部分展开就看懂了。
这里科普一下半正定矩阵的定义：给定一个大小为 n\times n 的实对称矩阵 A ，若对于任意长度为 n 的向量 $\boldsymbol{x} 有 \boldsymbol{x}^TA\boldsymbol{x}\geq0$ 恒成立，则矩阵 A 是一个半正定矩阵。,然后我们反过来看二次型的式子，我们会发现他就是一个半正定的矩阵。也因他是半正定的，我们可以对其特征分解：

$$M = V \Lambda V^T = \begin{bmatrix} \mathbf{v}_1 & \mathbf{v}_2 \end{bmatrix} \begin{bmatrix} \lambda_1 & 0 \\ 0 & \lambda_2 \end{bmatrix} \begin{bmatrix} \mathbf{v}_1^T \\ \mathbf{v}_2^T \end{bmatrix} \tag{1-10}$$

其中：
$\mathbf{v}_1$ 和 $\mathbf{v}_2$ 是特征向量， $\lambda_1$ 和 $\lambda_2$ 是特征值。特征分解可以理解为把一个矩阵变成一个从原来的三维坐标系先变换成当前矩阵的坐标系，再经过线性的伸缩，最后再变换回原来的坐标系的方法。表现了他的变化过程。
任意单位方向向量 $\mathbf{u}$ 可以用特征基 ${ \mathbf{v}_1, \mathbf{v}_2 }$ 线性表示：

$$\mathbf{u} = \alpha \mathbf{v}_1 + \beta \mathbf{v}_2 \tag{1-11}$$

由于 $\mathbf{v}_1, \mathbf{v}_2$ 是单位正交基，系数为投影：

$$\alpha = \mathbf{u}^T \mathbf{v}_1, \quad \beta = \mathbf{u}^T \mathbf{v}_2 \tag{1-12}$$

且由于 $\mathbf{u}$ 是单位向量：

$$\alpha^2 + \beta^2 = 1$$

最后推导：
将 $\mathbf{u} = \alpha \mathbf{v}_1 + \beta \mathbf{v}_2$ 代入 $E(\mathbf{u}) = \mathbf{u}^T M \mathbf{u}$：

$$\begin{aligned} E(\mathbf{u}) &= (\alpha \mathbf{v}_1^T + \beta \mathbf{v}_2^T) M (\alpha \mathbf{v}_1 + \beta \mathbf{v}_2) \\ &= \alpha^2 \mathbf{v}_1^T M \mathbf{v}_1 + \alpha \beta \mathbf{v}_1^T M \mathbf{v}_2 + \alpha \beta \mathbf{v}_2^T M \mathbf{v}_1 + \beta^2 \mathbf{v}_2^T M \mathbf{v}_2 \end{aligned} \tag{1-13}$$

利用特征向量的性质$M\mathbf{v}_i = \lambda_i\mathbf{v}_i 正交性 \mathbf{v}_i^T \mathbf{v}_j = \delta_{ij}$可以得到：

$$\begin{aligned} \mathbf{v}_1^T M \mathbf{v}_1 &= \mathbf{v}_1^T (\lambda_1 \mathbf{v}_1) = \lambda_1 \\ \mathbf{v}_1^T M \mathbf{v}_2 &= \mathbf{v}_1^T (\lambda_2 \mathbf{v}_2) = 0 \\ \mathbf{v}_2^T M \mathbf{v}_1 &= \mathbf{v}_2^T (\lambda_1 \mathbf{v}_1) = 0 \\ \mathbf{v}_2^T M \mathbf{v}_2 &= \mathbf{v}_2^T (\lambda_2 \mathbf{v}_2) = \lambda_2 \end{aligned} \tag{1-14}$$

代入上式，得到核心结论：

$$\boxed{E(\mathbf{u}) = \alpha^2 \lambda_1 + \beta^2 \lambda_2} \tag{1-15}$$

这是数学解释，按照物理的解释是：
由 $E(\mathbf{u}) = \alpha^2 \lambda_1 + \beta^2 \lambda_2$ 和约束条件 $\alpha^2 + \beta^2 = 1$，可以立即得出：
$\mathbf{u} = \mathbf{v}_1$ 时，$\alpha = 1, \beta = 0$，则

$$E(\mathbf{v}_1) = \lambda_1$$

$\lambda_1$ 是沿方向 $\mathbf{v}_1$ 的平均变化强度，并且这是所有可能方向中的最大值。
$\mathbf{u} = \mathbf{v}_2$ 时，$\alpha = 0, \beta = 1$，则

$$E(\mathbf{v}_2) = \lambda_2$$

$\lambda_2$ 是沿方向 $\mathbf{v}_2$ 的平均变化强度，并且这是所有可能方向中的最小值。
任意其他方向 $\mathbf{u}$，其变化强度 $E(\mathbf{u})$ 是 $\lambda_1$ 和 $\lambda_2$ 的加权平均，权重为 $\alpha^2$ 和 $\beta^2$，满足：

$$\lambda_2 \leq E(\mathbf{u}) \leq \lambda_1$$

Harris角点响应结束后需要对其rBRIEF 描述子生成
设旋转后的第 $i$ 个测试点对为 $(p_i, q_i)$，比较函数 $\tau$ 定义为：

$$\tau(p_i, q_i; I) = \begin{cases} 1, & \text{if } I(p_i) < I(q_i) \\ 0, & \text{otherwise} \end{cases}$$

对于 $n$ 个点对（通常 $n=256$），rBRIEF 描述子是一个 $n$ 位二进制字符串：

$$f_n(I) = \sum_{i=1}^{n} 2^{i-1} \cdot \tau(p_i, q_i; I) \tag{1-16}$$

这里是根据上一步计算出的关键点方向根据旋转矩阵原始的 BRIEF 测试点对坐标集进行旋转，得到矫正后的点对坐标。这样，无论图像如何旋转，描述子所使用的采样模式总是与关键点方向对齐，从而实现了旋转不变形。
这里会有很多朋友不理解旋转不变性的概念，类比一下就是
小白：我的一个特征左边是三角形，右边是圆形，上面是菱形，下面是正方形。
小黑：我有一个特征左边是正方形，右边是菱形，上面是三角形，下面是圆形。
虽然这两个是一样的，但是旋转导致的结果是不同的，这就是没有旋转不变性，同理一下对话：
小白：我的一个特征西边是三角形，东边是圆形，北边是菱形，南边是正方形。
小黑：我也有一个特征西边是三角形，东边是圆形，北边是菱形，南边是正方形。
这时候两个特征的描述就一致了，即拥有了旋转不变性。所以只要对特征定义方向，然后在同一个方向上进行特征描述就可以实现旋转不变性。这称之为Rotation Normalization。
OK扯远了，让我们回来讲述最后一个重点——汉明距离；
这个是对每个点的二进制数值串进行比较——对于两个等长二进制串，在相同位置上，值不同的位数，由此计算相似度，由于位运算是硬件亲和力极高的，所以运行速度极快。这也就在他的匹配策略中，对于两帧之间的相似度额按个昂视寻找两帧间汉明距离的最小值。这一步就完成了rBRIEF描述子的旋转与两帧之间的匹配，以上推导的这几步也是ORB百分之90的思路。
既然描述子已经生成，那么就可以开始匹配了，这里我们以LK算法作为匹配算法进行解释。
在谈论这个之前我们要知道LK算法他本身在做什么：他是在寻找在小范围窗口内，估计像素块从上一帧到下一帧平移了多少的方法。
这里我们假设有两帧图像:$I_1(x,y,t)$和$I_2(x,y,\delta t)$
现在想知道某个点附近的小块，在下一帧里移动到了哪里,就是求这个点的位移,这里我们把它定义为$(u,v)$
u：x 方向位移
v：y 方向位移
由于上面的假设中中的亮度一致性，我们同一个物体点在短时间内，灰度基本不变，在公式上就是

$$I_1(x,y,t) = I_2(x+u,y+v,t+\delta t)$$

意思是：第一帧这个点的亮度，等于下一帧它移动后那个位置的亮度。
然后引入第二个假设：机器人的运动是时间连续或运动是“小运动”。即时间的变化不会引起目标位置的剧烈变化。
这样我们自然而然就觉得，这个点在两帧之间的位移，应该很小。因此可以近似认为区域像素的位移都是相同的。
既然如此，我们就可以推导我们的光约束方程：

$$I(x,y,t) = I(x+u,y+v,t+\delta t) \tag{1-17}$$

我们对右边进行一阶泰勒展开：

$$I(x+u,y+v,t+\delta t) \approx I(x.y,t) +I_{x}u+I_{y}v+I_{t} \tag{1-18}$$

其中
$I_x = \frac{\partial I}{\partial x}$
$I_y = \frac{\partial I}{\partial y}$
$I_t = \frac{\partial I}{\partial t}$
代回亮度一致性，并消去$I(x, y, t) $，得：

$$I_x u + I_y v + I_t = 0 \tag{1-19}$$

这就是经典的光流约束方程,以$I_t$作为方程已知量求解u，v。由于有两个未知量所以要至少知道两个相关区域的的变化。
但是如果可以关注的区域足够多，那么可以用最小二乘（两个也是这么写的）：
对于窗口中的元素，都符合光约束方程：

$$I_{x,i} u + I_{y,i} v = -I_{t,i}$$

那么把他们堆积起来：

$$A = \begin{bmatrix} I_{x,1} & I_{y,1} \\ I_{x,2} & I_{y,2} \\ \vdots & \vdots \\ I_{x,N} & I_{y,N} \end{bmatrix} \begin{bmatrix} u \\ v\end{bmatrix} = \begin{bmatrix} -I_{t,1} \\ -I_{t,2} \\ \vdots \\ -I_{t,N} \end{bmatrix}$$

这里记作：

$$A \begin{bmatrix} u \\ v \end{bmatrix} = b$$

通常是超定方程，用最小二乘解：

$$\begin{bmatrix} u \\ v \end{bmatrix} = (A^T A)^{-1} A^T b \tag{1-20}$$

这里是线代知识我觉得没必要特别深入了，不会的回炉重造一下。
这里我们把这个b前面的矩阵展开一下：

$$A^T A = \begin{bmatrix} \sum I_x^2 & \sum I_x I_y \\ \sum I_x I_y & \sum I_y^2 \end{bmatrix} \tag{1-21}$$

是不是感觉很熟悉？他就是Harris角点响应的M矩阵不是吗（请看公式1-4），Harris用它来“判角点”，LK 用它来“解光流”,这个是为什么呢？
首先我们要知道这两个是在干什么，在特征提取器中，包括ORB在内的算法旨在寻找一个最有辨识度的角点，LK算法则用来解光流，解光流是为了找到两个帧之间的位移，从而实现运动估计。
那么他们都需要找到一个最独特的像素部分，他梯度要大，变化要剧烈，这样才算明显独特，因此好角点也是LK的好跟踪点，因此ORB介意与LK写作提取点，但是工程往往先用Shi–Tomasi找适合跟踪的角点而不是ORB，这里讲解ORB只是因为他比较综合，涵盖面比较广。

总结：光流的对机器人的作用

我觉得上太多数学名词对于工程是一个很差的东西，因此我在这里讲一下光流的作用都有什么。
一言蔽之：当摄像头连续拍摄两帧或多帧图像时，如果机器人在运动、目标在运动，或者两者都在运动，那么图像里的点的位置就会发生变化。光流就是利用这种变化，去估计图像中的运动信息。
在视觉里程计（VO）里，光流或者相关算法会对每一帧的图像的像素点变化情况，由此推得运动信息，从而得到位姿信息。他的成本较低，响应较快，但是精度较低。而且光流描述的是图像平面上的运动，而不是机器人在三维空间中的完整位姿，因此通常需要与几何模型、深度信息或惯性测量数据结合使用。

雷达点云处理方法

对于激光雷达这种东西，他的样子往往长得很抽象：弥散在空间的一堆点，仅仅能用个他的密集程度稍稍能看出来他的几何信息，畏怯它往往还存在大量的噪声，因此需要用一些方法来处理，这个数理方法叫做滤波，在滤波后还需要进行分割区分各个平面辅助机器人识别，在这一章节我会稍微讲解一下这两种方法，而且由于原理较为原始。没有什么设计梯度，特征点选择与估计变化的地方，所以不会有很多公式，请各位放心，而且这些方法很多都在都在PCL库中已经完成，可以直接调包使用。

滤波

滤波就是把一些无用的点去掉，比如激光雷达的点云数据，一般会用一些滤波方法来处理，这里我介绍两种滤波方法：半径滤波和体素滤波，剩下的就看各位兴趣自己查阅。

1. 半径滤波：
   
   半径滤波会对每个点进行搜索，搜索半径为r，搜索半径内的点，如果这个点与当前点距离小于r，那么这个点就属于当前点，否则不属于当前点。再设定一个阈值，如果当前点与搜索半径内的点个数小于阈值，那么这个点就属于当前点并进行保留，如果少于阈值，救人位此为噪声点且不保留。当然这对于远处的信息就不太友好了，因为激光密度越远越低。拗不过对于机器人这种导航来说本身近处的信息更重要，远处的点云要不要得倒是也无所谓。
2. 体素滤波：
   
   实际上是否算滤波，这要视情况而定，我个人认为他实际上是一个降采样方法：它的原理是根据输入的点云，首先计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并用该质心的坐标来近似该立方体内的若干点，这样总体点云数量减少还能保留几何信息。事实上，体素化可以说是影响了后续的一大批雷达算法，包括但不限于栅格化地图，点云目标识别等。
   滤波效果
   

分割

点云分割是一个比较重要的步骤，他可以对墙壁、物体、人、其他物体进行分割或者粗分类，从而得到各个平面，方便机器人进行导航。这里也是讲解两个方法：区域生长算法与法线域算法：

1. 区域生长算法：首先对每个需要分割的区域找出一个种子点作为生长的起点，然后将种子点周围邻域中与种子有相同或 相似性质的点合并到种子像素所在的区域中。而新的点继续作为种子向四周生长，直到再没有满足条件 的像素可以包括进来，一个区域就生长而成了。在PCL中，这个算法用pcl::RegionGrowing类实现，具体参数设置请参考官方文档。
2. 法线域算法：他的本来的名字叫做DoN算法，主要是因为他用了法向量辅助所以我习惯性的叫他法线域算法。他的DoN特征源于观察到基于所给半径估计的表面法向量可以反映曲面的内在几何特征，因此这种分割算法是基于法线估计的，需要计算点云中某一点的法线估计。而通常在计算法线估计的时候都会用到邻域信息，很明显邻域大小的选取会影响法线估计的结果。而在DoN算法中，邻域选择的大小就被称为support radius（支持半径）。对点云中某一点选取不同的支持半径，即可以得到不同的法线估计，而法线之间的差异，就是是所说的法线差异。（这里懒得写了，复制粘贴了CSDN的 blog 网址（点击））总体来说他是一种比较先进的点云滤波算法了，PCL中，这个算法用pcl::NormalDistanceSquared类实现，具体参数设置请参考官方文档。
   这里给一下点云分割的效果：
   分割效果
   

点云配准（正菜要来喽）

视觉有光流匹配这种方法，但是激光雷达没有光流啊，在同等生态位的名字叫做点云配准：即求一个位姿变换$\begin{bmatrix} R, t \end{bmatrix}$,把源点云$S$映射到目标点云$T$（统一到同一坐标系），使得$S$和$T$的点对之间的距离最小，这里我们主要介绍ICP，NDT算法（也是比较常见的，而且我也就会这俩）。

1. ICP：ICP在起始会有两个输入：源点云$S$和目标点云$T$（在给定的初始姿态$\begin{bmatrix} R, t \end{bmatrix}$）,他会通过迭代寻找一个最小化点云匹配的变换，使得源点云$S$和目标点云$T$的点对之间的距离最小。这个迭代过程是点云匹配和奇异值分解的过程。通俗一点就是：反正我不知道两组点云之间点的对应关系，那就认为距离最近的点是相互匹配的，然后以此来计算两组点云之间的变换关系。在得到变换关系之后，对点云进行变换并计算误差。然后再次选取距离最近的点作为匹配点并求解变换关系，重复该过程直至误差满足要求。
   那么它的缺点也是显而易见：搜索耗时，面对环境的局部变化难以收敛，容易陷入局部最优。
   正因为他要对点进行一个匹配，那么我们就可以构建一个最小二乘法并计算每个点距离这个直线的距离作为误差，这个距离的最小化就是这个点云的匹配关系。
   那么他的问题可被描述成： $$E(R, t) = \arg\min_{R \in SO(d),\, t \in \mathbb{R}^d} \sum_{i=1}^{n} w_i \left\| R q_i + t - p_i \right\|^2 \tag{2-1}$$ 这里的$R,t$是调节参数，指代的是点云匹配需要的旋转与平移，argmin表示这个函数的损失最小值的时候对应的$R,t$参数。$\sum_{i=1}^{n} w_i \left| R q_i + t - p_i \right|^2 $是加权欧氏距离平方和，衡量变换后点与原目标点的匹配程度，这个可能看着比较云里雾里，我叫他损失函数，这里面$w_i$是点$i$的权重，$q_i$是源点云$S$中的点，$p_i$是目标点云$T$中的点。所以，整个求和式是：所有点对经过变换后，与目标点之间的加权欧氏距离平方和，这里具体的求解过程不再展开。
   PS:虽然这里约束是$SO(d)和t \in \mathbb{R}^d$，但是实际上$SO(d)$的元素是旋转矩阵,$t$的约束也仅仅作为的维度约束，平移的性质不变，那么实际上这里可以改写成四元数，这样就能跟机器人的姿态解算很相似了，也能辅助理解。
2. NDT：NDT算法他就不一样了。ICP他的目的是点对点匹配，就是比如一辆车飞驰而过，一个人走过的时候这个对应上就有极大的难度了。但是NDT他是栅格化后的概率匹配可以很好地解决细微，它相对更有鲁棒性，这里详细介绍一下：
   首先他进行的是一个对每个体素进行一个多维高斯分布拟合： $$[\mu_i = \frac{1}{n} \sum_{j=1}^{n} x_j, \quad \Sigma_i = \frac{1}{n - 1} \sum_{j=1}^{n} (x_j - \mu_i)(x_j - \mu_i)^T] \tag{2-2}$$ 这里的$\mu_i$是体素$i$的均值（在体素中代表点云密度的中心），$\Sigma_i$是协方差（各点对于密度中心的偏移程度）。以此衡量数据离散程度，从而得到体素的概率密度函数（服务后续的近似拟合）： $$P(x) = \frac{1}{(2\pi)^{d/2} |\Sigma|^{1/2}} \exp\left( -\frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right) \tag{2-3}$$ 这里的$P(x)$是体素$x$的概率密度函数，$d$是维度，$\mu$是均值，$\Sigma$是协方差，他的目的是利用体素内本身的数据离散程度去构建一个最接近高斯分布的拟合。我们要寻找一个变换$T$，使得所有点都尽可能落入目标体素的高斯分布中心,这就需要最大化似然估计去估计每个高斯分布的情况，当然“最大化所有点的概率”等价于“最大化概率的乘积”。 $$\mathcal{E}(T) = \sum_{x \in T(P)} \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \tag{2-4}$$ 最终的误差函数$\mathcal{E}(T)$本质上是所有变换后的点 x到目标体素中心 μ的加权欧氏距离之和。这里的权重就是协方差矩阵的逆$Σ^{-1}$。如果一个方向的数据很分散（方差大），权重就小；如果数据很集中（方差小），权重就大。这使得算法对分布密集的方向更加敏感。这里的优化主要是用高斯牛顿迭代求解，这里就不展开，对于NDT的算法细节留给读者自行探索。

点云配准的应用

那么在配准后我们得出了旋转矩阵R与平移矩阵t，这玩意怎么用呢？既然这里描述的是点云的变化那么我们也就可以将他反推带机器人的姿态上去。一般来讲旋转与平移部分是$R_{w,t} = R_{w,t-1} R \text{ 与 } t_{w,t} = R_{w,t-1} t + t_{w,t-1}$，当然他是存在误差的，这里在SLAM里是作为前端里程计，与光流地位相同。不过在更加先进的图优化SLAM中，他不需要进行显式的反推，而是利用输出的旋转矩阵R与平移矩阵t，表示我觉得应该长这样。并作为一个优化变量，与其他传感器约束一起求解，最大化满足各个传感器的要求。而不是把这些传感器拧成一个最终输出的位姿，然后求解。

IMU输出处理方法（IMU预积分）

IMU是什么不用我多说了（我说惯性导航王朝了尼尔多隆么），这在感知层面可以说比激光和摄像头还重要的器件在机器上的应用真是比野外的皮卡丘还多，那么这里我就讲一下他的预积分的解决的问题，以及作用机理与应用。

预积分作用

在我们的感知系统中，由于各个传感器之间的输出频率的不同，我们还是需要因地制宜处理各个传感器的时间同步问题的，常见的包括IMU降采样与Lidar平滑插帧等，但是对于IMU，如果每次对数据进行积分未免有些困难。那么我们是否可以把几百个数据分割一下组成一个增量位置约束，然后需要这段时候直接把前者的路程以矩阵乘法的形式乘进去呢，这样就变相实现了路程的全积分。这种操作是很节省资源的，兼容性也奇佳————面对图优化可以把预积分作为边约束，对于Lida可以利用预积分直接同步频率（就是积分后直接输出相对运动变化，不读输出了，跟两帧Lidar的时间差做同步时间对应），就算单独用也是节约计算量的好帮手，评价为夯。

原有的IMU积分处理

我们刚刚说了他是节约了大量的资源，那么之前的积分方法是怎么做的呢，答案就是存数据纯积分，朴实无华的方法。这就导致积分的计算量是线性增长的因为每次优化都要对误差融合进入重新积分，但是预积分是以时间检测为单位计算的，这就导致实际上他的计算难度是一个波动的缓慢上升（乘法的项数还是在增多的，但是每次积分的数量少了很多，设1000个数据做一次积分，那么积分计算量除1000）。同时优化也可以进行离散的优化（每个地区他的精度是不一样，台阶上这种颠簸和平底不一样），优化也变得方便得多（图优化）。在另一个视角考虑，如果把测量值当做偏差的函数，只需要在旧的测量值上添加一个近似的修正量就可以获得近似的新测量值，而不需要重新积分。为什么测量值能当做偏差的函数？因为这是假设……

由于身体原因和个人工作（准备个人搓一个自动室内感知系统），IMU的剩余部分可参考此知乎IMU预积分的理解和推导(点击)
————————————————未完待续———————————————