LIO-SAM

代码整体架构

LIO-SAM/
  config/
    params.yaml             # 参数配置
  include/
    utility.h               # 读取参数，提供一些工具方法
  launch/
    run.launch              # 启动文件
  src/
    featureExtraction.cpp   # 点云计算曲率，提取特征（角点、平面点）
    imageProjection.cpp     # 激光点云运动畸变校正
    imuPreintegration.cpp   # imu预积分因子图优化，计算每时刻imu里程计
    mapOptmization.cpp      # scan-to-map匹配，因子图优化，闭环优化

LIO-SAM则采用因子图优化方法，包含四种因子：IMU预积分因子，激光里程计因子，GPS因子，闭环因子。
变量节点是关键帧。相邻的关键帧之间，通过IMU数据计算预积分，获得位姿变换，构建IMU预积分因子。每个关键帧还有对应的GPS数据参与校正。如果有闭环出现，闭环帧之间可以构建约束。关键帧之间有若干普通帧，这些帧不参与图优化，但是会执行scan-to-map的配准，优化每帧位姿。

整体流程：

1. 激光运动畸变校正。利用当前帧起止时刻之间的IMU数据、IMU里程计数据计算预积分，得到每一时刻的激光点位姿，从而变换到初始时刻激光点坐标系下，实现校正。
2. 提取特征。对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点特征。
3. scan-to-map匹配。提取局部关键帧map的特征点，与当前帧特征点执行scan-to-map匹配，更新当前帧的位姿。
4. 因子图优化。添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿。
5. 闭环检测。在历史关键帧中找候选闭环匹配帧，执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入到因子图中一并优化。

utility.h

• imuConverter：将IMU数据的加速度和角速度数据从IMU坐标系旋转到Lidar坐标系(只旋转无平移)
• publishCloud：点云转ROS消息发布

ImageProjection 激光运动畸变校正

1. 利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
2. 同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。
   订阅

订阅

1. 订阅原始IMU数据；
2. 订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿；
3. 订阅原始激光点云数据。

发布

1. 发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；
2. 发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

FeatureExtraction点云特征提取

功能简介

对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）

订阅

1. 订阅当前雷达点云帧运动畸变校正后的点云信息，来自ImageProjection。

发布

1. 发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云、平面点云，发布给MapOptimization；
2. 发布当前激光帧提取的角点点云，用于rviz展示；
3. 发布当前激光帧提取的平面点点云，用于rviz展示。

ImuPreintegration IMU预积分

TransformFusion类

功能简介

主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。

订阅

1. 订阅激光里程计，来自MapOptimization；
2. 订阅IMU里程计，来自ImuPreintegration；

发布

1. 发布IMU里程计，用于rviz展示；
2. 发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。

ImuPreintegration类

功能简介

1. 用激光里程计，两帧激光里程计之间的IMU预积分量构建因子图，优化当前帧的状态(包括位姿、速度、偏置)；
2. 以优化后的状态量为基础，施加IMU预积分量，得到每一时刻的IMU里程计。

订阅

1. 订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预积分量，预测每一时刻(IMU频率)的IMU里程计；
2. 订阅激光里程计(来自MapOptimization)，用两帧之间IMU预积分量构建因子图，优化当前帧位姿(这个位姿仅用于更新每时刻的IMU里程计以及下一次因子图优化)。

发布

1. 发布IMU里程计

MapOptimization因子图优化

功能简介

1. scan-to-map匹配：提取当前激光帧特征点(角点、平面点)，局部关键帧map的特征点，执行scan-to-map迭代优化，更新当前帧位姿；
2. 关键帧因子图优化：关键帧加入因子图，添加激光里程计因子、GPS因子、闭环因子，执行因子图优化，更新所有关键帧位姿；
3. 闭环检测：在历史关键帧中找距离最近，时间间隔较远的帧作为匹配帧，匹配帧周围提取局部关键帧map，同样执行scan-to-map匹配，得到位姿变换，构建闭环因子，加入因子图优化。

订阅

1. 订阅当前激光帧点云信息，来自FeatureExtraction；
2. 订阅GPS里程计；
3. 订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。

发布

1. 发布历史关键帧里程计；
2. 发布局部关键帧map的特征点云；
3. 发布激光里程计tf变换，rviz中表现为坐标轴；
4. 发布激光里程计；
5. 发布激光里程计路径，nav_msgs/path；
6. 发布地图保存服务；
7. 发布闭环匹配局部关键帧map；
8. 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云；
9. 发布闭环边，rviz中表现为闭环帧；
10. 发布局部map的降采样平面点集合；
11. 发布历史帧(累加的)的角点、平面点降采样集合；
12. 发布当前帧原始点云配准之后的点云；

ubuntu20.04使用LIO-SAM

1. 在utility.h中将 #include <opencv/cv.h> 替换为#include <opencv2/opencv.hpp>
2. 在CMakeLists.txt 中将set(CMAKE_CXX_FLAGS "-std=c++11")替换为set(CMAKE_CXX_FLAGS "-std=c++14")
3. 如果我们在一些opencv标头之后包含了flann标头并使用了g++ -std>=c++11，那么序列化代码中就会出现一个神秘的错误。 /usr/include/flann/util/serialization.h:34:14: error: ‘class std::unordered_map<unsigned int, std::vector<unsigned int> >’ has no member named ‘serialize’
   在std≥C++ 11的情况下，似乎opencv头文件泄漏#define USE_UNORDERED_MAP 1。然后由原始flann库中的lsh_table.h中的#if获取。
   解决方案是在OpenCV之前包含flann。在utility.h中将 #include <opencv2/opencv.hpp> 头文件放在pcl库头文件之后。
4. GTSAM和EIGEN的版本对应问题
   GTSAM自带的eigen和系统安装的eigen之间有冲突。GTSAM编译的时候默认使用了自带的eigen，而系统中如果还手动安装过一个eigen的话，就会出现两个eigen的冲突。
   在GTSAM的CMakeLists.txt文件中的判断使用哪个EIGEN指令前边添加如下代码：set(GTSAM_USE_SYSTEM_EIGEN ON)
   这样就相当于GTSAM编译也使用了系统eigen。
5. error while loading shared libraries:libmetis-gtsam.so: cannot open shared object file
   解决方法：libmetis-gtsam.so这个文件位于路径/usr/local/lib中，而程序默认寻找的动态链接库位置是/usr/lib
   只需在.bashrc中加入如下命令增加动态链接库路径即可：

   export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
   

速腾robosense运行LIO-SAM

1. 需要使用rs_to_velodyne将robosense雷达点云格式转换为velodyne点云格式
   修改参数文件param.yaml

pointCloudTopic: "/velodyne_points"
imuTopic: "/imu/data"  

lidarFrame: "velodyne"
baselinkFrame: "base_link"
odometryFrame: "odom"
mapFrame: "map"

useImuHeadingInitialization: false           # if using GPS data, set to "true"
useGpsElevation: false