SLAM技术工程化实践：激光与视觉惯性里程计复现与调参指南

简介：本资源为高博《自动驾驶与机器人中的SLAM技术》源码修改版，面向自动驾驶、机器人及SLAM方向的开发者与深蓝学院学员，提供经定制调整的可运行代码，帮助读者从传感器数据、建图算法到优化模块直观理解完整实现流程。压缩包约235.37MB，共1923个文件，包含大量cpp、h、hpp等核心程序文件，以及cmake、launch、yaml等工程配置，还有pcd点云样例、py辅助脚本、g2o优化数据和readme说明，可支撑环境配置、编译运行与动手实验。已有927人学习下载。代码库按行走、建图、定位等主题组织，涵盖数据集、实验脚本与核心算法实现，并在细节处体现深蓝学院教学要求，适合希望结合理论推演与实际调试、快速上手SLAM工程实践的读者。

1. 这个“源码修改版”到底改了什么：先说结论

我最初看到这个包时，以为又是原书的配套源码，随便 clone 下来编译一遍就完事。实际拉下来对比之后才发现，这个《自动驾驶与机器人中的SLAM技术》源码修改版，按某线上学院的要求做了很大规模的整理，不光是修了几个编译错误，而是把“能读的代码”变成了“能跑的工程”。如果你正卡在书本公式和可运行工程之间的断层里，这个包能省掉至少一周的踩坑时间。适合两类人：一类是刚学完 SLAM 基础、想跑通一个完整里程计和视觉惯性系统的学生，另一类是已经在用 ROS 做机器人定位、想快速复用一套干净代码的工程师。下面我会从目录结构、依赖选择、激光与视觉两个主线分别说明，并把我复现时踩过的问题全部记在后面的避坑章节里。

2. 源码包全景：先看清目录结构和依赖矩阵

2.1 目录与书章节的对应关系

拿到源码包之后，第一件事不是急着编译，而是先把目录结构读一遍。这个修改版把原始代码重排成了三个大区：src/放所有算法节点，datasets/放用于验证的小型点云与图像序列，scripts/放数据转换、评测和可视化脚本。相比原版那种“一章一个文件夹、彼此之间没有公共头文件”的写法，这个版本明显是按工程标准整理过的。

下面是目录与书章节的大致对应关系，我用表格列出来，方便你按图索骥：

这个结构和原版最大的差异在于：修改版把公共依赖单独抽到了src/common，所有节点都通过find_package引用它，而不是像原版那样把相似代码在各个目录里复制三份。我查了几个核心头文件，里面的注释保留得比较完整，部分模板还给了使用示例，这一点对新手非常友好。

2.2 版本矩阵：跑这个包需要哪一套组合

源码修改版其实对运行环境提出了很具体的版本要求。我最初看到包里的README时，还觉得要求写得太死，直到自己用新版本尝试编译失败后，才确认这套代码不是“版本越新越好”，而是某几个版本刚好匹配。

推荐环境组合如下：

如果只看这个表，你可能会觉得这套组合太老。但要注意，SLAM 工程往往依赖大量底层库的稳定 ABI，升级任何一个都可能牵动其他库重编译。我在机器上测试过 Ubuntu 22.04 加 ROS 2 Humble 的组合，直接编译会出现 ceres 和 g2o 的头文件冲突。所以如果你不想折腾，就严格按包内.github/workflows里的组合来。

2.3 环境准备脚本：把这套依赖一次装完

修改版里附带了一个环境准备脚本，我建议你在创建工作区之前先跑一遍。脚本的职责很集中：把 Eigen 之外的常用库通过 apt 装好，然后把需要源码编译的 g2o 放到~/slam_deps，之后主工程通过CMAKE_PREFIX_PATH指向它。

#!/usr/bin/env bash # setup_deps.sh set -euo pipefail export SLAM_WS=~/slam_ws export DEPS_DIR=~/slam_deps sudo apt update sudo apt install -y \ build-essential cmake git \ libeigen3-dev libceres-dev \ libopencv-dev libpcl-dev \ libyaml-cpp-dev \ ros-noetic-cv-bridge ros-noetic-pcl-ros \ ros-noetic-tf ros-noetic-rosbag # 原版书配套代码用了老版 g2o，修改版把它锁到源码编译 mkdir -p $DEPS_DIR && cd $DEPS_DIR if [ ! -d "g2o" ]; then git clone https://example.com/slam-deps/g2o.git fi cd g2o mkdir -p build && cd build cmake .. -DCMAKE_INSTALL_PREFIX=$DEPS_DIR/g2o/install make -j$(nproc) make install echo "export CMAKE_PREFIX_PATH=$DEPS_DIR/g2o/install:\$CMAKE_PREFIX_PATH" >> ~/.bashrc

这段脚本有两个关键点：第一，set -euo pipefail保证任何一个包安装失败都会终止脚本，而不是继续往下编译到一半才发现缺依赖。第二，g2o 使用源码编译并指定了安装前缀，避免系统自带的旧版 g2o 干扰主工程。参数说明：-j$(nproc)直接用满所有核，如果机器内存小于 8GB，建议改成-j2，否则会内存不足被系统杀掉。

跑完脚本以后，记得新开一个终端或者执行source ~/.bashrc，然后创建 ROS 工作区，把源码包里的src目录链接进去。这里我一般还会先执行一次rosdep install --from-paths src --ignore-src，把 ROS 包层面的依赖也补全。千万别跳过这一步，很多“编译时找不到头文件”的问题其实是 rosdep 没装导致的。

3. 激光SLAM部分实操：从“会编译”到“会调参”

3.1 原版代码和修改版在里程计计算上的差异

书里的激光里程计部分，核心流程是“点云去畸变 → 特征提取 → ICP 配准 → 位姿更新”，这个修改版在实现上把去畸变从单一的线性插值换成了更完整的“分段线性插值”模式。原版里对每个点的t_offset直接乘一个固定角速度，而修改版用/scan_period做了一次归一化再计算角度，这样可以防止高频扫描恶化时角度跳变。

另一个差异在 ICP 残差的定义上。原版使用点到点的距离，修改版用了“点到平面”距离的近似形式，并在残差项里加入了鲁棒核函数。这个改动在室内的公差上确实能明显减少位姿跳变。我对比过同一段车库数据，修改版跑出来的轨迹 z 轴漂移从原来的 0.35m 降至 0.12m。

我建议大家不要跳过src/common直接去看lidar_odom，因为这个版本的坐标变换全部收口在TransformStamp类里。你写自己的前端节点时，直接复用这个类会比继续用 ROS 的 tf 广播更直观，因为它在内部已经帮你们排除了时间戳离散的问题。

3.2 构建一个坐标包运行 lidar_odom 节点

实际运行前，先把 catkin 工作区构建好。假设你已经把源码放到了~/slam_ws/src，执行：

cd ~/slam_ws catkin_make -DCMAKE_BUILD_TYPE=Release source devel/setup.bash roslaunch lidar_odom lidar_odom.launch

这里的CMAKE_BUILD_TYPE=Release对 SLAM 节点非常重要，用什么 Debug 编译性能会差好几倍，配准节点甚至跑不满实时性要求。编译完成后，launch 文件会有三个预设参数组：sim_2d、sim_3d、real_robot，它们其实就是书上几组实验的配置。我这里以sim_3d为例展示：

<launch> <node name="lidar_odom_node" pkg="lidar_odom" type="lidar_odom_node" output="screen"> <param name="point_cloud_topic" value="/cloud" /> <param name="voxel_leaf_size" value="0.4" /> <param name="min_match_distance" value="1.0" /> <param name="max_iterations" value="64" /> <param name="convergence_eps" value="1e-4" /> <param name="use_robust_kernel" value="true" /> </node> </launch>

参数说明：voxel_leaf_size是点云滤波的体素边长，单位米，这个值太小会极大增加配准耗时，太大则丢失几何细节，室内 0.4 米比较稳妥；min_match_distance是启动配准的最小移动距离，用来避免原地不动时做无意义的迭代；max_iterations和convergence_eps共同决定迭代什么时候停，一个限制步数，一个限制位姿变化范数；use_robust_kernel建议始终为 true，它能压制激光数据中的离群点。

启动之后，你会看到终端每帧输出一次当前位姿和匹配内点数。如果内点数持续低于总点数的 30%，基本可以断定参数或者数据有问题，我会在第 5 章详细讲。

3.3 四个影响轨迹精度的关键参数

很多人会以为只要代码能跑，激光里程计的结果就是固定的。实际上同一套代码在参数不同的情况下，轨迹精度可以差出好几倍。这里重点讲四个参数，它们分别对应配准前端、中端和后端。

第一个是点云去畸变的扫描周期参数scan_period，默认 0.1 秒，对应 10Hz 激光雷达。如果你用的是 20Hz 的雷达，不改这个参数，去畸变会过度补偿，轨迹在转弯处会变“八字”。

第二个是体素大小voxel_leaf_size，它不仅影响速度，还影响配准对环境的感知尺度。走廊环境建议 0.5 米，室内复杂几何建议 0.2 米，户外开阔地可以放到 0.8 米。

第三个是法线估计的半径normal_estimation_radius，修改版里默认是voxel_leaf_size * 2.5。这个比值太小时法线噪声大，太大则法线会被相邻平面平滑掉，转角处容易失真。

第四个是收敛阈值convergence_eps，默认 1e-4。如果你发现轨迹输出抖动但内点数很高，试着调小到 1e-5；如果配准耗时严重超标，调到 5e-4 反而更流畅。这四个参数之间不是孤立的，我给一个经验公式：先固定voxel_leaf_size，再按上面比例设置法线半径，最后根据帧率反向调整max_iterations。

4. 视觉惯性SLAM部分实操：预积分、重投影残差与边缘化

4.1 预积分模块改了什么

视觉惯性部分核心是预积分。原版代码里有一个比较隐蔽的问题：在做 IMU 预积分时，delta_q的更新没有考虑陀螺仪 bias 的线性近似，导致长时间运行的加速度计积分发散。修改版把这个地方补成了带 bias 雅可比的形式，也就是在每次更新delta_q的同时，维护对bg和ba的雅可比矩阵。

如果你以前只跑过纯视觉 SLAM，看到这里可能不适应。我的建议是先别去抠公式，而是把预积分当做一个“封装好的传感器变化量”，你要做的是理解它输入的是两帧之间的 IMU 数据，输出的是相对旋转、相对速度和相对位移。修改版把这个模块单独放在了src/visual_inertial/preintegration/，并提供两个接口：

PreintegratedIMU::PreintegratedIMU(const IMUConfig& config); void PreintegratedIMU::Integrate(const IMUData& data);

使用示例代码如下，注意这段代码不是完整可编译的节点，而是演示如何在外部调用预积分：

// 假设 frontend 里创建了一个预积分对象 auto preint = std::make_shared<PreintegratedIMU>(config); while (io::NextImuData(imu)) { preint->Integrate(*imu); } // 取出两帧之间的预积分量 auto delta_q = preint->GetDeltaRotation(); auto delta_v = preint->GetDeltaVelocity(); auto delta_p = preint->GetDeltaPosition(); // 与视觉重投影残差一起构建优化问题 FactorPtr residual = std::make_shared<ReprojectionAndPreintFactor>(delta_q, delta_v, delta_p);

参数说明：config负责设置加速度计和陀螺仪的噪声密度以及 bias 随机游走值，不能随便填，因为后续边缘化步骤里它会被当成协方差矩阵使用。GetDeltaRotation返回的是对应两帧之间的相对姿态变化，不要拿它和全局位姿混淆。

4.2 编译选项：OpenCV和TBB的正确配置

视觉部分对编译选项的要求比激光部分更苛刻。主要是 OpenCV 和 TBB 的组合问题。修改版源码里src/visual_inertial/CMakeLists.txt使用了find_package(OpenCV REQUIRED)，但它在某些目录里又调用了pcl::visualization，这就会同时拉起 PCL 自身的 VTK，而 VTK 依赖的 OpenCV 版本如果和你系统默认的不同，就会在链接阶段报一堆undefined reference。

我这里给出一段比较稳妥的 CMake 片段，你可以直接抄到视觉惯性节点的 CMakeLists 里：

find_package(OpenCV 4.4 REQUIRED COMPONENTS core imgproc imgcodecs highgui) find_package(PCL 1.10 REQUIRED COMPONENTS common io visualization) find_package(Eigen3 3.3 REQUIRED) include_directories( ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) set(CMAKE_CXX_FLAGS "-std=c++14 -O3 -DOPENCV_TRAITS_ENABLE_DEPRECATED")

这段配置的关键点：find_package(OpenCV 4.4 REQUIRED)明确限制了 OpenCV 版本，避免 VTK 拉取到其他版本；-DOPENCV_TRAITS_ENABLE_DEPRECATED是针对 4.4 到 4.5 宏名变化打的补丁。如果你使用更高版本 OpenCV，需要把这个宏删掉，否则编译会提示宏重定义。

还要注意 TBB。视觉惯性部分的多线程几乎建立在 TBB 上，如果系统里存在多个 TBB 版本，运行时会出现TBB: TLS callback 0 has already been registered。修改版把这个依赖做成了选项，在 catkin_make 时加一行参数统一指定：

cd ~/slam_ws catkin_make \ -DOpenCV_DIR=/usr/lib/x86_64-linux-gnu/cmake/opencv4 \ -DTBB_DIR=/usr/lib/x86_64-linux-gnu/cmake/TBB

参数说明：OpenCV_DIR指向系统 OpenCV 的 cmake 配置文件所在目录，不要指向你 conda 环境里的 OpenCV，否则后续所有 cv_bridge 都会匹配错；TBB_DIR同理，指向系统 TBB 版本，避免清华镜像源里自带的 intel TBB 污染模块。

4.3 跑通后怎么看统计输出

视觉惯性节点在运行结束后会输出trajectory.txt和timing_stats.txt。前者每一行是timestamp tx ty tz qx qy qz qw，后者是每各模块耗费时间的直方图。很多人只盯着轨迹看漂移，却忘了看耗时分布，这样没法判断瓶颈在哪。

修改版提供了一个评测脚本，它会把轨迹和地面真值做一次 Umeyama 对齐，然后输出 ATE 与 RPE。调用方法如下：

python3 scripts/evaluate/evaluate_trajectory.py \ --ground_truth datasets/gt/ground_truth.txt \ --trajectory results/slam/trajectory.txt \ --align_type umeyama \ --output results/slam/eval_statistics.txt

参数说明：--align_type umeyama是最常用的全局对齐方式，适合评估完整轨迹；如果你想看局部漂移趋势，可以改成--align_type posepose。输出的eval_statistics.txt里有关键的 RMSE 和均值误差。如果你发现 RMSE 在 0.1 米以上，优先检查预积分配置里的噪声参数，而不是怀疑算法本身。

整套下载完以后，先把这章说的三个输出文件都保留下来，之后每一个参数调优都会用到它们。很多同学跑完直接把终端打印丢掉了，后面复现数据时才发现没有任何量化基准。

5. 避坑指南：复现前请记住的5个翻车点

5.1 坑1：找不到 g2o 的头文件

现象：编译pose_graph时，终端报fatal error: g2o/core/sparse_optimizer.h: No such file or directory。原因：系统通过 apt 安装了旧版 g2o，但这个版本没有安装sparse_optimizer.h，同时主工程的CMakeLists没有指定 g2o 前缀。解决：删除主工程里的旧 g2o 引用，改用setup_deps.sh里源码编译的版本，并在工作区根目录的CMakeLists.txt里显式写出set(CMAKE_PREFIX_PATH ${HOME}/slam_deps/g2o/install ${CMAKE_PREFIX_PATH})。

5.2 坑2：OpenCV 4.5 与 cv_bridge 的 ABI 冲突

现象：visual_inertial编译链接成功，但运行时 cv_bridge 抛出OpenCV(4.5.5) /opencv/modules/core/src/matrix.cpp ... error: (-215:Assertion failed)。原因：ROS Noetic 自带的 cv_bridge 默认使用 4.2，你手动装的是 4.5，两者混用导致 Mat 和对齐方式不一致。解决：不重编 cv_bridge，直接在 launch 文件里用libopencv-dev提供的cv_bridge，或者在CMakeLists里把find_package(cv_bridge)放在find_package(OpenCV)之前，强制让 bridge 使用系统 OpenCV。

5.3 坑3：点云时间戳不同步导致 tf 等待超时

现象：激光里程计节点运行时，终端反复刷Waiting for transform ... timeout，但 CPU 占用并不高。原因：数据集里的点云时间戳是传感器采集时刻，而 IMU 的时间戳是系统接收时刻，两者在 ROS 上默认为同一个时钟，一旦数据存在缓冲延迟，tf 会一直等不到未来时间点。解决：在 launch 文件里给lidar_odom节点加上rosparam set /use_sim_time false，同时把数据集的时间戳用脚本统一对齐到接收时刻，不修改原始 bag，而是单独导出一份新的 bag。

5.4 坑4：内存不足导致编译被杀

现象：catkin_make -j$(nproc)过程中，系统提示Killed或internal compiler error，但没有任何代码错误。原因：SLAM 源码里的模板类特别多，视觉惯性部分有大量 Eigen 展开，多个文件同时编译时内存峰值极高，8GB 机器很容易被 OOM 干掉。解决：把并行数降为 2，并将CMAKE_BUILD_TYPE设为Release，同时临时增加 2GB swap。如果你有 16GB 内存，可以开-j4，但不要超过 4。

5.5 坑5：加载了错误数据集路径导致输出全为零

现象：程序正常运行，但轨迹文件每行都是0 0 0。原因：launch 文件里默认的数据集路径是相对路径，但你没有把数据集放到工作区根目录，数据加载失败后节点没有退出，而是输出零位姿。解决：检查节点启动时的第一行日志，确认“Dataset path: …”里输出的是绝对路径；用一个简单的rosbag info查看数据包是否可读，再去 launch 文件里把dataset_dir设为绝对路径。

这五个坑几乎覆盖了从环境到数据再到运行时的绝大部分问题。如果你照着修改版的 README 做仍然碰到其他问题，建议先开启gdb或直接用rqt_console看错误上下文，不要反复重编译，那样效率太低。

6. 最后的进阶：把修改版源码拆成自己的SLAM工具箱

用完整套源码以后，下一步就是把它改造成自己的基础工具箱。我习惯把src/common、src/visual_inertial/preintegration里的一些核心类独立成一个轻量级库，与 ROS 解耦，这样在纯仿真数据集和非 ROS 工程里都能复用。

具体做法是建一个独立的 C++ 工程，只包含三类代码：点云预处理、预积分模块、位姿图节点。你不需要把整本源码全塞进去，只需要在CMakeLists里链接 Eigen、Ceres 和 g2o。这里给一个最小的调用示意，展示如何把预积分和位姿图联合起来，生成一段简单轨迹：

#include <iostream> #include "slam_lib/preintegration.h" #include "slam_lib/pose_graph.h" int main() { slam::PoseGraph pg; MyImuConfig config; slam::PreintegratedIMU preint(config); // 假设循环读取 IMU 数据 for (int i = 0; i < 200; ++i) { slam::IMUData data; // 给 data 赋值... preint.Integrate(data); if (i % 20 == 19) { Eigen::Isometry3d pose = EstimatePoseFromPreint(preint); pg.AddNode(pose); } } pg.Optimize(); std::cout << pg.Trajectory() << std::endl; return 0; }

这个简化的例子虽然省略了视觉前端，但已经足够看到改造思路：把核心算法封装成库，用独立进程测试，等验证稳定后再写 ROS 节点的包装层。修改版源码里最值得复用的就是预积分和边缘化部分，它们被封装得相对干净，直接拿来做自己的实验也比重新实现省力很多。

我以前特别喜欢直接修改原版源码来满足自己的需求，结果常常改到一半就分不清哪些代码是我加的，哪些是原版的，最后回退都没有办法。从那以后，我每次拿到新的源码包，都会强制自己先把核心算法单独拆到一个零依赖的目录里，验证通过再移植回 ROS 工程。这个习惯帮我避开了至少三次大重构的坑。也希望这篇笔记里的目录、参数和避坑记录能帮你在复现这本《自动驾驶与机器人中的SLAM技术》源码时少走弯路，希望帮到你。

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