激光slam与视觉slam

介绍

Slam：同步定位与建图，就是在定位的同时，建立环境地图。
主要思路是根据运动学模型计算位姿，并通过传感得到的环境信息，对估计位姿调整优化，从而得到准确位姿，根据定位及感知数据绘制地图。
下图为slam主流框架：

1. 传感器感知在视觉SLAM中主要为传感信息的读取和预处理。
2. 前端里程计(Radar/Visual Odometry)。特征点匹配及运动估计。
3. 后端优化(Optimization)。后端接受不同时刻里程计测量位姿，以及回环检测的信息，对估计轨迹及环境信息进行全局优化。
4. 回环检测(Loop Closing)。回环检测判断机器人是否到达过先前的位置。
5. 建图(Mapping)。根据估计的环境信息及机器人运动轨迹，建立对应的地图。

前端：主要是提取处理传感器得到的数据，比如，激光的点云，视觉的图像，结合传感器数学模型及机器人运动学模型，推算得到机器人的位姿及热环境特征点的相对位置。
后端：由于模型偏差，会导致误差累积，因此，需要后端做一次全局的优化
回环检测：就是闭环检测，检测机器人是否到达原始位置，从而结合后端算法，对机器人的运动轨迹及地图进行全局的优化和调整。

前端差异

前端完成的主要任务包括：特征点提取，特征点匹配，机器人运动位姿估计。
一般前端运动估计都是与里程计结合的，如IMU或轮式里程计等。视觉匹配也算是里程计。通过里程计的运动学模型得到估计位姿，但是由于里程计模型的误差(静差)以及测量误差等，会导致累计误差产生，因此需要通过环境反馈的信息对位姿估计值进行修正优化。

环境特征：

• 视觉利用图像来解析位姿：特征点法和光流法均对图像特征点进行操作，大多使用角点(图像像素变化最快的点)，特征点法为特征点建立描述信息(描述子)，用来与其他帧匹配特征点。光流法则利用灰度一致性来匹配特征点，直接法则直接用像素块进行操作，使用像素块的移动解析出相机的位姿变化，直接法相对特征点法对相机硬件要求较高(需要较好的曝光及较高的帧率)，但计算量相对减少，一般会与特征点结合使用来保证精度。详细请看特征点法&光流法&直接法
• 激光利用得到的三维数据点来解析位姿：激光束打到实际位置返回来的激光束，得到的点。雷达收到的是各束光返回的距离信息。

提取特征点差异对比：

• 视觉得到的是像素特征点，需要通过前后帧图像对比计算得到实际的环境特征点的位置，因此会导致
• 激光得到是环境信息相对于激光的极坐标下的坐标。但是得到的环境信息量比较少，会加大点云配准的难度。

匹配算法对比：

• 视觉前端：利用帧间图像差估计运动，称视觉里程计
  代表算法 ：特征点法，光流法，直接法
  整体思路：就是根据前后帧得到的图像来估计机器人的运动变换
  特征点法：通过提取图像特征点，采用描述子来匹配前后帧图像的特征点，计算得到特征点的环境深度及估计机器人位姿。
  光流法：假设灰度信息不变，对比前后帧图像，得到同一特征点在图像上的移动。从而得到得到特征匹配，位姿估计算法同特征点法。特征点法与光流法是特征点的匹配方法不同。一个利用图像描述子，一个通过像素移动匹配特征点。
  直接法：基于直接法，利用得到的像素移动，来直接估计机器人运动。
• 激光：激光一般与其他里程计结合使用。那么其他里程计就是激光slam的前端。
  单独的激光slam(不含其他里程计) 也是利用前后帧特征点(点云)的变化来估计运动。
  激光由于环境信息量少，虽然知道具体特征点精确相对位置，但是帧间匹配由于信息量比较少，两帧点云的匹配相对视觉有难度，因此常与其他里程计结合。
  代表算法：ICP算法。ICP是3D-3D的一种求解位姿算法，多用于已知环境点的三维位姿，因此在激光slam中使用较多。ICP原理
  ICP算法其实是一种求解运动的思路：迭代优化，因为由于激光点云特殊，点云匹配和运动估计构成了鸡生蛋，蛋生鸡的问题，因此出现点云匹配和运动估计交叉迭代的求解思路，就是ICP。
  ICP是一种交叉迭代优化，即分为两步优化，优化点云配准与优化运动估计。
  演化算法：NICP NDT等算法。 后期演化算法求解思路一致，只是约束方程不同，约束方程的思路不同。
  NICP加入了点所在曲面的法向量信息，增加点云配准的可靠性。NICP详细介绍
  NDT算法是加入障碍物信息(栅格地图占用率)来进行匹配。NDT详细介绍
  目前激光使用点到线点到面的优化算法求解运动估计(代表算法LOAM)。

后端优化

用来修正估计位姿的累计误差，前端多是以单点建立约束方程，而后端则使用批量点或者新的传感器信息来求解，后端的计算量更大，所以一般降低帧率，或只在关键上处理，但是更精确。后端更详细整理请看后端优化算法
优化算法思路：激光和视觉的优化方法是类似的。通过后验来评估先验的准确性。
简单说，由A推出B，那么通过检验B的存在与否也能推测出A的存在与否。—来自十四讲

代表算法：拓展卡尔曼滤波器(EKF)，粒子滤波器(PF)，最小二乘法-图优化
KF: 应用于线性系统。基于高斯噪声分布，计算权重(乘子)，将估计位姿代入观测模型得到的观测值与真实观测值的差，乘以得到的乘子，就是位姿估计的误差修正。

EKF：就是将运动方程微分线性化(运动方程的一阶泰勒展开)，在瞬时就相当于线性方程，从而使用KF算法。缺点：有求逆过程，计算量随特征点数多少指数级上升。

PF：是使用粒子来模拟机器人，不同粒子所携带的位置信息有不同的可信度，通过得到的观测值来衡量粒子携带位置信息的可信度，并不断更新粒子。可信度最高的粒子就代表机器人的运动过程种的位置情况。缺点：粒子耗散问题

最小二乘法：使用最优化方法(最速下降法;GN;LM等)迭代改变估计位姿，使估计位姿代入观测模型得到的观测值与真实观测值的范数最小化。从而得到优化估计位姿。
图优化：基于最小二乘法，将多个时刻机器人的位姿同时进行优化。

回环检测

用来判断机器人是否到达之前位置
算法思路：有基于里程计结合的，就是根据机器人位姿大致估计有没有到回环。
因为不知道什么时候能是回环，实时比对太耗计算量，随机帧则可能错过回环。现在常用局部回环，
视觉：使用图像相似性来匹配回环
激光：点云信息少，回环不好建立。Cartographer的回环检测方法。通过匹配submap来匹配回环。

匹配代表算法：Bow(词袋模型)
词袋模型，找到一个字典，通过这个字典，得到每帧点云或者图像基于这个字典的表示。通过对比这个表示得到图像或者点云差异。
就好比判断人脸，可以使用眼睛，鼻子，嘴作为字典，查看图像是否包含这些元素，来判断是不是人脸图像。
字典生成：多层k-means聚类方法k-means介绍，就是使用不同特征分类，将点集进行分类，从而字典就使用kd-tree来表述。遍历到叶子节点的路径，就是点集基于该字典的描述。

视觉/激光优劣势对比

硬件上: 相机相比雷达安装方便，雷达贵
视觉：语义丰富，但是计算量大，对环境依赖性强，对光照条件要求高。
激光：相对更精确，但是雷达贵，探测范围有限，另外缺乏语义信息，回环检测难度大。
在环境场景多样性差的环境中，如结构一致的走廊或隧道等等结构化的场景中，会出现退化的问题，此时，激光相对于视觉(信息量大)，更容易出现退化的问题。SLAM中状态估计优化退化问题。

比如大平面，视觉和激光扫过去，得到每帧的特征点相类似，得到帧间匹配的特征点距离几乎均为0，从而无法采用优化法得到位姿变换估计。这就是退化问题。
位置求解中，可将约束函数线性化后，求解线性方程状态矩阵的特征值，特征值小于阈值的方向，则表明此方向上，位置求解存在退化，即位置估计在该方向上的不准确度大。如下图b在蓝色解在橙色方向有退化现象，橙色则正常。

激光slam及视觉slam其实流程相似，在特征匹配上差别：
视觉slam：提取图像特征点，特征点匹配通过匹配描述子确定。
激光slam：处理点云数据，通过ICP(交叉迭代)，NDT(概率模型)一类的方法匹配。参见点云配准及运动估计算法

开源算法

视觉slam开源算法

ORB算法：描述子做了取舍折中，特征匹配上做了提速。
ORB2开源地址

稠密建图：个人觉得对于位姿估计的精确度影响一般，但是会拖延运算速率。主要优点，人机交互好一些。
直接法：运算速度快，但是有灰度不变的假设，匹配上精确度会变差。

VINS mono是香港大学开源的算法，主要是基于单目+IMU紧耦合，实时性及准确度都高，但是静止时会出现漂移，初始化时要充分移动相机，获得足够的匹配信息(单目需要解析深度)。

VIO：视觉惯性里程计，在视觉SLAM中增加IMU传感器后，按照融合方式分为松耦合和紧耦合。松耦合是分别单独利用视觉和IMU传感器估计位姿，最后将求得的两个状态融合；紧耦合是将视觉和IMU的测量信息统一起来，构建运动方程和观测方程来估计位姿。

tagslam基于外部marker的视觉定位
https://github.com/berndpfrommer/tagslam_root

激光slam开源算法

gmapping技术特点
gmapping代码地址
slam_gmapping 是openslam_gampping在ros下的二次封装
openslam_gampping代码地址

基于粒子滤波，类似AMCL方法加上了同步建图功能。
运动估计，依赖里程计数据，并添加粒子高斯噪声
运动优化，点云匹配，根据匹配值，更新粒子的可信度，可信度最高的就是优化后的运动估计。
无回环
缺点：不适合用于大型场合(每个粒子都包含一个地图，大型场合，会造成计算的爆发增长)以及噪声比较多的场合(会出现粒子耗散问题)。

cartographer
paper：《Real-Time Loop Closure in 2D LIDAR SLAM》
官网介绍
cartographer开源代码地址
cartographer开源地址

运动优化，采用图优化方法，就是最小化估计位姿带入观测方程得到的估计观测值与实际观测值的差距。得到运动优化。
建图：采用子图匹配回环。分支界定法加速。

LOAM技术特点：
paper: 《Lidar Odometry and Mapping in Real-time》
LOAM介绍

LOAM地址
A_LOAM代码地址
LOAM_notes代码地址

注：文中通过hukuyo，装到电机上完成三维点云扫描，一次完整得扫描为一次sweep。一个sweep包含多个scan。雷达每次在一个位姿状态发射光束，这是scan，完成一个完整旋转，是sweep。

运动估计：高频粗略估计，根据IMU里程计或者运动连续性(下一时刻)。利用线性插值，对各个角度得激光束对应得机器人姿态角进行估计。
点云匹配：将点分为边线(edge)点和平面(planar)点，边线用两点确定，平面用三点确定。
将k时刻雷达坐标系下点云$P_k$，根据估计位姿，映射到k+1时刻的雷达坐标下${\overline{P}}_k$，在k+1时刻雷达坐标系下点云$P_{k+1}$，找到${\overline{P}}_k$最邻近点，线找两个，平面找三个点。点云使用kdtree结构存储，用来寻找最近邻的点。
运动优化：低频，这步计算量大
利用匹配好的点，建立约束方程，最小化点到线距离或点到面距离。得到优化的位姿。
建图：输出点云地图。

Lego-LOAM
相比LOAM加入点云聚类，去噪点，以及使用GTSAM作为后端的回环检测
地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git

LIO-SAM
优化包含LiDAR里程计因子，IMU预积分因子，GPS因子和回环因子来得到机器人的全局一致的位姿。
加入惯性融合，为"Lego-LOAM"的后续版本。

其他开源代码地址参考

当前的开源SLAM方案
GitHub 上优秀的开源SLAM repo

知识点

激光雷达的参数定义

分辨率：激光雷达输出的图像也被称为“点云”图像，相邻两个点之间的夹角就是角分辨率。
帧率：一幅点云图像代表一帧，对应到激光雷达内部就是电机旋转一圈完成一次扫描。帧率即代表一秒钟内激光雷达电机旋转的圈数，也就是每秒钟完成一圈扫描的次数，完成多少次扫描。
FOV :Field of view，雷达每scan能够覆盖的角度。
多线激光雷达：就是通过多个激光发射器在垂直方向上的分布，通过电机的旋转形成多条线束的扫描。
https://www.sohu.com/a/302268573_473608

深度摄像头会不会取代雷达

深度相机可以说是相机结合雷达技术的一个变种，但是相机主动式测距采用的光源，没有脉冲雷达的传输距离远，且由于波长与普通光源更接近，因为也更容易受干扰。
视觉导航是否会替代激光导航原理技术详解
深度摄像头，相当于目前已经商用化、最便宜、性能比较差的“激光雷达”。摄像头怎么成了激光雷达了，大家的共同点是通过测距实现三维空间感知。虽然叫摄像头，其实也是测距传感器，都属于主动光源测距传感器（发光，接收回光测量距离）。包括结构光和相位TOF两种原理。

可测深度摄像头TOF Camera 原理

结构光

Light Coding（编码光）的结构光技术，当光投射到物体表面时，不可避免将出现畸变。因此，通过检测畸变光线，便能够获得关于物体的3D信息。而后通过算法分析处理，最终便能够获得一张完整的3D结构图。
Light Coding技术虽然也是结构光的一种，但其投射的是“激光散斑”。散斑具有高度的随机性，并且距离不同，散斑的形状也不同。这也就相当于，当光源发射出散斑后，整个空间都已经被标记。因此，当有物体进入这个空间的时候，通过记录散斑的变化，就能够监测到物体的空间位置。

TOF

同结构光不同，ToF（Time of flight 飞行时间）技术首先会发射光脉冲，当光脉冲遇见障碍物时，必然会出现反射。距离不同，反射运动时间也不同。通过记录光脉冲的反射运动时间，推算出光脉冲发射器同目标物体的距离，并以此生成一张目标物体的3D信息图。
激光的距离测试也常采用TOF技术
ITOF(indirect time of flight)，间接测量光飞行时间，大部分的间接测量方案都是采用了测相位偏移的方法，即发射的正弦波与接收的正弦波之间的相位差，先算相位，再算时间，进而到距离。在iToF中，发射的并非一个光脉冲，而是调制过的光。接收到的反射调制光和发射的调制光之间存在一个相位差，通过检测该相位差就能测量出飞行时间，从而估计出距离。目前iToF的主要应用场景都是测距距离在数米范围的应用（例如手机等）
DTOF(direct time of flight)， 直接测量飞行时间解算距离，DTOF向目标发射一个光脉冲信号，直接测量光子从出发到返回的用时；ITOF发射一连串经过调制的光波，通过检测来回光波间的相位差测量飞行时间。dtof在很多关键性能方面对itof都有绝对优势，但其技术壁垒比较高。DTOF多使用激光雷达，多用于车载LiDAR。脉冲激光，比雷达利用的无线电波波长更短、频率更高，可以得到分辨率更高的图像，精度可达厘米甚至毫米级别。

激光/视觉点云区别

根据激光测量原理得到的点云，包括三维坐标（XYZ）和 激光反射强度（Intensity）。
根据摄影测量原理得到的点云，包括三维坐标（XYZ）和 颜色信息（RGB）