背景
- 传统的激光雷达是通过 ICP（Iterative Closest Point）算法去找到两个连续 lidar 帧（点云）之间的相对变换来实现的。
- ICP 是一种迭代优化算法，输入为两组点云数据（源点云和目标点云），输出为两组点云之间的刚体变换（旋转矩阵 R 和平移向量 T）。
- ICP 能用来估计相邻雷达帧之间的相对运动，通过点云配准计算出两帧之间的旋转和平移，能有效解决帧间点云的对齐问题，但当帧与帧之间的变换角度过大时，ICP 可能难以收敛。
- 并且由于激光雷达在采集点云时会旋转，点云会发生倾斜，当使用倾斜的点云特征进行配准时，会导致较大的漂移。
- 因此，激光雷达通常与其它传感器（如 IMU、GPS）结合，用于状态估计和地图构建，即传感器融合。其融合方式又分为两种：松耦合融合和紧耦合融合。
- 松耦合融合：各传感器独立运行，通过其整体的输出（如 IMU 的位姿估计、GPS 的全局位置）进行融合，即先融合各个传感器的最终结果，再用简单加权或滤波方式进行综合计算。
- 紧耦合融合：各传感器的底层原始数据（如 IMU 加速度、GPS 伪距、激光点云）直接参与联合优化，实时生成整体状态，其融合精度和鲁棒性更高。
LIO-SAM 算法工作机理
2.1 点云去畸变处理
- 激光雷达数据的一个重大问题是点云中的时间差引起的畸变，LIO-SAM 通过 IMU 数据来修正扫描点的时间偏移（通常称为点云去畸变）。具体步骤如下：
- 提供点时间戳：使用 IMU 数据进行点云矫正，需要提供相对扫描点的时间。
- 提供点环号：环号用于将点分配到激光雷达的通道中，组织成矩阵结构。
2.2 因子图优化
- 因子图主要是用于对多传感器融合后的状态估计问题进行建模，LIO-SAM 算法的因子图构建主要使用 1 种变量类型和 4 种因子类型。
变量类型：表示机器人在特定时间的状态，并分配给图的节点。
因子：
- IMU 预积分因子：用于使用来自 IMU 的测量值来推断机器人的运动，主要解决 IMU 噪声和偏置问题。
- Lidar 里程计因子：用于特征提取、关键帧选择和相对变换。
- GPS 因子：系统在长时间导航任务中会收到漂移的影响，因此需要使用提供绝对测量值的传感器（如 GPS）来消除漂移。
- 回环闭合因子：采用基于欧几里得距离的环路闭合检测方法。
仿真操作
3.1 环境准备与配置
- 所需功能包：解压特定的 LIO-SAM ROS2 压缩包到 ubuntu 系统中，src 路径中包含算法包、机器人控制包、Gazebo 仿真包和 Velodyne 模拟器包。
- 配置文件修改：
params.yaml：确保点云 (Point Cloud) 和 IMU 的话题名称 (Topic Name) 与仿真模型 SDF 文件中定义的一致。修改 LiDAR 和 Base Link 的坐标系名称。设置 savePCD 为 true 以便仿真结束后保存地图。SDF 文件：确认 Velodyne 传感器的定义，确保话题名称与 params.yaml 中一致。
3.2 安装依赖
3.2.1 手动下载源码包
- 访问 GTSAM 官方下载页面获取源码。
- 下载并解压至本地目录。
3.2.2 准备编译环境
现在我们需要把这个解压出来的文件夹搬到主目录，并准备编译。请在终端中执行：
一。增加虚拟内存 (Swap) - 防止编译崩溃
# 创建 4GB Swap 文件
sudo fallocate -l 4G /swapfile
600 /swapfile
mkswap /swapfile
swapon /swapfile
free -h