3D激光SLAM(原理 + 公式 + 代码 理解)

总结: 

3D SLAM 的大体流程:(以多传感器融合为例)

(1) 首先进行传感器信息的读取: 包括激光雷达 . IMU(IMU 传感器可以提供 线加速度, 角速度 以及 旋转角RPY 信息)

(2) 扫描匹配:  对得到的数据进行畸变处理,校正位姿, 对得到的相邻两帧点云 计算位置变换矩阵和平移矩阵,从而进行配准, 使得相邻的点云相同部分可以重叠.  得到一个局部的点云地图.这个过程中还要进行下采样处理,过滤掉无关的点云.

(3) 精确定位: 对得到的局部地图进行优化, 使其能够得到一个完整的地图. 这里就需要进行相应的后端优化, 进行局部地图的配准, 位姿矫正, 同时也要进行回环检测, 判断是否之前经过某个位置, 从而得到全局一致的轨迹 和 地图. 这个过程中还要进行下采样处理,过滤掉无关的点云.

(4) 构建地图: 根据估计的轨迹以及自身的位姿信息 建立未知环境下的地图.

利用GPS GNSS 传感器融合 目的就是为了校正 路径漂移,使得最后的路径结果能尽可能与真实的路径重合, 使漂移降到最低.

网图简单理解:

各种库:

Ceres:   SLAM本质剖析-Ceres - 古月居

Eigen:  SLAM本质剖析-Eigen - 古月居

值得注意的是Eigen函数库是不存在动态链接库的，我们在CMakeList.txt编译时候只需要引入include即可。[这篇博客关于Eigen 矩阵的函数 和 操作讲的挺清楚的]

G2O:  SLAM本质剖析-G2O - 古月居

相较于Ceres而言，G2O函数库相对较为复杂，但是适用面更加广，可以解决较为复杂的重定位问题。Ceres库向通用的最小二乘问题的求解，定义优化问题，设置一些选项，可通过Ceres求解。而图优化，是把优化问题表现成图的一种方式，这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。

Sophus::  SLAM本质剖析-Sophus - 古月居

李群 李代数  Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。

PCL:   SLAM本质剖析-PCL - 古月居

代码详解 :::: 信息的转换 ,  KDtree ,  Octree , 滤波,   三角化 , 分割 , NDT 配准,  

Gtsam: SLAM本质剖析-GTSAM - 古月居

Scan Context回环检测解读和使用 - 古月居    基于SC  的回环检测

SLAM之划窗优化 - 古月居    滑动窗口

​​​​​​【math】 向量运算：叉乘_阳光快乐普信男的博客-CSDN博客_向量叉乘

点到直线距离以及雅克比矩阵计算见 泡泡机器人 文章

6-DoF相关基础知识整理_WangKingJ的博客-CSDN博客

IMU 里程计估算 对应的LOAM 代码：

点云运动畸变矫正