LIO-SAM是基于因子图构建的激光雷达惯性里程计，可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入激光雷达惯性里程计系统中。IMU预积分的运动估计被用来去处激光雷达运动畸变，并为激光雷达惯性里程计的优化提供初值。获得的激光惯性里程计的结果反过来用作估计IMU的偏差。为了确保实时性与高性能，进行位姿优化时边缘化掉了一些旧的激光雷达数据，而不是将激光雷达点云与整个地图进行匹配。在局部范围而不是全局范围进行扫描匹配可以有效提高系统的实时性，选择性地引入关键帧和高效的滑动窗口也能提高实时性能。

局限性：LIO-SAM对输入数据精度较为敏感，由于采用局部范围扫描匹配，导致在局部过窄或特征不足时，出现建图偏移现象。

3. 什么是点云降采样

三维点云往往包含大量冗余数据，直接处理计算量大，消耗时间长，因此对其进行降采样是十分必要的。降采样同时也是点云预处理过程中的关键环节。采样算法里，比较常用的是体素网格下采样。

3.1 体素网格下采样原理

体素（Voxel）：将三维空间划分成一个个立体的方格，每个方格就叫一个体素。

在每个体素中可能存在几个点，也可能没有点。降采样的思路为：检查每个体素中是否有点存在，若有，则用一个点代替体素内的点集，通常，这个采样点可以是体素中所有点坐标的平均值（质心），也可以是中心点或者离中心点最近的点。

3.2 流程

计算点云的包围盒，将包围盒离散成小体素。体素的长宽高尺寸可以通过用户设定，也可以指定三个方向上的体素个数；
获取落在每个体素中的点集，在每个体素中取一个采样点代替原来的点集。

3.3 特点

效率高
采样点分布比较均匀
可以通过控制体素大小间接控制采样点的距离（采样后点云的稀疏程度）
采集到的点云数量不可控

3. 基于Autoware.AI的ndt

Autoware 建图用的坐标系之间的关系，Autoware 用的坐标系有四类：世界坐标系 world，地图坐标系 map，小车坐标系base_link，传感器坐标系 velodyne，它们之间的关系如图所示：

从 world 到 map 的坐标系转换与从 base_link 到 velodyne 的坐标系转换是固定的，用 ROS 的 TF 即可。从 map 到 base_link 的映射就需要正态分布变换（NDT）算法，autoware 在建图采用的是 ndt_mapping。

4. 建图环境

Lidar：雷神16线（格式与velodyne一致）
IMU：H30
底盘：WheelTech阿克曼底盘
OS：Ubuntu 20.04
Autoware：autoware.ai docker

二、准备工作

1. 编译Autoware.ai

由于Autoware.ai 1.x版本源码编译只支持到Ubuntu 18，手上已经没有这个版本的设备，所以这里直接在Ubuntu 20.04下用docker环境来编译。具体方法不再赘述，详见：

Autoware—AI版本的部署

2. 传感器数据

需要准备以下3个topic的输出

2.1 雷达：/points_raw

Autoware 默认使用 velodyne 雷达，默认订阅的雷达话题为/points_raw，雷达话题的 frame_id 为 velodyne。使用雷达驱动包时，雷达话题名称与 frame_id 必须与上述的一致。帧率20hz。

2.2 IMU：/imu_raw

默认订阅的IMU话题为/imu_raw，帧率200hz。

2.3 小车里程计：/vehicle/odom

Autoware 中 Localization 自定位模块使用正态变换算法来匹配雷达当前帧和3D 地图来实现定位，同时也可以勾选使用里程计来辅助定位，默认订阅里程计话题为/vehicle/odom 话题，使用到里程计定位时，需发布话题为/vehicle/odom 的里程计话题，帧率20hz。

2.4 RTK：/gps/fix

G70+4GDTU插入上网卡并配置千寻账号后，可以输入厘米级的RTK数据，发布的话题为/gps/fix，帧率5hz。

三、录制ROSBAG

1. 启动底盘+传感器+Autoware

# 1. 启动底盘+传感器
roslaunch turn_on_wheeltec_robot open_autoware.launch

# 检查topic帧率
rostopic hz /imu_raw /points_raw /vehicle/odom /gps/fix /tf /camera/rgb/image_raw 
# 检查topic时间戳是否正常
rostopic echo /imu_raw /gps/fix -p

# 2. 启动Autoware
cd ~/autoware.ai/
./open.sh
source ~/Autoware/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

2. 录制 ROSBAG

录制 ROSBAG 主要是为了后面的建图以及生成导航的路径点文件需要。Autoware 建图方式是利用 rosbag 包进行离线建图，因此我们先录数据（注：录制的过程中，适当控制车速，尽量避免压坑或车身剧烈抖动，会影响建图效果）。

需要录制的话题列表：

IMU话题 /imu_raw
雷达话题 /points_raw
里程计话题 /vehicle/odom
RTK话题 /gps/fix

在 Autoware 界面中，点击[ROSBAG]按钮，在弹出来的界面中点击[Ref]选择 ROSBAG 的存储路径（docker 安装方式的注意保存在 autoware_shared_dir 目录下面），然后勾选话题，点击“start”即可开始录制。

点击开始录制后，可以通过底盘遥控或ros键盘控制命令控制小车移动。

也可以使用命令录制：

# rosbag录包（用于离线建图）
 rosbag record /imu_raw /points_raw /vehicle/odom /gps/fix /gps/vel /tf /camera/rgb/image_raw

录制完成后点击[stop]按钮便可结束录制，录制的 bag 文件会自动保存。

*注：如果底盘里程计不精准（例如无法走直线），则不用录制/vehicle/odom，后续建图时可以通过实时rtk+imu融合生成odom供ndt使用。

录制后，必须关闭Autoware和底盘程序，否则会影响接下来的建图。

四、使用ROSBAG建图

1. 重启Autoware

播包不能与底盘topic同时输出，所以这里需要把底盘节点关闭掉，把Autoware节点重启一下：

# 重启Autoware
cd ~/autoware.ai/
./open.sh
roslaunch runtime_manager runtime_manager.launch

2. 加载从base_link到Lidar坐标系的TF、小车模型

点击进入【Setup】菜单，分别设置Localizer、TF、Vehicle Model：

在【Localizer】中选择Velodyne。
在【Baselink to Localizer】中设置好各个参数，其中 x、y、z、yaw、pitch、roll表示实车雷达中心点与车身后轴中心点的 TF 位置关系，可根据雷达实际位置进行修改，设置好之后点击【TF】按钮。
- x：Lidar中心点到后轴中心点的前后距离差，单位为米。Lidar相较后轴更超前安装为正值，往后为负值。我这里实际测量是0.26
- y：Lidar中心点到轴中心点的左右距离差，单位为米。Lidar相较后轴中心点往左安装为正值，往右安装为负值。因为我的Lidar左右是居中安装的，所以这里值为0
- z：Lidar中心点距离轴中心点的高度差单位为米。我这里加高前Lidar距地面高0.298，加高后距地面0.438，减去前/后轴中心点距离地面高度0.063，所以用加高前采的数据建图应设置为0.235，用加高后数据应设置为0.375
- yaw：对于镭神 16 线雷达参数先固定设置成-1.57，后便有空时试下改为0是否有影响
docker版本的urdf小车模型在 autoware_shared_dir/urdf 目录下面，选择后点击【Vehicle Model】加载模型（如果Vehicle Model为空，那么会加载一个默认模型，在 rviz 显示时，如果有激光雷达数据，车辆会显示为黑色）。

3. 设置从 world 到 map 转换

点击【Map】菜单，加载 world 到 map 的坐标转换：

点击【TF】右侧的【ref】按钮，选择~/autoware_shared_dir/lgsvl-tf.launch 文件后
点击【TF】按钮加载该文件，以加载 world 到 map 的坐标转换

cat ~/autoware_shared_dir/lgsvl-tf.launch
    <launch>
        <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />
    </launch>

4. 配置并启动ndt-mapping（map到 base_link的转换）

在【Compulting】菜单栏找到【lidar_localizer】下的【ndt_mapping】选项，先点击其右侧的【app】设置一些参数（对 app 内的参数数据修改完成时，必须按回车键，左侧的滑动栏位置发生变化后数据才能保存成功）。

Ndt_Mapping 有一些参数设置需要注意一下：

Resolution：ndt 分辨率，最大建图精度，默认为 1 即可，室外范围较大时建议设为2
Step size：步长，默认为 0.1
Transformation epsilon：两次正态变换允许的最大值，默认为 0.01 米
Maximum iterations：最大迭代次数，默认为 30 次
Leaf size：点云降采样用的体素叶大小，室内可设置小点，室外可设置大点1-2米
Minimum scan range：设置雷达最小范围，过滤较近处雷达点云，推荐设置小点：0.15 米
Maximum scan range：设置雷达最大范围，过滤较远处雷达点云，默认 200米
Maximum add scan shift：前帧距离上一个关键帧位移超过阈值则认为当前帧为关键帧，默认1米
在[method type]栏中，勾选[pcl_anh]选项（选其中一个，最终节点能成功运行即可。运行失败可以尝试勾选其他选项。目前测试使用[pcl_anh_gpu]无法显示建图进度，用[pcl_anh]选项正常）。
如果要用到里程计或imu辅助定位的，把[use odometry]、[use imu]选项勾选上。（*注：很多底盘直行时侧偏的厉害，或者底盘行驶中出现打滑的情况下，建图时都不要使用底盘的odom里程计，会徒增干扰导致地图偏移。只使用200hz的imu会比底盘odom里程计精准）

设置完参数点击【Close】按钮后，勾选【ndt_mapping】以启动ndt建图模块。

5. 加载点云数据.bag文件

点击Autoware的【Simulaton】 Tab页，点击界面右上方【Ref】按钮，加载上一步骤录制的 bag 文件。
点击【Play】按钮开始播放数据，此时右边会显示播包进度条，在命令行窗口也可以看到ndt的建图进度。
也可以手动播包：（如果有回环位置有交错的情况，可以检查tf是否准确，并适当去掉一部分刚开始采集的数据）

rosbag play autoware-20241124082629.bag -r 0.1 -s 60 --duration=1796

命令行最下边的建图进度部分，左边数字为已处理的数据，右侧的数字为总的数据。
如果是嵌入式设备，二者相差太大程序有可能未处理数据积攒太多导致卡死，推荐相差 200 左右就按下【Pause】按钮暂停播放数据，等待当前数据处理完成就再次点击【Pause】按钮继续播放数据即可。
如果采集的地图范围非常大，例如3-4km以上，bag包左下方的Rate回放的速度建议设置为0.1倍速，可以避免出现积攒太多未处理的数据导致std::bad_alloc错误。如果积攒的太多，必要的话可以点击【Pause】暂停，等待处理跟上后再继续播放处理。
如果系统负载很低则可以忽略，ndt算法看起来并行处理性能上不是很好，耐心等待建图完成即可。

7. 保存 3D 点云地图

7.1 检查建图进度

等待点云数据加载完毕，命令行停止打印相关日志时，建图过程即完成：

7.2 保存PCD地图

回到【Compulting】菜单栏的【ndt_mapping】选项，点击其右侧的【app】，在【ref】中选择点云地图保存的路径后，点击【PCD OUTPUT】按钮即可保存点云地图（注意：docker版本的需要保存在~/autoware_shared_dir目录下面）。

也可以写个脚本订阅ndt_map的topic保存地图：

$ vim ~/Autoware/src/autoware/utilities/data_preprocessor/scripts/save_pcd.py

#!/usr/bin/env python3
# 自动定时保存ndt_mapping地图到pcd文件
#   python3 save_pcd.py ${topic_name} ${save_path}
#   python3 ~/Autoware/src/autoware/utilities/data_preprocessor/scripts/save_pcd.py /ndt_map /media/work/FelixSSD/autoware/autoware_shared_dir/bag/map-6-park/pcd
#

import sys
import os
import time
import rospy
import numpy as np
import cv2
import pcl
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from cv_bridge import CvBridge

class SavePcd:
    def __init__(self):
        self.save_path = None
        self.filter_resolution = 0.2
        self.save_count = 0

    def save_pcd(self, msg):
        filename = self.save_path + '/pcd' + '_' + "{:.5f}".format(time.time()) + '.pcd'
        print("save_pcd start ", filename)

        cloud = pcl.PointCloud(np.array(list(pc2.read_points(msg)), dtype=np.float32)[:, 0:3])
        if self.filter_resolution > 0.0:
            sor = cloud.make_voxel_grid_filter()
            sor.set_leaf_size(self.filter_resolution, self.filter_resolution, self.filter_resolution)
            cloud_filtered = sor.filter()
            cloud_filtered.to_file(filename.encode())
        else:
            cloud.to_file(filename.encode())

        self.save_count += 1
        print("save_pcd done!")

    def run(self):
        try:
            self.topic = sys.argv[1]
            self.save_path = sys.argv[2]
        except Exception as e:
            sys.exit("Example: save_pcd.py topic_name save_path")

        rospy.init_node("save_pcd_node", anonymous=True)
        rospy.Subscriber(self.topic, PointCloud2, self.save_pcd)
        # rospy.spin()
        while True:
            if self.save_count > 0:
                break

if __name__ == '__main__':
    pcd = SavePcd()
    pcd.run()

写个shell定时保存：

$ vim ~/save_pcd.sh 
set -x
set -e
echo $USER
source /home/work/.bashrc
python3 /home/work/Autoware/src/autoware/utilities/data_preprocessor/scripts/save_pcd.py /ndt_map /media/work/FelixSSD/autoware/autoware_shared_dir/bag/map-6-park/pcd

$ crontab -e
*/10 * * * * /bin/bash /home/work/save_pcd.sh >> /tmp/save_pcd.log 2>&1

7.3 查看PCD地图

保存完地图之后，可以通过pcl_viewer查看当前建图效果。

cd ~/autoware_shared_dir/bag/map-2/
pcl_viewer autoware-241118.pcd

五、使用ROSBAG/PCD生成路径点

1. 重启Autoware

同上一章

2. 加载从base_link到Lidar坐标系的TF、小车模型

同上一章

3. 设置从 world 到 map 转换

同上一章

4. 加载点云地图 PCD 文件

打开【map】页面，点击【point cloud】右侧的【ref】按钮，选择上一步建图保存的3D点云地图.pcd文件
点击【point cloud】按钮，此时下方会出现一个进度条，当进度条显示加载了 100％并出现[OK]字样时，证明 3D 点云地图已经成功加载完毕

5. 点云降采样过滤

打开【sensing】页面，找到【points_downsampler】下的【voxel_grid_filter】选项，设置[app]的一些参数后，勾选[voxel_grid_filter]。 [voxel_grid_filter]有一些参数设置如下图：

Voxel Leaf size：体素降采样算法中的体素叶大小，室内可设置小点，室外可设置大点

6. 启动ndt_matching（从 map 到 base_link 的转换）

找到【Computing】左菜单栏下的【ndt_matching】选项，设置【app】里面的参数后，勾选【ndt_matching】，一些参数设置如下所示：

使用RTK时，【topic:/config/ndt】勾选【GNSS】。没有使用 GNSS时，【topic:/config/ndt】勾选【Initial_Pose】，x，y，z，roll，pitch，yaw 的值表示小车的初始位置（默认建图起点位置）。
在[method type]栏中，无 GPU 加速的选【pcl_generic】，有 GPU 加速的选[pcl_anh_gpu]。
用到里程计和 imu 辅助定位，需要把【use odometry】和【useimu】选项勾选上，其余参数参考默认值就可以了。

7. 启动[vel_pose_connect]

找到【Computing】左菜单栏下的【vel_pose_connect】
打开【app】并确保选项 [Simulation_Mode] 没有被勾选。
退出并勾选【vel_pose_connect】。

8. 设置路径点文件保存路径

找到【Computing】右菜单栏下的【waypoint_saver】，打开【app】。
点击【ref】选择保存的路径（注意： docker 版本的需要保存在~/autoware_shared_dir 目录下面），勾选上【save/current_velocity】。
退出并勾选【waypoint_saver】。

9. 加载点云数据.bag文件

点击Autoware的【Simulaton】 Tab页，点击界面右上方【Ref】按钮，加载上一步骤录制的 bag 文件。
点击【Play】按钮开始播放数据，等待数据包回放结束，路径点文件会自动保存在设置好的路径下。

11. rviz 查看路径点

在路径点生成过程中，源码安装方式的可以点击界面右下角的[rviz] 打开rviz，相关配置在~/autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo/default.rviz 中（docker 版本的需要在小车上运行 rviz 单独打开，相关配置在~/autoware_shared_dir/default.rviz 中）。此时我们可以看到 rviz 中逐渐出现一条路径：