基于darpa subt洞穴仿真环境对比aloam和liosam

一.仿真环境搭建
darpa地下挑战赛洞穴仿真环境搭建参考如下链接：
https://github.com/osrf/subt_hello_world/blob/master/posts/01_hello_world.md
http://zhaoxuhui.top/blog/2021/03/25/subt-simulation.html#3安装subt

本次仿真环境选择洞穴环境simple_cave_02，机器人模型为X1_SENSOR_CONFIG_6，搭载一个相机，一个2D雷达以及一个3D雷达。
启动仿真环境界面如下：

打开rviz获取雷达点云和相机视角

二.数据录制
录制相关话题bag

三.aloam解读
3.1 框架

核心两部分，高频里程计和低频的建图，两种算法处理。里程计通过高频低数量的点云进行scan_to_scan匹配，估计出两帧之间的运动关系，输出结果给mapping算法；建图频率1hz，用scan_to_map的方式将去畸变的点云匹配并对齐到地图上。最后将两种算法发布的姿态变换集成，得到激光雷达姿态对于地图约10hz的变换输出。
3.2特征提取
核心是在点云中提取出两种特征点，分别是角点和面点，实际还有次角点和次面点等。
计算曲率的公式为：

基于曲率c值对扫描中的点进行排序，然后选择c最大的(即边缘点)和最小的c(即平面点)的特征点。为了在环境中均匀分布特征点，将扫描分为四个相同的子区域。每个子区域最多可以提供2个边缘点和4个平面点。只有当点i的c值大于或小于阈值，并且选定的点数不超过最大值时，才能将点i选作边缘点或平面点。
在选择特征点时，我们希望避免选择已经选择的点的临近点，或局部平面上大致平行于激光束的点(下图(a)中的点B)，因为这些点太近了，很不可靠。此外，我们还希望避免位于遮挡区域边界上的点(下图(b)中的点A)。

因此选择特征点时需要满足条件：
1.特征点数量满足子区域最大值，每个子区域的特征点数目是一定的。
2.特征点周围点不能再次被选择，本来每个子区域的特征点数量就比较少了，进一步防止特征点集中分布。
3.特征点非平行激光束的平面点，非被遮挡的平面点，非离群点。这些都是不可靠的点云数据，不能在连续两帧点云数据中得到，不能作为特征点。
3.3 点云去畸变
用里程计算法估计出激光雷达在一帧扫描范围内的运动。一帧扫描开始的时间是tk。在每次扫描结束时，得到扫描期间感知的点云Pk，将其重新投影到下一帧扫描时间戳tk+1，得到_{Pk，在下一次扫描k+1期间，}Pk与新接收到的点云Pk+1一起使用，以估计雷达的运动。

3.4 帧间匹配
LM方法，主要核心求点到线和点到面的距离，点到线距离公式如下：

两向量叉积的模代表四边形面积，再除以底边的模就是点到线的距离。
点到面的距离公式如下：

先用向量叉积求出平面法向量，点积求平行四边形面积。最后除以法向量的模得到点到面的距离。
四.rqt节点计算图
ALOAM相关节点运行和话题订阅方式如下：

五.地图
运行结束后完整地图如下：

六.分析评估
aloam地图分为all_map和surrond两种，all_map为全局地图，surrond为划分的当前网格区域内的地图，当前点云为彩色部分地图坐标系下点云。

特征提取部分主要提取出四种点云分别是，sharp，less_sharp, flat,less_flat，且全部在雷达坐标系下。
sharp和flat就是需要前一帧的 less_sharp， less_flat进行匹配，得到高频低精度的里程计。


最终less_sharp和less_flat会分别作为laserCloudCornerLast和laserCloudSurfLast成为laser_mapping的输入点云，并与全局地图匹配。

6.1 数据分析
下面四个这折线图分别是sharp，lesssharp，flat lessflat四种特征点每帧的数量分布情况。

由于角点提取是每帧每条线取12个点，所以每一帧的上限是192个点，且提取的时候为避免特征聚集，将选中的周围五个点都不做选择。次弱角点每帧每条线是120个点，其中包含了角点数，所以每帧次弱角点的的上限是1920个点。又图可以看出点数基本上分布在一个区间内波动。

面点的提取与角点方式一致，每帧每条线选24个点，上限384。但因为平面点数量较多所以提取的面点一致维持在最高上限，次弱面点就是面点加上剩下没有筛选过的点。

6.2 里程计

上图红色为scan_to_scan输出的10hz高频里程计，绿色为scan_to_map的1hz里程计。

七.总结
1、将里程计和建图分隔开，一个高频低精(处理每次帧数据)，一个低频高精(累积一定次数) 整理框架是串行结构，将整个问题逐步划分为多个层次。
2、运动补偿（时间戳）
3、融合了scan-scan scan-map
缺点：
1.无回环
2.无后端优化
3.不能处理大规模的旋转变换

LIO_SAM分析
一.框架
1.1 代码框架

1.2 系统框架

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

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

三.建图
lio_sam构建地图如下所示

相比较aloam建图精度看起来相差不大，区别在于lio_sam使用的大量的降采样，看起来比aloam轮廓清晰点。并且计算效率比aloam高。
3.1去畸变点云
去畸变后的点云如下:

这部分点云就是利用激光帧起止时刻间的imu数据计算旋转增量，imu里程计数据（经过雷达里程计约束后的里程计，这里相当于对里程计再次精修一下）计算平移增量。进而对当前帧激光雷达每一时刻的激光点进行运动畸变矫正（利用相对于激光帧起始时刻的位姿增量去畸变，这里aloam采用基于匀速模型按时间进行线性插值的方式）；
同时用imu数据的姿态角（rpy），imu里程计数据的位姿，对当前帧进行粗略的初始化。
3.2特征点分析
相比较aloam，liosam的特征点只分了如下角点和面点两种：
在去畸变后的点云中进行特征提取，每帧每条线提取120个角点，剩下的小于阈值的都是面点，其实就是把aloam的两个集成一个来处理，顺便剔除了一些不可信的点。如表面和激光线平行，遮挡的点等。

3.2 里程计分析
1.imu_incremental:使用gtsam进行imu与激光里程计进行紧耦合优化后发布的imu里程计数据。
2.odometry/imu：经激光里程计约束后的imu里程计。

3.lio_sam/mapping/odometry:全局激光里程计。
4.odometry_incremental:与 lio_sam/mapping/odometry基本一致，只是roll，pitch用imu数据加权平均了一下，z轴做了限制。

总结
1.紧耦合了多种传感器，运动估计更鲁棒
2.使用增量平滑和isam进行因子图优化。
缺点：
1.关键帧之间的特征被完全丢弃

EVO 对比里程计
一.准备工作
评估里程计数据之前，需准备保存aloam或liosam的里程计数据的txt文件（tum格式）

二.使用
evo工具主要有如下六个常用命令：
evo_ape - 用于评估绝对位姿误差；
evo_rpe - 用于评估相对位姿误差；
evo_traj - 这个主要是用来画轨迹、输出轨迹文件、转换数据格式等功能；
evo_res - 比较来自evo_ape或evo_rpe生成的一个或多个结果文件的工具；
evo_fig - （实验）工具，用于重新打开序列化图（使用–serialize_plot保存）；
evo_config - 这个主要用于evo工具全局设置和配置文件操作。

-p 表示绘图；
--plot_mode=xyz - 表示绘图模式为xyz三维；
--save_results – 表示保存结果，后面为文件保存路径及文件名。

三.评估数据
3.1 轨迹评估、
指令
evo_traj tum ALOAM.txt liosam_odom.txt -p

下图为aloam和liosam的轨迹对比，存在细微偏差。

通过下图xyz坐标对比发现偏差出现在z轴上。这是因为liosam利用imu的数据对z轴坐标进行了一定的约束。

欧拉角主要分布在roll和pitch，也就是坡道对里程计数据的影响。

3.2 误差评估
evo_rpe tum ALOAM.txt liosam_odom.txt -p