SLAM源码修改版实战：从编译依赖到ICP/NDT参数调优

简介：面向自动驾驶与机器人领域学习者的《自动驾驶与机器人中的SLAM技术》源码修改版，依据深蓝学院教学与研究需求定制，帮助读者通过可运行的工程代码理解SLAM定位与建图的核心原理，并适配实际项目实践。压缩包共1923个文件，约235.37MB，主体为c/h/cc/cpp等源码文件，配合txt说明文档、yaml配置、py脚本、pcd点云数据等，涵盖传感器数据处理、SLAM算法实现、地图构建优化、测试与演示模块，目录结构完整，便于按章节或功能查阅。已有927人学习下载。这份定制源码并非单纯代码陈列，而是将理论讲解与动手修改相结合：读者可结合书中章节逐行调试，验证位姿估计、回环检测与全局优化等关键环节，并借助周边工具脚本与实验数据快速搭建自己的测试环境，尤其适合深蓝学院学员、SLAM入门研究者及希望将算法落地到实际机器人项目的工程师。

1. 为什么一本SLAM源码书需要“修改版”

翻开《自动驾驶与机器人中的SLAM技术》随书源码，第一道坎不是算法，而是编译。这本书把激光、视觉、点云配准、后端图优化拆到不同章节，每个章节一个 demo，本来方便对照阅读，可一旦你按自己的环境装好依赖，跟着 README 执行，报错数量常常比示例代码本身还多。所谓“源码修改版”，并不是算法推倒重来，而是课程平台按实训要求，把散落的章节代码重排成统一工程，锁定依赖版本，补上 ROS 话题、TF 和评估脚本，让同一份源码在不同电脑上都能编译、能跑出轨迹。它解决的问题很直白：让自动驾驶与机器人方向的学习者从“看懂代码”走到“调参数看效果”，也让工程人有份能直接抄的参考骨架。这篇笔记就按我拿到修改版后的处理路线，讲怎么改、怎么调参数、坑在哪，以及最后怎么变成自己的项目。

2. 把这套源码改成可复现：目录重排与依赖锁定的思路

修改版最花时间的不是调算法，而是让环境一致。源码本身能跑通和换一台电脑也能跑通是两回事，前者靠运气，后者靠结构。拿到修改版源码，我先只做两件事：把目录重排成构建系统能预测的形态，把依赖版本钉死。这两件事做完，后面所有算法实验才有基础。

2.1 先分清源码里哪是算法、哪是作业脚手架

书籍源码大多按章节给 demo，比如一个文件夹就是一个知识点，里面带着 main 函数和所有实现。这种代码对阅读友好，对工程不友好：同一份 ICP 代码可能被三个章节复制了三遍，你改 A 处忘了 B 处，编译时间也白白浪费。课程平台的作业要求通常又需要统一提交、统一评测，所以修改版最常见的工作就是拆包：算法放 modules，入口放 apps。

拆包不是随便挪文件夹。我拿到源码后会先执行一行命令，把所有带 main 的 cpp 找出来，确认哪些文件是入口，哪些是库。入口文件应只负责读参数、调模块、发结果；库文件应只负责算法。若 modules 里混着 main，说明封装还没拆干净，改起来要多费一步。典型的结构如下。

slam_workshop/ ├── CMakeLists.txt ├── cmake/ │ ├── FindSophus.cmake │ └── CompilerOptions.cmake ├── modules/ │ ├── common/ # 日志、时间戳、坐标系转换工具 │ ├── frontend/ # 点云配准、视觉特征提取与匹配 │ ├── backend/ # 图优化封装（g2o / Ceres 二选一） │ └── io/ # rosbag 读写、点云文件读写 ├── apps/ │ ├── run_icp_ndt.cpp │ ├── run_odometry.cpp │ └── run_mapping.cpp └── scripts/ ├── eval_ate.py └── viz.launch

看到这个结构，你先去 modules/common 看公共工具，再去 modules/frontend 看前端算法，最后看 apps。不要从 apps 底下的 main 一头扎进一个几千行的文件里，那样很容易被细节淹没。修改版把公共的 timer、tf、日志放在 common，避免到处重复，后面调优时往往只改 frontend 或 backend，其他都不用重编。

如何快速区分？执行下面的命令，它能列出所有入口文件。如果输出集中在 apps，结构是干净的；如果 modules 里出现多个 main，说明原源码还没有完全组件化，你至少要把这些 main 对应的文件提取出来，再决定归属。

grep -rn "int main" --include="*.cpp" modules apps

2.2 依赖锁定：先定版本再写代码

依赖版本这件事，很多初学者把它当成玄学，其实逻辑很清楚。Eigen 是矩阵库，Sophus 是李代数库，后者构建在前者之上；g2o 和 Ceres 做图优化，又依赖 Eigen；OpenCV 和 PCL 为了编辑几何类型，也各自绑定了 Eigen 相关头文件。当 find_package 搜索到两套 Eigen，比如一套在 /usr/include/eigen3，另一套在 conda 环境或者 PCL 的 ThirdParty，编译时就会出现两个版本宏混用，甚至运行到对齐分配那里崩溃。

修改版通常会在根 CMakeLists 里把系统库作为第一优先级，找不到才用仓库内 ThirdParty。核心逻辑如下。

cmake_minimum_required(VERSION 3.16) project(slam_workshop CXX) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) # 优先用系统库，找不到再回退到 ThirdParty find_package(Eigen3 3.3 REQUIRED) find_package(OpenCV 4.2 QUIET) find_package(PCL 1.10 QUIET) find_package(g2o QUIET) find_package(Ceres QUIET) find_package(Sophus REQUIRED) if(NOT Sophus_FOUND) add_subdirectory(ThirdParty/Sophus) endif()

Eigen3 用 REQUIRED，确保一进来就有底层；OpenCV 和 PCL 用 QUIET，是允许它们暂时缺席，后续由 WITH_VISION/WITH_LIDAR 决定是否需要；Sophus 单独处理，因为很多系统默认不装它。版本号 3.3 是最低要求，不代表只接受 3.3，Ubuntu 22.04 自带的 3.4 也能满足。

如果你发现某个依赖无论如何都 find 到错误的上下文，先检查编译环境变量。我遇到过最典型的翻车就是 conda 环境先于 /usr 被搜索，链接到一个不匹配的 OpenCV，PCL 的头文件却在系统目录，两边 Eigen 宏不一致，最终在运行到 point-to-point 残差时崩掉。临时解决办法很粗暴：

unset CMAKE_PREFIX_PATH

让 CMake 回到默认路径。若你长期用这套源码，建议把依赖装进一个干净的容器或独立环境里，不要动系统库。

2.3 用CMake开关把不用的模块关掉

结构统一后，还要能按需裁剪。SLAM 源码常常同时带视觉前端和激光前端，但课程作业可能只要求其中一个。如果因为相机驱动没装，整个工程编译失败，就很不值。修改版的做法是用 CMake option 控制子目录。

option(WITH_VISION "Build vision frontend" ON) option(WITH_LIDAR "Build lidar frontend" ON) option(WITH_BACKEND "Build backend optimizer" ON) if(WITH_VISION) add_subdirectory(modules/vision) endif() if(WITH_LIDAR) add_subdirectory(modules/lidar) endif() if(WITH_BACKEND) add_subdirectory(modules/backend) endif()

这里有个新手常踩的坑：只关 CMake 层，不关 C++ 层。比如 WITH_VISION=OFF 只是不编译 modules/vision，但 apps 里的 run_odometry.cpp 仍然实例化了 VisionFrontend，链接阶段就会报 undefined reference。所以修改版会在 app 的 CMakeLists 里把开关转成编译宏：

target_compile_definitions(run_odometry PRIVATE WITH_LIDAR=$<BOOL:${WITH_LIDAR}> WITH_VISION=$<BOOL:${WITH_VISION}>)

然后源码里用 #ifdef 选择前端。这样改参数不用动代码，只动 cmake 命令：

cmake -B build -DWITH_VISION=OFF -DWITH_LIDAR=ON

如果你只是临时调试前端，也可以只 add_subdirectory 对应的模块，减少等待。这个开关设计不是平台要求的一部分，但几乎所有修改版都会带上，因为到后面跑实验时太常用。

3. 动手改源码：从“能编译”到“能跑出图”

依赖和结构就位后，下一个目标是让一个可执行文件跑起来，并把结果可视化。我会按三步走：先搞定最小编译，再调配准参数，最后把位姿发布成 ROS 话题。

3.1 最小改动能编译：把路径和包名统一

最小编译的原则是，一次只编译一个目标，别用 make -j 一把抓。拿点云配准来说，原始章节代码的 include 路径是相对的，挪进统一目录后第一件事就是改 include。

# 原始写法： # #include "chapter3/icp_registration.h" # 修改版写法： #include "modules/frontend/icp_registration.h"

接着在根 CMakeLists 里指定这个可执行文件依赖哪些目录和库。这个 CMake 片段是修改版里最常见的模板：

add_executable(run_icp_ndt apps/run_icp_ndt.cpp) target_include_directories(run_icp_ndt PRIVATE ${CMAKE_SOURCE_DIR}/modules ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ) target_link_libraries(run_icp_ndt PRIVATE frontend_lib ${PCL_LIBRARIES} )

用 CMAKE_SOURCE_DIR 而不是相对路径，是因为 CMake 在执行 target_include_directories 时，相对路径是相对于当前源码目录，但你可能在 build 目录里运行，路径一换就找不到。EIGEN3_INCLUDE_DIR 来自 find_package(Eigen3) 的暴露变量；PCL_INCLUDE_DIRS 是从 PCLConfig.cmake 里带出来的，直接使用即可，前提是前面已经 find_package(PCL)。

编译时我建议先单线程：

cmake -B build -DWITH_VISION=OFF -DWITH_LIDAR=ON cmake --build build --target run_icp_ndt -j2

如果报错，错误信息里第一个文件就是第一个问题，不要往下翻。修好一个再编译，比一次看二十个错误要省时间。

3.2 点云配准模块怎么改：ICP与NDT的关键参数

跑通后，配准效果基本就看参数。修改版为了实验方便，参数默认写在 C++ 里，你只需要对着场景调。下面这组是我从室内和室外两组数据里整理出的起点值：

#include <pcl/registration/icp.h> #include <pcl/registration/ndt.h> pcl::IterativeClosestPoint<PointT, PointT> icp; icp.setMaximumIterations(50); icp.setMaxCorrespondenceDistance(2.0); // 米，室内可压到 1.0 icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1e-6); pcl::NormalDistributionsTransform<PointT, PointT> ndt; ndt.setResolution(1.0); // 体素栅格边长，室外加大到 2.0 ndt.setStepSize(0.1); // 步长，太大发散，太小收敛慢 ndt.setTransformationEpsilon(0.01); ndt.setMaximumIterations(35);

几个关键参数逐个说。ICP 的 setMaxCorrespondenceDistance 决定一对点要被当作匹配点时，最近距离不能超过这个值，单位是米。室内小场景激光点云密集，1.0 米就够；室外大场景初始位姿偏差可能到 2 米，先给 2.0，否则匹配点数太少。TransformationEpsilon 和 EuclideanFitnessEpsilon 都是收敛条件，一般保持默认，只有你发现算法在收敛阈值附近来回抖时才调大一个数量级。

NDT 的 setResolution 是最有存在感的参数：它把空间划成 voxel，每个 voxel 里统计正态分布。分辨率太小，体素多、计算慢且容易陷局部最优；分辨率太大，局部结构被抹平，匹配精度下降。室内 0.5 到 1.0，室外 1.5 到 2.0 是常见区间。修改版如果默认给 1.0，园区场景建议先改成 2.0 试试。

另外，这套源码里如果同时提供 ICP 和 NDT，不要只挑一个跑。我一般让 NDT 粗对齐给 ICP 提供初值，再让 ICP 精修，效果比单独任一个都稳定。代码是：

pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>); ndt.align(*aligned, initial_guess); icp.setInputCloud(source); icp.setTargetCloud(target); icp.align(*aligned, ndt.getFinalTransformation());

3.3 把里程计输出改成Rviz能订阅的话题格式

很多原始 demo 只在终端打印变换矩阵，看不出建图效果。修改版按课程平台要求，会改成发布 ROS 话题。你需要两个东西：Odometry 消息给 rviz 画轨迹，TF 让点云能显示到车体坐标系下。代码：

#include <nav_msgs/Odometry.h> #include <tf2_ros/transform_broadcaster.h> nav_msgs::Odometry odom; odom.header.stamp = ros::Time::now(); odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; odom.pose.pose.position.x = t.x(); odom.pose.pose.position.y = t.y(); odom.pose.pose.position.z = t.z(); odom.pose.pose.orientation.x = q.x(); odom.pose.pose.orientation.y = q.y(); odom.pose.pose.orientation.z = q.z(); odom.pose.pose.orientation.w = q.w(); static tf2_ros::TransformBroadcaster br; geometry_msgs::TransformStamped tf_msg; tf_msg.header.stamp = odom.header.stamp; tf_msg.header.frame_id = "odom"; tf_msg.child_frame_id = "base_link"; tf_msg.transform.translation.x = t.x(); tf_msg.transform.translation.y = t.y(); tf_msg.transform.translation.z = t.z(); tf_msg.transform.rotation.x = q.x(); tf_msg.transform.rotation.y = q.y(); tf_msg.transform.rotation.z = q.z(); tf_msg.transform.rotation.w = q.w(); br.sendTransform(tf_msg);

这段代码有两个关键点。第一，header.frame_id 是 world 系，child_frame_id 是车体系，二者不能写反；写反后轨迹会被画到车体上，点云跟随转动。第二，发布频率要和前端输出频率一致，直接在雷达回调里发，不要放在一个单独的 10Hz 线程里，否则位姿滞后明显。

然后启动 rviz 配置：

<launch> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_workshop)/rviz/odom.rviz"/> <node pkg="tf2_ros" type="static_transform_publisher" name="lidar_tf" args="0 0 0.5 0 0 0 base_link velodyne"/> </launch>

static_transform_publisher 的六个数字是车体到雷达的外参平移和旋转。这里用了 0.5 米高度，真实车上要按安装位置填。最后用 bag 回放验证，如果 rviz 里轨迹在走、点云贴在路上，说明修改版的 ROS 层已经通了。

4. 在自动驾驶/机器人场景里验证修改版：数据与指标

能跑出图只是第一步，接下来要在真实数据上验证这套修改版。验证分两块：数据回放是否同步、轨迹指标是否达标。两块都要量化，不然你根本不知道一个参数改好了还是改坏了。

4.1 用数据包回放：时间戳同步与坐标系

我用 bag 回放，而不是把算法接到实车上。原因是回放过程完全可控，每帧点云的时间和内容都固定，适合做参数对比。录包时最好把原始 topic 名留住，回放时用包内时间。命令如下：

# 录包时建议用原始 topic 名，别在车里改名 rosbag record -O room.bag /velodyne_points /imu/data /odom # 回放时强制用包里的时间，避免和系统时间冲突 rosparam set /use_sim_time true rosbag play --clock room.bag

rosbag record 的三个 topic 是雷达、IMU 和参考里程计；如果你的传感器没有输出 odom，可以录一个 GNSS 或轮速计做参考。回放时 --clock 很关键，它让 ros::Time::now() 跟着包里时间走，而不是系统墙钟。若不加，录包和回放有时间差，TF 缓存里会疯狂报错。

坐标系方面，修改版里 odom 和 base_link 是动态 TF，由里程计算法发布；base_link 到各传感器的外参是静态 TF，必须在 launch 里声明。漏掉静态 TF 是最常见的问题，表现是 rviz 里点云偏离车体几十厘米甚至飞出去。检查外参是否生效，可以用：

rosrun tf tf_echo base_link velodyne

如果输出一直停在初始位置，说明静态 TF 没发，或者坐标系的 parent/child 写反了。

4.2 评估轨迹：用evo算ATE/RPE

轨迹出来后，用 evo 评估。修改版一般会把位姿写到 tum 格式，一行一个时间戳加平移四元数。下面是我固定用的三条命令：

# 先看轨迹形态，并做 SE(3) 对齐 evo_traj tum estimated.tum --ref groundtruth.tum -a -s # 绝对轨迹误差，反映整体漂移 evo_ape tum groundtruth.tum estimated.tum -va # 相对位姿误差，反映局部抖动 evo_rpe tum groundtruth.tum estimated.tum -va --delta 1

evo_traj 先看轨迹形态，-a 是做 SE(3) 对齐，-s 是尺度对齐，适合没有全局定位的里程计。evo_ape 算绝对位姿误差，反映整体漂移；evo_rpe 算相对位姿误差，--delta 1 表示每隔 1 米算一次局部误差，能看出前端有没有突然跳变。如果 ATE 的 RMSE 从 0.3 变成 0.5，说明你的参数改坏了；如果 RPE 的最大值很大，可能是某个退化帧导致跳变。

注意：跑评估时一定要先让算法跑完，生成完整轨迹文件，再对同一个文件反复评估。不要在算法运行时同时做评估，两者都会抢 CPU，导致前端掉帧，指标虚高或虚低。

4.3 不同场景下的参数表

参数表不是固定的，下面是修改版在不同场景下我常用的起点。抄过去先跑一遍，再微调。

退化走廊没有足够几何特征，点云匹配很容易顺着走廊方向滑。这时候单纯调 NDT 分辨率解决不了，必须引入 IMU 做帧间预测，或者把走廊两侧的平面约束加进后端。修改版里如果预处理器带 IMU 融合，这是最值得先开通的功能。

5. 修改版避坑：5个最常翻车的地方

下面这五条坑，是我在把修改版源码移植到不同电脑、不同数据集时踩过的。每一条都是先在现象上困惑了很久，最后定位到具体原因。写在这里，给你做个排错索引。

5.1 Sophus 构造报错

现象：编译时出现 no matching function to call to 'Sophus::SO3d::SO3d()'，或者运行到某处直接崩溃。

原因：Sophus 在不同版本里 API 有差异，有的版本不允许默认构造，必须用旋转矩阵、四元数或轴角显式构造。书籍源码的某些 demo 为了简洁，写了默认构造，到了较新版本就失效。

解决：统一用四元数构造，不要用默认对象再赋值。示例：

Eigen::Quaterniond q(w, x, y, z); Sophus::SO3d R(q); Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); T.linear() = R.matrix();

这样写对老版本也兼容。如果还有问题，优先检查 Sophus_FOUND 找的是不是 ThirdParty 里的副本。

5.2 Eigen 对齐崩溃

现象：程序运行到把位姿 push_back 进 std::vector 时崩掉，崩得毫无规律；有时候 Debug 版没事，Release 版必崩。

原因：Eigen 里的固定大小类型比如 Isometry3d，内部使用 SSE 对齐，容器默认分配器不保证 16 字节对齐。这是 Eigen 文档明确警告过的，第一次写都会遇到。

解决：在 vector 声明时用 aligned_allocator：

using Poses = std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>; Poses keyframe_poses;

同样的问题也会出现在 std::vector Sophus::SE3d 里。修改版如果封装了 Keyframe 结构体，要注意该结构体里包含 Eigen 成员时，也要定义 operator new，或者直接把它放进 aligned_allocator 的容器。

5.3 PCL 与 OpenCV 的 Eigen 冲突

现象：编译时出现一堆“EIGEN_MAY_ALIAS_ONCE 重定义”“宏被覆盖”的报错，或者运行到特征提取时崩溃。

原因：源码中先 include 了 PCL，后 include 了 OpenCV，两个库各自带 Eigen 配置，宏定义互相踩踏。另一个来源是 CMake 里有两个 Eigen 头文件目录先后被搜索。

解决：先控制 include 顺序，把 OpenCV 放前面，PCL 放后面，避免 PCL 的 Eigen 配置覆盖 OpenCV 的。代码：

#include <opencv2/core/core.hpp> #include <pcl/point_cloud.h> #include <pcl/registration/icp.h>

如果还是冲突，检查根 CMakeLists 里是否通过 include_directories 加入了 PCL 的 ThirdParty 目录；统一改成 find_package(PCL) 提供的 PCL_INCLUDE_DIRS，并把 /usr/include/eigen3 放在前面。我的习惯是在所有 target_compile_options 里统一加 -DEIGEN_MPL2_ONLY，让两边都能接受同一套宏策略。

5.4 TF 时间戳不同步

现象：rviz 里点云一会儿正常一会儿飞出去，终端刷消息“TF buffer has a hole”或“message from before current time”。

原因：rosbag 回放时，点云消息的时间戳来自包，而算法里 ros::Time::now() 来自系统时钟，两条时间线不同步，TF 数据库里查不到对应时间的变换。

解决：回放前打开 use_sim_time，并让所有模块都读 /clock。具体是：

rosparam set /use_sim_time true rosbag play --clock room.bag

如果你在一个 launch 文件里同时启动算法，就把 use_sim_time 写在 launch 的 param 里，确保算法节点和 rviz 都拿到模拟时间。这样 TF 缓存和点云时间戳能一一对应。

5.5 内存占用过高被系统杀死

现象：建图跑到十几分钟，终端直接打印 Killed，退出码 137；或者系统变卡，swap 疯狂读写。

原因：修改版默认把所有关键帧的原始点云都保留在内存里，用于回环检测。高频雷达一帧几十万点，关键帧几百个，内存就爆了。

解决：在关键帧处理处做体素降采样，只保留降采样后的点云。代码：

pcl::VoxelGrid<PointT> voxel; voxel.setLeafSize(0.1f, 0.1f, 0.1f); voxel.setInputCloud(frame_cloud); voxel.filter(*filtered_cloud); frame_cloud = filtered_cloud;

leaf size 0.1 适合低速小场景，室外园区可以用 0.2 到 0.3，既省内存又不丢主要结构。如果还需要更高精度，把降采样结果单独存一份用于显示，另一份原始点云用完即丢，不要全保留。

6. 把这套修改版变成项目骨架：三个验证习惯和一个扩展技巧

修改版源码只是起点。课程作业提交后，我一般会立刻把它当成自己的项目骨架继续维护。这里说三条我坚持的验证习惯，都不难，但能省掉大把排错时间。第一，每次只改一个参数，改完固定跑同一段 bag，记录 ATE/RPE 数值。如果不跑同一段数据，优化和回环的影响会混在一起，根本判断不出这次改动是变好还是变坏。第二，把评估命令写成一个 shell 脚本，里面固定好对齐方式、平移单位，放到仓库里。这样你三个月后回来还能知道当时的指标是怎么算出来的，而不是靠聊天记录。第三，给重要的源码改动打 git tag，作业被退回时能快速定位是哪个版本出了问题。

最后一招是扩展技巧：把容易变的参数从 C++ 里抽出来。修改版里许多算法参数埋在 .cpp 里，每次调参都要重编，浪费时间。我会改成读取 YAML 配置，运行时改参数，不用重新编译。配置长这样：

frontend: type: "ndt_icp" ndt_resolution: 1.0 ndt_step_size: 0.1 ndt_max_iterations: 35 icp_max_correspondence: 2.0 icp_max_iterations: 50 voxel_leaf_size: 0.1

C++ 读取可以用现成的 yaml-cpp：

YAML::Node cfg = YAML::LoadFile(config_path); auto frontend_cfg = cfg["frontend"]; ndt.setResolution(frontend_cfg["ndt_resolution"].as<double>()); ndt.setStepSize(frontend_cfg["ndt_step_size"].as<double>());

这样当你换一个场景，只要改 yaml 里的数值，再启动一次算法，效果立即可见。我吃过每次改参数编译五分钟，最后发现只是分辨率从 1.0 改成 2.0 的亏。这之后我再也没把算法参数写在代码里。希望帮到你。

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