简介面向深蓝学院教学与科研需求定制的高博《自动驾驶与机器人中的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.mdsrc下按书章节拆成了独立功能包每个包都能单独catkin_make这比原版“一个仓库塞所有算法”的做法更适合按章节做作业。我把包与书章节的对应关系整理成了表方便你定位要改哪里包名对应章节核心内容修改版变化lidar_odometry第3-4章激光前端配准、NDT/ICPodom统一发nav_msgs/Odometry增加参数暴露接口lio_mapping第5章LIO-SAM简化版拆出帧率控制与地图线程作业处留TODOpoint_lio第6章Point-LIO紧耦合里程计时间戳统一按IMU主时钟对齐bag路径参数化mulls第8章多策略激光SLAM增加地图评测脚本便于对比多组参数输出注意一点修改版里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/Odometrytf只保留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 深蓝学院课程要求的配套改造点这版源码和公开仓库版本之间最直观的差别就是课程作业框架。我挑三个写出来你编译之前心里有数TODO注释挖空。凡是课程作业涉及的关键函数源码里保留了默认实现但都打上了// TODO: 请在此处完成XXX深蓝学院作业的标记。例如第5章的lio_mapping里有前后端解耦的实现默认能编译能跑但你把里面函数体替换成自己的实现后输出精度会有明显变化。对照书里“自己动手”的章节做对比实验收获比直接看代码大得多。launch文件参数化。数据集的bag路径不再硬编码在main.cpp的参数里而是统一提到launch文件launch node namepoint_lio_node pkgpoint_lio typepoint_lio_node outputscreen param namebag_path value$(find point_lio)/../datasets/course_ch6.bag/ param nameimu_topic value/livox/imu/ param namelidar_topic value/livox/lidar/ param nameoutput_dir value$(find point_lio)/output// /node /launch注意bag_path用了$(find point_lio)做路径定位这样工作空间整体拷贝到别的机器上时不需要改绝对路径。我以前在原版里改过一版硬编码路径的代码后来换电脑重新编译差点没被路径问题逼疯参数化绝对是课程组做过实机验证的结论。日志全面接入glog。原版很多demo是cout打法修改版统一接入了glog并且按INFO、WARNING、ERROR分级输出。排查问题时不再是一团乱麻可以直接grep WARNING看告警。加上launch文件里统一加了outputscreen终端里能看到完整日志而不是只有ROS的topic echo。3. 从零编译这套源码环境配置与实测3.1 推荐环境与依赖清单我踩过最深的坑是ROS版本和PCL版本不匹配。这套源码的CMakeLists大量依赖PCL 1.10以上的接口所以环境上建议直接上Ubuntu 20.04 ROS Noetic不要用18.04硬刚。Noetic自带的PCL是1.10Eigen是3.3.7OpenCV是4.2这三个版本正好覆盖源码的编译需求。如果你在18.04上装PCL往往还是1.8pcl::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就说明功能包没问题。别急着继续按CtrlC退出。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.bag4. 核心代码阅读路线这版源码里最值得看的三处实现4.1 激光前端配准从NDT到PL-ICP的选择逻辑书里第3章到第4章的核心是激光里程计的前端。修改版lidar_odometry包里有NDT和PL-ICP两套配准实现它们之间的选择不只是在main函数里改个flag而是参数文件里有一组映射。打开config/lidar_odometry.yamlregistration: method: NDT # NDT 或 PLICP ndt_resolution: 1.0 # 体素格大小单位米 max_correspondence_dist: 2.0 max_iterations: 32 transformation_eps: 0.01ndt_resolution是最需要调的参数。它在NDT里表示体素网格的大小数值越大匹配越粗但鲁棒性越好数值越小精度越高但容易陷入局部极小。室内环境我一般给0.5室外大场景给1.5。如果你跑校园道路数据发现轨迹扭曲先把这个值往上调比动迭代次数管用得多。代码里lidar_odometry_node.cpp的handleFrame函数是阅读入口void HandleLidarFrame(const CloudPtr cloud_in) { // 降采样提高配准速度同时减少动态物体的干扰 pcl::VoxelGridPointType 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 pkgtf2_ros typebuffer_server nametf_buffer outputscreen param namecache_time value10.0/ /node5.3 Point-LIO启动后CPU占用打满帧率极低现象节点起来了但rviz里的点云更新率不到1Hzhtop看CPU占用五个核全满。原因Point-LIO对点云逐点做状态更新复杂度与点数线性相关。如果你没开降采样就喂入了Livox的百万点云CPU自然扛不住。另外滚动网格的体素分辨率设太小也会拖慢。解决检查launch文件里是否有降采样参数修改版通常在yaml里暴露了voxel_grid_leaflidar: 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.tum6.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评估、画曲线。习惯一旦固定下来就很少再被参数玄学折磨了。希望帮到你。本文还有配套的精品资源点击获取