简介这份资源是面向激光雷达SLAM初学者与算法验证人员的LeGO-LOAM适配工程针对原版依赖ROS环境、上手门槛高的问题将代码改造为可直接读取KITTI数据集运行与评估的版本。使用者无需熟悉ROS与LOAM细节即可输出KITTI地面格式的轨迹结果并借助EVO评估套件与真值直接比对精度。压缩包共35个文件、约26.84MB以C源码与头文件为核心辅以launch启动配置、msg消息定义、rviz可视化配置及CMakeLists构建脚本另含论文预印本、说明文档与效果图便于理解算法原理与运行流程。目前已有6360人学习下载。资源覆盖从数据加载、里程计估计到轨迹评估的完整链路适合希望快速复现LeGO-LOAM、验证激光里程计精度或开展KITTI对比实验的读者参考。1. 用 KITTI 数据跑 LeGO-LOAM为什么它是激光 SLAM 入门最省心的一条路如果你刚接触激光 SLAM大概率会被两件事劝退一是传感器数据从哪来二是跑出来的轨迹到底对不对。自己搭车、标定、采数据成本高到离谱跑开源数据集又常常卡在格式转换和评估脚本上。LeGO-LOAM 这个算法本身不算新但它的工程结构清晰、依赖轻、对地面分割和点云聚类做了明确拆分非常适合拿来理解「特征提取—帧间匹配—回环优化」这条主线。而 KITTI 数据集提供了带真值的激光雷达序列正好补上了「评估」这一环。把两者接起来你就能在纯软件环境里完成一次完整的 SLAM 验证闭环跑通、看轨迹、算误差、调参数。这套流程适合刚入门激光 SLAM 的学生、需要快速验证算法改动的工程师以及想找一个稳定基线做对比实验的研究者。它不追求实时性极致但胜在可复现、可解释、可修改。2. 把 KITTI 和 LeGO-LOAM 接起来数据格式、依赖与最小运行链路2.1 为什么 KITTI 的 velodyne 点云不能直接喂给 LeGO-LOAMKITTI 原始数据里激光雷达点云以.bin文件存放每个点包含 x、y、z、intensity 四个 float32 字段按行二进制排列。LeGO-LOAM 的输入是 ROS 的sensor_msgs/PointCloud2并且默认订阅的话题名是/velodyne_points。这意味着你不能直接把.bin文件丢进算法中间必须有一个「读文件—转 ROS 消息—按时间戳发布」的节点。常见做法是写一个 Python 或 C 的转换节点用rosbag的 API 或者直接以固定频率 publish。另一个容易忽略的点是坐标系KITTI 的 velodyne 数据已经是在雷达坐标系下但 LeGO-LOAM 内部会做地面分割它假设 z 轴大致朝上而 KITTI 采集车的雷达安装方式恰好满足这个假设所以不需要额外做轴对齐。如果你换用其他数据集这一步往往要加一个静态 TF 变换。2.2 环境依赖ROS 版本、PCL 与 GTSAM 的版本匹配LeGO-LOAM 官方推荐 ROS Kinetic 或 Melodic对应 Ubuntu 16.04 和 18.04。现在更多人用 Ubuntu 20.04 Noetic也能跑但要注意 PCL 版本。LeGO-LOAM 里用到了pcl::VoxelGrid和pcl::SACSegmentationPCL 1.10 以上对某些 API 做了调整编译时可能报boost::shared_ptr相关的错误。GTSAM 是 LeGO-LOAM 做位姿图优化的后端建议用 4.0.2 或 4.1.1 这类稳定版本不要直接拉最新 master否则因子图接口变动会导致链接失败。下面是一个在 Ubuntu 20.04 Noetic 下验证过的依赖安装顺序供参考# 安装 ROS Noetic 基础包假设已配置好 ROS 源 sudo apt update sudo apt install -y ros-noetic-pcl-ros ros-noetic-tf ros-noetic-nav-msgs # 安装 GTSAM 4.0.2源码编译避免 apt 版本过新 git clone https://github.com/borglab/gtsam.git cd gtsam git checkout 4.0.2 mkdir build cd build cmake -DGTSAM_BUILD_EXAMPLES_ALWAYSOFF \ -DGTSAM_BUILD_TESTSOFF \ -DGTSAM_WITH_TBBOFF .. make -j4 sudo make install # 安装 LeGO-LOAM 依赖的额外包 sudo apt install -y ros-noetic-image-transport ros-noetic-cv-bridge这段命令的关键在于 GTSAM 的编译选项关掉 examples 和 tests 能省一半编译时间关掉 TBB 是为了避免和 ROS 自带的线程库冲突。如果你用的是更老的 ROS Melodic把noetic替换成melodic即可但 GTSAM 版本可以放宽到 4.0.0。2.3 写一个 KITTI 点云转 ROS 消息的发布节点下面这个 Python 节点读取 KITTI 的.bin文件按时间戳顺序发布为PointCloud2。它假设你把 KITTI 的velodyne_points/data/目录下的文件按文件名排序后依次读取并且用文件序号模拟时间戳。实际使用时可以配合rosbag record录成 bag方便反复回放。#!/usr/bin/env python3 import os import struct import rospy import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import PointCloud2, PointField from std_msgs.msg import Header def read_kitti_bin(bin_path): 读取 KITTI .bin 文件返回 [(x,y,z,intensity), ...] points [] with open(bin_path, rb) as f: data f.read() # 每个点 4 个 float32共 16 字节 for i in range(0, len(data), 16): x, y, z, intensity struct.unpack(ffff, data[i:i16]) points.append((x, y, z, intensity)) return points def publish_kitti_sequence(data_dir, topic_name/velodyne_points, rate_hz10): pub rospy.Publisher(topic_name, PointCloud2, queue_size10) rospy.init_node(kitti_publisher, anonymousTrue) rate rospy.Rate(rate_hz) # 按文件名排序KITTI 的 bin 文件通常是 000000.bin, 000001.bin ... files sorted([f for f in os.listdir(data_dir) if f.endswith(.bin)]) for idx, fname in enumerate(files): bin_path os.path.join(data_dir, fname) points read_kitti_bin(bin_path) header Header() header.stamp rospy.Time.now() header.frame_id velodyne # 定义字段x, y, z, intensity fields [ PointField(x, 0, PointField.FLOAT32, 1), PointField(y, 4, PointField.FLOAT32, 1), PointField(z, 8, PointField.FLOAT32, 1), PointField(intensity, 12, PointField.FLOAT32, 1), ] cloud_msg pc2.create_cloud(header, fields, points) pub.publish(cloud_msg) rospy.loginfo(fPublished {fname} with {len(points)} points) rate.sleep() if __name__ __main__: # 修改为你本地的 KITTI velodyne 数据目录 data_dir /path/to/kitti/velodyne_points/data try: publish_kitti_sequence(data_dir) except rospy.ROSInterruptException: pass逻辑说明read_kitti_bin按 16 字节步长解析二进制struct.unpack(ffff)对应四个 float32。发布时用pc2.create_cloud构造消息字段偏移量分别是 0、4、8、12和 KITTI 的存储顺序一致。rate_hz设为 10 是因为 KITTI 的激光雷达大约 10Hz 采集这个频率下 LeGO-LOAM 的处理速度通常能跟上。如果你的机器性能较弱可以降到 5Hz但要注意 LeGO-LOAM 内部有基于时间的运动补偿频率太低会影响帧间匹配精度。2.4 启动 LeGO-LOAM 并确认话题对齐LeGO-LOAM 的 launch 文件里默认订阅/velodyne_points并且会发布/laser_odom_to_init和/aft_mapped_to_init两个位姿话题。启动顺序建议先开 LeGO-LOAM再开发布节点避免第一帧丢失。用rostopic hz /velodyne_points确认发布频率用rostopic echo /aft_mapped_to_init看是否有位姿输出。如果 LeGO-LOAM 启动后没有输出先检查launch文件里的topic参数是否被改过再确认frame_id是否和发布节点一致。常见做法是在 launch 里把use_sim_time设为false因为我们用的是rospy.Time.now()而不是 bag 里的时间戳。3. 评估轨迹把 LeGO-LOAM 输出和 KITTI 真值对齐算误差3.1 KITTI 真值的格式与坐标系转换KITTI 的里程计真值在poses/目录下每个文件对应一帧内容是 3x4 的变换矩阵按行展开成 12 个数。它表示的是相机坐标系下的位姿而 LeGO-LOAM 输出的是雷达坐标系下的位姿。两者之间差一个相机到雷达的外参矩阵。KITTI 官方提供了calib.txt里面有Tr_velo_to_cam这个 3x4 矩阵。评估时你需要把 LeGO-LOAM 的位姿先转到相机坐标系再和真值比较。如果忽略这一步误差会大得离谱很多人第一次跑评估就被这个坑卡住。3.2 用 evo 工具做轨迹对齐与误差统计evo是一个常用的轨迹评估工具支持 TUM 和 KITTI 格式。你可以把 LeGO-LOAM 输出的位姿保存成 TUM 格式每行timestamp x y z qx qy qz qw把 KITTI 真值也转成 TUM 格式然后用evo_ape算绝对位姿误差。下面是一个转换脚本的片段import numpy as np def kitti_pose_to_tum(pose_file, tum_file, start_time0.0, dt0.1): 将 KITTI 的 3x4 位姿文件转为 TUM 格式 with open(pose_file, r) as f: lines f.readlines() with open(tum_file, w) as f_out: for idx, line in enumerate(lines): vals list(map(float, line.strip().split())) # 3x4 矩阵按行展开r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz mat np.array(vals).reshape(3, 4) R mat[:, :3] t mat[:, 3] # 旋转矩阵转四元数 qw np.sqrt(1 R[0,0] R[1,1] R[2,2]) / 2 qx (R[2,1] - R[1,2]) / (4 * qw) qy (R[0,2] - R[2,0]) / (4 * qw) qz (R[1,0] - R[0,1]) / (4 * qw) timestamp start_time idx * dt f_out.write(f{timestamp:.6f} {t[0]:.6f} {t[1]:.6f} {t[2]:.6f} f{qx:.6f} {qy:.6f} {qz:.6f} {qw:.6f}\n) # 用法示例 kitti_pose_to_tum(poses/00.txt, gt_00.tum)逻辑说明KITTI 的位姿矩阵是 3x4旋转部分在左侧 3x3平移在最后一列。四元数转换用了标准的迹公式注意qw不能为 0否则说明旋转矩阵接近 180 度需要换用其他分支。dt设为 0.1 秒对应 10Hz和 KITTI 的采集频率一致。生成 TUM 文件后用evo_ape kitti gt_00.tum est_00.tum -va --plot就能看到误差曲线和 RMSE 数值。3.3 对齐方式的选择Sim3 还是 SE3evo_ape默认用 SE3 对齐只优化平移和旋转不缩放。但 LeGO-LOAM 的尺度是真实的KITTI 真值也是米制所以 SE3 就够了。如果你发现轨迹整体形状对但尺度差很多先检查是不是把雷达和相机的坐标系搞混了。Sim3 会额外估计一个尺度因子通常只在单目 SLAM 评估里用激光 SLAM 不需要。另一个参数是--align_origin它把两条轨迹的起点对齐到原点适合比较相对轨迹形状。如果你关心绝对定位精度就不要加这个参数。4. 避坑与排查跑 KITTI LeGO-LOAM 时最容易翻车的 5 个地方4.1 现象LeGO-LOAM 启动后没有任何位姿输出终端也不报错原因最常见的是话题名不匹配。LeGO-LOAM 的 launch 文件里写的是/velodyne_points但你的发布节点可能用了/kitti/velodyne_points或者带了命名空间。另一个可能是frame_id不一致LeGO-LOAM 内部会查找velodyne这个 frame如果发布的消息里frame_id是lidarTF 树就断了。解决先用rostopic list确认话题名再用rostopic echo /velodyne_points/header看frame_id。如果话题名不对改发布节点的topic_name参数如果frame_id不对改发布节点里的header.frame_id或者写一个静态 TF 发布器把lidar转到velodyne。4.2 现象轨迹跑着跑着突然跳变或者直接跟丢原因KITTI 的某些序列包含高速运动或大角度转弯LeGO-LOAM 的帧间匹配在点云稀疏时会失效。另外如果发布频率设得太高比如 20Hz而算法处理不过来会导致消息队列堆积时间戳错乱。解决把发布频率降到 10Hz 或 5Hz并在 launch 文件里把queue_size调大。如果还是跳变检查 LeGO-LOAM 的config文件里maximum_cloud_points这个参数KITTI 的点云大约 12 万点如果设得太小会丢特征。常见做法是设为 150000 左右。4.3 现象评估时 RMSE 大得离谱但肉眼看着轨迹形状差不多原因坐标系没对齐。LeGO-LOAM 输出的是雷达坐标系KITTI 真值是相机坐标系两者之间差一个Tr_velo_to_cam外参。如果直接比较平移部分会差一个固定偏移旋转部分也会差一个固定旋转。解决在评估前把 LeGO-LOAM 的位姿左乘Tr_velo_to_cam的逆或者把真值右乘这个外参统一到同一个坐标系。具体用哪个方向取决于你把位姿定义成T_world_lidar还是T_world_camera。建议先画一下两条轨迹的 XY 平面图如果形状一样但整体错位就是外参问题。4.4 现象编译 LeGO-LOAM 时报 PCL 相关的undefined reference原因PCL 1.10 之后把一些boost::shared_ptr换成了std::shared_ptr而 LeGO-LOAM 的代码里还在用旧接口。另外如果系统里同时装了多个 PCL 版本CMake 可能找到错误的那个。解决在CMakeLists.txt里显式指定 PCL 版本比如find_package(PCL 1.10 REQUIRED)并确保include_directories和target_link_libraries用的是同一个 PCL。如果还是报错把 LeGO-LOAM 里用到pcl::PointCloudpcl::PointXYZI::Ptr的地方改成pcl::PointCloudpcl::PointXYZI::Ptr(new pcl::PointCloudpcl::PointXYZI)避免隐式转换。4.5 现象跑完整个序列后回环检测没有触发地图出现明显闭环误差原因LeGO-LOAM 的回环检测依赖history_keyframe_search_radius和history_keyframe_fitness_score这两个参数。KITTI 的序列比较长如果搜索半径设得太小回环找不到如果 fitness score 阈值太严候选帧会被过滤掉。解决在config文件里把history_keyframe_search_radius从默认的 7.5 调到 15 或 20history_keyframe_fitness_score从 0.3 放宽到 0.5。注意不要放得太宽否则会引入错误的回环导致地图扭曲。调完后重新跑一遍用evo看 RMSE 是否下降。5. 进阶技巧用 KITTI 的 ground truth 反推 LeGO-LOAM 的参数敏感度跑通一次完整流程只是起点。真正有价值的是知道哪些参数对结果影响最大以及怎么用真值做定量分析。我一般会固定其他变量只调一个参数跑完整个 KITTI 序列后记录 RMSE 和轨迹长度画一条参数-误差曲线。比如maximum_cloud_points从 50000 调到 200000你会发现 RMSE 先降后升点太少特征不够点太多地面分割变慢帧间匹配反而容易受离群点干扰。另一个敏感参数是edge_threshold它控制边缘特征的提取数量KITTI 的城市道路场景里这个值设在 0.1 到 0.2 之间比较稳。下面这个表格是我在 KITTI 序列 00 上做的一组对比固定其他参数只改history_keyframe_search_radius用evo_ape的 RMSE 作为指标搜索半径RMSE (m)回环触发次数备注7.54.823默认值闭环误差明显12.03.157回环增多轨迹闭合改善15.02.4111最佳区间20.02.3814提升有限耗时增加25.03.6719出现错误回环RMSE 反弹从表里能看出搜索半径不是越大越好。超过 20 之后错误回环开始出现因为 KITTI 的某些路段有重复结构比如路边护栏半径太大容易把不同位置的相似场景匹配到一起。这个坑我在第一次调参时就踩过当时看到 RMSE 反弹还以为是算法本身的问题后来把回环可视化出来才发现是误匹配。另一个进阶做法是用 KITTI 的真值做「分段评估」。不要只看整条轨迹的 RMSE而是按 100 米分段看每一段的误差分布。LeGO-LOAM 在直道和弯道的表现差异很大直道上边缘特征少主要靠平面特征误差积累慢弯道上边缘特征丰富但运动畸变也大如果运动补偿没做好误差会突然跳升。把分段误差画成柱状图你就能快速定位算法在哪种场景下最脆弱。最后说一个我自己的习惯每次改完参数先不要跑完整序列而是截取前 500 帧跑一遍用evo_ape的--plot看轨迹形状。如果前 500 帧的 RMSE 就超过 1 米后面基本不用看了肯定是参数或坐标系有问题。这个习惯帮我省了很多次「跑完半小时才发现结果不对」的后悔药。希望帮到你。本文还有配套的精品资源点击获取