激光slam理论与实践
本篇是记录曾书格老师的课程《激光slam理论与实践》
先贴一下个人总结（有理解的不正确的，麻烦指出来）：
第一章：激光SLAM简要介绍
1、输出Metrical map尺度地图，slam分为两种：基于滤波的 filter-based 的SLAM，和Graph-based的SLAM。
2、
（1）基于Graph-based的代表是cartographer，可以修复 t 时刻之前的误差分为两部分Front-end ，和Back-end。
（2）基于PF的代表gmapping，只能估计X(t)当前时刻的位姿，不能修复之前时刻产生的误差。在大面积的情况下，就无法建图了。粒子滤波实际上是一个贝叶斯估计的过程，其流程是一个状态预测的过程，分为6步：
卡尔曼是贝叶斯在线性高斯的特殊情况。
（3）
实际中，IMU算角度，里程计算距离
（4）帧间匹配算法：
ICP 是计算点对点的，没人用了。而PI-ICP是计算点对线的。
NDT用于三维的slam。
CSM：是最主要的方法。
scan-scan已经没人用了；
scan-to-Map代表是cartographer。
Map-to-Map还没有用，但是效果最好。与X(t)时刻之前的几帧进行回环检测。
3、2D激光SLAM（用的2D的激光雷达）
（1）2D激光slam的帧间匹配
（A）PI-ICP
（B）CSM(Correlation Scan Match)
（C）梯度优化的方法：Hector—SLAM
（D）CSM+梯度优化：cartographer
（2）2D 激光SLAM的回环检测方法
（A）分支定界Branch and Bound&lazy Decison（延迟决策） ，代表：cartographer
（B）Scan-to-Map
（3）
（A）EKF-SLAM构建的是feature 地图，而不是grid栅格地图，所以无法用于导航，只能用于定位。
（B）gmapping是Grid版本的FastSLAM，加入了scan-match。
（C）在Optimal RBPF在gmapping上做了优化，是MRPT上的一个开源算法。
2010年之后就由FP 转化为 图优化的方法了。
Karto SLAM 运行时经常崩溃。所以在此基础上Catographer作了优化。Catographer是所以开源算法中效果最好的。
（4）、数据的预处理
4、3D激光雷达
（1）3D激光雷达的帧间匹配
（A）Point-to-Plance ICP 点到面的匹配
（B）Feature-based Method
第二章：传感器数据处理I：里程计运动模型及标定
1、里程计运动模型
（1）两轮差分底盘的运动学模型
（2）航迹推算（Dead Reckoning）
2、里程计标定
（1）线性最小二乘的基本原理
通常就是 求解一个近似解。
（2）最小二乘的直线拟合
（3）最小二乘在里程计标定中的应用：
a，直接线性方法，精度不高，但实现简单；用这个方法
b，基于模型的方法，精度高，但是现实起来相当复杂，是一个非线性的过程。不用。
作业：实现一个直接线性方法的里程计标定模块
小结：由于机器人结构的系统性误差（比如轮子直径的大小不一样，机械结构缝隙等），所以需要坐里程计标定，达到去除系统误差的目的，而随机误差无法去除。
第三章：传感器数据处理II：激光雷达数学模型和运动畸变去除
1、概念
（1）激光lidar介绍：基于三角测距原理，距离越近，精度相对越高。而飞行时间（TOF）远距离时精度才可以，价格昂贵。高端的lidar有德国的sick，日本的北阳。
固态lidar：是一个发展趋势，价格低，类似于深度相机了，100线——300线。
如果lidar的帧率小于10HZ的话，那么运动畸变是非常明显的。
（2）激光lidar数学模型介绍：
光束模型：一帧激光与地图的重合程度，计算量太大，现在已经不用了。
似然场模型：对图像进行高斯平滑，计算量低。应用的多。
（3）运动畸变介绍：
产生的原因：在获取一帧激光lidar的数据用时 t 时，机器人的位置发生了运动，导致每个激光点的基准位置不是固定的。
2、畸变去除方法
（1）纯估计方法：ICP 的方法，就是点云匹配。
考虑到机器人的运动，采用VICP的方法。（Velocity estimate ICP）
（2）传感器辅助方法：（IMU/odom）
odom,直接测量位移和角度。更新频率（100Hz——200Hz），而IMU测量的是加速度，还需要积分。
二次曲线的近似：一帧激光数据N个激光点，每个激光点对应的位姿{P1，P2 ... Pn}通过这种方法得到，把转换后的激光数据发布出去。
作业：实现一个激光雷达运动畸变去除模块
小结：由于机器人存在运动畸变，所以要发布publish经过去除畸变的激光lidar数据。
第四章：激光SLAM的前端配准方法
帧间匹配方法：
（1）ICP 匹配方法：点到点的距离作为误差，一阶收敛。
（2）PL-ICP方法：点到线的距离作为误差，二阶收敛，精度比ICP更高。
（3）基于优化的匹配方法(Optimization-based Method）：应用于Hector SLAM 和cartographer
梯度的优化：Hector SLAM，不需要里程计，纯靠优化的方法。
cartographer 是CSM+优化。
（4）相关匹配方法和分枝定界加速：Karto SLAM，cartographer，分枝定界在相关方法中的加速作用。
第五章：基于滤波器的激光SLAM方法(Filter-based)
1、（1）只估计当前时刻的状态X(t)叫滤波。视觉地图——feature特征地图；激光——》栅格地图
2、
（1）Bayes滤波：是一类方法的统称，卡尔曼家族和PF都属于贝叶斯滤波。
（2）粒子滤波：PF是Bayes滤波的特例。应用于物体跟踪，机器人定位，SLAM导航。用一系列的粒子近似概率分布。
有一个重采样的过程，即去除权重小的粒子。权重表示和地图的匹配程度。
重采样的目的：生成后验分布的粒子群。总粒子数不变。
对于SLAM，存在粒子耗散问题，且无法解决，所以在环境小的时候才有用。
（3）FastSLAM：是gmapping的基本原理，也是基于PF的。存在三个问题和优化a,b,c：
a.每个粒子都包含自己的栅格地图，占用较大的内存；保持粒子数量较小，提升proposal采样的位姿质量；
b.粒子耗散问题；缓解粒子耗散；减少重采样的次数；
c.在a的基础上再优化，把proposal限制在一个较小的区域
（4）Gmapping：以FastSLAM为基础。在fastSLAM的基础上实现了优化a和b，没有使用优化c。
主要是两个函数: addScan( ) 和 processScan( )
第六章：基于图优化的激光SLAM方法(Grid-based)
1、Grid-based 是基于非线性最小二乘原理的求解。Grid-based包含前段Front-end和后端Back-end。
2、非线性最小二乘：f(x) = z ，f(x)是一个非线性的函数 ，z可以是一个预测值，然后通过传感器可以得到一个测量值z'，根据二者就可以去非线性最小二乘。
3、有回环检测。
4、高斯牛顿方法
5、cartographer方法中mapping_2d目录：
第七章：基于已知定位的建图
位姿估计包含 帧间匹配和优化的方法，两者构成图优化，通过对机器人的位姿（x ，y ， θ）来构建地图。
1、地图分类：（栅格地图、特征地图和点云地图，都属于）尺度地图、拓扑地图、语义地图
2、覆盖栅格建图算法（gmapping里面用的这个）：
栅格地图的数学模型：给定机器人的位姿（由里程计信息得到）和激光lidar的数据，来估计出栅格地图。
地图估计：栅格是一个二元随机变量。1 标准占用，0表示没被占用。
激光穿过的栅格为free，激光击中的栅格为occupied
3、计数（count model）建图算法：
激光击中的次数 / 激光穿过的次数 到达一个比值，就表示障碍物，occupied。
第八章: 3D激光SLAM介绍
1、3D激光SLAM
2D激光SLAM和3D激光SLAM的区别：
（1）2D ：单线激光lidar，只能在一个截面，形成2D栅格地图，室内机器人
（2）3D：多线激光lidar，形成三维点云地图或者三维栅格地图，室外移动机器人
2、3D帧间匹配：
（1）点面ICP
(2) Feature-based method
(3) NDT 方法 normal-distribution-Transform
2、LOAM (lidar odometry and mapping)
（1）是3D 激光SLAM算法，只有匹配，没有回环。
（2） KITTI数据集Odometry排行榜上长期霸占第一