SLAM源码修改版全解析：从编译到精度评估

简介：面向深蓝学院教学与科研需求定制的高博《自动驾驶与机器人中的SLAM技术》源码修改版，将书中理论与可运行的C/C++代码实现逐一对应，适合正在学习视觉里程计、后端优化、回环检测、建图与定位的自动驾驶和机器人方向读者，也适合希望从零搭建实际SLAM工程的研究者。压缩包共1923个文件、约235.37MB，其中.h头文件与.cpp源文件超过1200个，构成核心算法主体；另有txt说明、cmake/makefile构建脚本、yaml配置、py工具、pcd点云数据等，便于按章节复现实验并开展二次开发。已有926人学习下载，内容与深蓝学院课程要求同步，使用价值更贴近实际教学。通过研读和修改源码，读者既能理解SLAM前后端模块的实现细节，也能掌握从传感器数据处理到地图构建的完整工程链路，整体结构完整。

1. 先别急着clone：这版SLAM源码修改版解决的是“看得懂”的问题

高博新书《自动驾驶与机器人中的SLAM技术》配套源码，很多同学已经在GitHub上见过原版仓库。但我手里这份是“源码修改版”，是深蓝学院课程按书章节顺序和作业要求二次调整过的版本。差别在哪？原版仓库更像一本精装书的全彩插图，例子能跑但章节之间的索引关系要自己理；而这份修改版把与课程章节对应的主流程、参数文件、数据集调用接口单独拆开，评审逻辑也做了统一。适合两类人：一是深蓝学院课程学员，想跳过环境地狱直接看代码；二是做自动驾驶或机器人感知的从业者，想在LIO-SAM、Point-LIO这类方案上改自己的算法。这篇笔记我按代码结构、编译实测、阅读路线、踩坑和精度验证的顺序写完，你对着抄就行。

2. 这版源码改了什么：与原版仓库的逐项对比

2.1 项目结构盘点：src下到底哪几个模块

拿到解压包后第一件事不是编译，而是先看顶层目录。修改版延续了ROS工作空间的布局：

slam_in_autonomous_driving_modified/ ├── src/ │ ├── lidar_odometry/ │ ├── lio_mapping/ │ ├── point_lio/ │ ├── mulls/ │ └── common/ ├── datasets/ # bag文件与yaml配置的存放目录 ├── launch/ # 课程统一launch文件 └── README.md

src下按书章节拆成了独立功能包，每个包都能单独catkin_make，这比原版“一个仓库塞所有算法”的做法更适合按章节做作业。我把包与书章节的对应关系整理成了表，方便你定位要改哪里：

注意一点：修改版里LIO-SAM不叫lio_sam，而是叫lio_mapping。如果你在GitHub上搜过原版，会习惯性去找LIO-SAM的launch文件，这版里名字改了，一开始我也找了十分钟。做项目第一件事永远是catkin_make前先把包名和话题名列出来，能省掉后面大量排查时间。

2.2 从单传感器到多传感器：代码层面的三个统一

原版GitHub仓库里的很多演示工程是“能跑就行”，时间同步、坐标变换这类细节都压在了rosbag的时间轴上。但自动驾驶和机器人场景里，IMU与激光雷达的时钟偏差是直接影响精度的问题，深蓝学院课程确实有作业要求，所以修改版在以下三处做了统一处理。

第一，里程计输出统一走nav_msgs/Odometry。早期版本有的节点发tf变换、有的发Odometry、有的两个都发，接收端就得写两套回调。修改版在common包里定义了一套消息转换工具，所有odom统一走nav_msgs/Odometry，tf只保留map到odom的变换。这看起来是小事，但跑多方案对比时就太省事了。

第二，时间同步以IMU为主时钟。激光雷达点云时间戳和IMU时间戳做对齐时，修改版的做法是取IMU的header.stamp作为基准，点云回调里先做去抖动：

// 从common/include/common/time_utils.h中摘出的对齐逻辑 bool SyncImuAndLidar(const sensor_msgs::ImuConstPtr& imu_msg, const sensor_msgs::PointCloud2ConstPtr& cloud_msg, double max_delay_sec) { // 计算IMU与点云到达时间差 double diff = std::abs(cloud_msg->header.stamp.toSec() - imu_msg->header.stamp.toSec()); // 超过阈值就抛弃这一帧点云，避免匹配到错位的IMU数据 if (diff > max_delay_sec) { return false; } return true; }

参数max_delay_sec默认给的是0.05，也就是50ms。如果你的bag是仿真器生成的时间戳比较准，可以收紧到0.02；如果是实车采集，IMU与激光之间经常隔着几十毫秒的驱动延迟，放到0.1更稳妥。

第三，地图保存统一走PCL的ASCII接口。原版里有的节点用savePCDFileBinary，有的用savePCDFileASCII，后处理脚本要写两套解析。修改版统一成ASCII，代价是文件体积大一些，但换来了Python端直接用open3d.read_point_cloud就能读，不用装额外依赖。实车数据动辄几百MB的PCD，这块体积差异在能接受的范围。

2.3 深蓝学院课程要求的配套改造点

这版源码和公开仓库版本之间最直观的差别，就是课程作业框架。我挑三个写出来，你编译之前心里有数：

1. TODO注释挖空。凡是课程作业涉及的关键函数，源码里保留了默认实现，但都打上了// TODO: 请在此处完成XXX（深蓝学院作业）的标记。例如第5章的lio_mapping里有前后端解耦的实现，默认能编译能跑，但你把里面函数体替换成自己的实现后，输出精度会有明显变化。对照书里“自己动手”的章节做对比实验，收获比直接看代码大得多。

2. launch文件参数化。数据集的bag路径不再硬编码在main.cpp的参数里，而是统一提到launch文件：

<launch> <node name="point_lio_node" pkg="point_lio" type="point_lio_node" output="screen"> <param name="bag_path" value="$(find point_lio)/../datasets/course_ch6.bag"/> <param name="imu_topic" value="/livox/imu"/> <param name="lidar_topic" value="/livox/lidar"/> <param name="output_dir" value="$(find point_lio)/output/"/> </node> </launch>

注意bag_path用了$(find point_lio)做路径定位，这样工作空间整体拷贝到别的机器上时不需要改绝对路径。我以前在原版里改过一版硬编码路径的代码，后来换电脑重新编译，差点没被路径问题逼疯，参数化绝对是课程组做过实机验证的结论。

3. 日志全面接入glog。原版很多demo是cout打法，修改版统一接入了glog，并且按INFO、WARNING、ERROR分级输出。排查问题时不再是一团乱麻，可以直接grep WARNING看告警。加上launch文件里统一加了output="screen"，终端里能看到完整日志而不是只有ROS的topic echo。

3. 从零编译这套源码：环境配置与实测

3.1 推荐环境与依赖清单

我踩过最深的坑是ROS版本和PCL版本不匹配。这套源码的CMakeLists大量依赖PCL 1.10以上的接口，所以环境上建议直接上Ubuntu 20.04 + ROS Noetic，不要用18.04硬刚。Noetic自带的PCL是1.10，Eigen是3.3.7，OpenCV是4.2，这三个版本正好覆盖源码的编译需求。如果你在18.04上装，PCL往往还是1.8，pcl::Registration接口变化会让编译报一堆不明所以的错误。

依赖项我用一条命令装齐：

sudo apt-get install -y \ ros-noetic-pcl-conversions \ ros-noetic-laser-geometry \ ros-noetic-tf2-geometry-msgs \ libgoogle-glog-dev \ libgflags-dev \ libyaml-cpp-dev \ libeigen3-dev \ libopencv-dev

每条的作用：前三个是ROS的PCL与tf消息转换层，编译任何激光SLAM功能包都绕不开；glog和gflags是日志和命令行参数库，修改版里大量使用；yaml-cpp负责读配置文件；eigen3和opencv是数值计算与图像处理底座。这里说个血泪经验——不要用Anaconda里的eigen和opencv去编ROS包，conda的库路径会干扰catkin的include顺序，轻则警告重则直接link错版本。系统装一份，让编译器找/usr/include/eigen3就行。

3.2 编译顺序与CMakeLists排查

代码解压后按顺序执行编译：

# 1. 创建并初始化工作空间 mkdir -p ~/slam_book_ws/src cd ~/slam_book_ws catkin_init_workspace src # 2. 解压源码到src目录 unzip slam_in_autonomous_driving_modified.zip -d src/ # 3. 先编译common工具包，再编译依赖它的算法包 catkin_make --pkg common -j4 catkin_make -j$(nproc)

为什么要分两步编译？因为修改版里common包是其他所有包的公共依赖，如果一次性并行编译，catkin偶尔会把common排在后面编译，导致先编译的算法包找不到头文件。我第一次全量编译时就是玄学翻车，后来强制先编公共依赖包，一次通过。如果你机器核数多，-j$(nproc)全核并行很快，但如果内存小于16G，建议-j4就好，否则编到link.txt阶段会OOM。

常见的CMakeLists报错是找不到某个包：

CMake Error: Could not find a package configuration file provided by "livox_ros_driver"

这是因为Point-LIO依赖Livox雷达驱动。修改版在third_party目录里带了livox_ros_driver源码，需要你手动把它放进工作空间再编译：

cp -r third_party/livox_ros_driver ~/slam_book_ws/src/ cd ~/slam_book_ws && catkin_make --pkg livox_ros_driver -j4 source devel/setup.bash

编译完成后用roslaunch-l检查一遍所有launch文件能否被解析：

source devel/setup.bash roslaunch lio_mapping lio_mapping.launch --screen

能正常看到process started就说明功能包没问题。别急着继续，按Ctrl+C退出。

3.3 数据集与bag播放的准备工作

编译只是第一步，后面跑慢的原因一大半在数据集上。修改版对应的bag由深蓝学院课程配套提供，下载后放到datasets/目录即可。拿到bag先做一次体检：

rosbag info datasets/course_ch6.bag

关键看两点：一是话题名是否与launch文件里的一致，Point-LIO通常要/livox/lidar和/livox/imu两个话题；二是时间戳是否连贯，如果bag里有大段gap，建图轨迹会画成“断头路”。我用Livox采集的数据经常遇到的问题是IMU话题名不统一，有的驱动发/imu/data，有的发/livox/imu，所以rosbag info输出的topic列表务必和launch文件逐字比对。

播放bag也有玄学。实操建议用--pause参数先暂停，等所有节点都起来后再空格放行，避免节点没初始化完就丢了开局的关键帧：

rosbag play --pause datasets/course_ch6.bag

4. 核心代码阅读路线：这版源码里最值得看的三处实现

4.1 激光前端配准：从NDT到PL-ICP的选择逻辑

书里第3章到第4章的核心是激光里程计的前端。修改版lidar_odometry包里有NDT和PL-ICP两套配准实现，它们之间的选择不只是在main函数里改个flag，而是参数文件里有一组映射。打开config/lidar_odometry.yaml：

registration: method: "NDT" # NDT 或 PLICP ndt_resolution: 1.0 # 体素格大小，单位米 max_correspondence_dist: 2.0 max_iterations: 32 transformation_eps: 0.01

ndt_resolution是最需要调的参数。它在NDT里表示体素网格的大小，数值越大，匹配越粗但鲁棒性越好；数值越小，精度越高但容易陷入局部极小。室内环境我一般给0.5，室外大场景给1.5。如果你跑校园道路数据发现轨迹扭曲，先把这个值往上调，比动迭代次数管用得多。

代码里lidar_odometry_node.cpp的handleFrame函数是阅读入口：

void HandleLidarFrame(const CloudPtr& cloud_in) { // 降采样，提高配准速度，同时减少动态物体的干扰 pcl::VoxelGrid<PointType> voxel; voxel.setLeafSize(0.5f, 0.5f, 0.5f); voxel.setInputCloud(cloud_in); CloudPtr cloud_filtered(new Cloud); voxel.filter(*cloud_filtered); // NDT配准，预测位姿来自上一帧解算结果 ndt_.setInputSource(cloud_filtered); ndt_.align(*cloud_aligned_, predict_pose_); }

注意predict_pose_的来源，它决定了一个关键问题——你是在做帧间配准还是帧到局部地图配准。修改版里默认是帧到局部地图，也就是source是当前帧，target是滑窗内累积的局部点云。这样轨迹比纯帧间配准平滑很多，代价是内存占用高一些。

4.2 惯性预积分与因子图：lio_mapping的作业核心

第5章的lio_mapping对应LIO-SAM的简化实现，也是课程作业的主要阵地。它最值得读的部分是IMU预积分在因子图里的应用。读书时容易忽略的一个细节是：LIO-SAM并不要求IMU和激光严格同时到达，它会维护一个IMU预积分缓冲，等待激光帧到来后一次性处理。

源码里imu_preintegration.cpp的核心是三段式递推：

// 状态递推：位置、速度、姿态分别更新 predicted_state_.position += predicted_state_.velocity * dt; predicted_state_.velocity += accel_ * dt; predicted_state_.orientation *= delta_q_; // 预积分量更新：只累积相对运动，避免重复积分绝对加速度 delta_p_ += delta_v_ * dt + 0.5 * accel_ * dt * dt; delta_v_ += accel_ * dt;

这里有个容易踩的认知误区：预积分不是简单地把IMU积分结果当作位姿，而是把两次激光观测之间的IMU增量累积起来，作为因子图里的一项约束。修改版在factors/factor_prvag.cpp里实现了残差构造，其中对重力向量和零偏的雅可比是区别旧版PRVAG的关键。作业里如果让你改零偏估计，本质是去动bias相关的协方差矩阵，而不是调dt的大小。

4.3 参数文件对照：这套源码给了你哪些调参把手

我数了一下，修改版所有yaml参数加起来超过40个，但真正值得花时间的是三组。第一组是滤波参数，包括体素大小与距离裁剪阈值；第二组是匹配策略，包括前面说的ndt_resolution和max_iterations；第三组是后端优化里的关键参数：

mapping: keyframe_distance: 1.0 # 关键帧平移距离阈值，单位米 keyframe_angle: 0.5 # 关键帧旋转阈值，单位弧度 loam_scan_period: 0.1 # 单帧扫描周期，秒

keyframe_distance决定了地图里关键帧的密度，数值越小关键帧越多，后端优化越慢但地图更细。我习惯先给1.0跑通流程，再按场景缩小到0.5，观察轨迹误差的变化趋势。注意一点：修改版和原版一样，参数是加载完launch之后从yaml读的，不是运行时动态生效。改完参数要重启节点，别指望控制器里热更新。

5. 避坑与排查：我在复现时遇到的四个典型问题

5.1 编译时找不到livox_ros_driver

现象：catkin_make在point_lio包处报错，提示找不到livox_ros_driver的头文件或包配置。

原因：Livox雷达驱动不在ROS官方源里，需要单独编译。修改版的third_party目录里有源码，但没被自动加入工作空间的src路径。

解决：先把第三方库拷进src并单独编译，再编译主工程。顺序如下：

cp -r third_party/livox_ros_driver ~/slam_book_ws/src/ catkin_make --pkg livox_ros_driver -j4 source devel/setup.bash catkin_make -j4

建议不要用-j$(nproc)并行编译所有包，livox驱动和point_lio之间有头文件依赖，并行时偶发找不到头文件的玄学问题，串行-j4最稳。

5.2 跑lio_mapping时tf树报错：odom到base_link断连

现象：rviz里地图只建立了一部分，或者节点启动后终端刷Lookup would require extrapolation into the past。

原因：bag播放是异步的，节点启动后前几帧的IMU和激光时间戳差太大，tf缓存里还没有最近时刻的变换。常见于刚启动bag后立刻按空格键全速播放。

解决：播放bag前先rosbag play --pause，等节点打印出第一条Received lidar frame日志后再按空格。同时在launch文件里增大tf缓存延时：

<node pkg="tf2_ros" type="buffer_server" name="tf_buffer" output="screen"> <param name="cache_time" value="10.0"/> </node>

5.3 Point-LIO启动后CPU占用打满，帧率极低

现象：节点起来了，但rviz里的点云更新率不到1Hz，htop看CPU占用五个核全满。

原因：Point-LIO对点云逐点做状态更新，复杂度与点数线性相关。如果你没开降采样就喂入了Livox的百万点云，CPU自然扛不住。另外滚动网格的体素分辨率设太小也会拖慢。

解决：检查launch文件里是否有降采样参数，修改版通常在yaml里暴露了voxel_grid_leaf：

lidar: max_range: 100.0 min_range: 0.5 voxel_grid_leaf: 0.5 # 调到0.35以上，别小于0.2

实车数据我一般给0.5，既能保证建图细节又不会让CPU长期处于100%状态。如果你机器性能很好还想加密集度，从0.3起调，跑一帧看看耗时再决定是否继续降。

5.4 保存的地图出现双层“鬼影”

现象：建图过程中看rviz很清晰，但保存的PCD地图在CloudCompare里打开，墙体边缘被拉出半米宽的虚影。

原因：这是典型的回环未闭合导致的误差累积，同时地图保存选了Binary格式导致法向量精度丢失也是推手。修改版统一用ASCII后这个现象少了一些，但回环闭合的问题依然存在。

解决：修改版后端里默认回环检测阈值是0.6，我一般改成0.5以下多触发几次回环，代价是优化时间变长。如果地图依然有轻微偏移，可以保存轨迹后跑一遍evo看末尾轨迹有没有跳变，如果跳变幅度超过0.3米，说明你的关键帧抽取频率太低，把keyframe_distance从1.0降到0.6再重跑一次就好。

6. 进阶验证：用evo工具评估这套SLAM的轨迹精度

6.1 先把位姿轨迹导成TUM格式

跑完任意一节课的建图，都会在output_dir下生成一个轨迹文件。修改版默认写了TUM格式的trajectory.txt，每行是timestamp x y z qx qy qz qw。如果某个包只输出了nav_msgs/Odometry话题而没有保存文件，可以用一行Python把它转出来：

rosrun point_lio export_trajectory.py \ --topic /integrated_odom \ --bag datasets/course_ch6.bag \ --output output/estimated_traj.tum

6.2 用evoeval比较估计轨迹与真值

有了TUM格式轨迹后，评估精度用evo系列命令就够了：

# 先装工具 pip install evo --upgrade # 计算ATE，评估整体漂移 evo_ape tum groundtruth.tum estimated_traj.tum -a --plot # 计算RPE，评估局部平滑性 evo_rpe tum groundtruth.tum estimated_traj.tum --delta 1 --plot

-a参数让估计轨迹与真值做一次SE(3)对齐，对齐后算出的ATE才是可跨方案比较的数字。我第一次跑时忘了加-a，结果ATE里混入了两个坐标系原点不重合造成的系统性偏移，数值大得吓人，加了对齐之后才恢复正常。如果你手里没有真值轨迹，也可以用evo_ape kitti把两段不同参数跑出来的估计轨迹互相比，看相对差异。

6.3 调一个参数，用误差曲线判断灵敏度

最后给一个能落地的实验思路：固定真值不变，只改keyframe_distance，从0.5、1.0、2.0各跑一遍，然后分别算ATE，做一张误差-参数曲线。这个方法对判断你的数据集到底吃不吃关键帧密度非常直观，也能在写报告时给出一个有说服力的图表，而不是只说“我们进行了调参”。从那以后我每换一套数据集，都会先强制走一遍这个流程：编译、跑bag、导轨迹、evo评估、画曲线。习惯一旦固定下来，就很少再被参数玄学折磨了。希望帮到你。

本文还有配套的精品资源，点击获取