简介这是一份面向自动驾驶与激光雷达SLAM学习者的KITTI数据适配版LeGO-LOAM代码包重点解决在KITTI数据集上运行、描述轨迹并评估LeGO-LOAM结果的问题适合具备C与ROS基础、希望绕开复杂环境配置直接验证算法的读者。压缩包共35个文件约26.84MB包含5个cpp与3个头文件等核心源码、launch与rviz配置、msg消息定义、CMakeLists与package.xml构建文件以及png/jpg效果图、md说明、pdf论文和LICENSE等辅助材料兼顾编译运行、结果展示与许可说明。已有6361人学习说明其在KITTI激光点云与里程计实验中具有较高参考价值。读者可据此获得KITTI地面真值格式的轨迹输出思路并借助EVO评估套件完成精度对比同时参考GTSAM依赖与工程目录组织快速复现LeGO-LOAM流程并定位常见配置问题。1. KITTI 的 bin 文件跑 LeGO-LOAM先搞清这条链路在转什么第一次拿 KITTI 数据跑 LeGO-LOAM最容易被骗的地方是它「看起来在动」rviz 里点云在刷位姿话题也在发可把轨迹导出来丢进 evoATE 直接跳到十几米。翻回来查才发现utility.h里还留着 16 线雷达的N_SCAN16、Horizon_SCAN1800KITTI 那台 64 线激光雷达一帧十二万个点绝大部分在投影成距离图像时因为行号越界被直接扔了。这套 kitti-lego-loam 的适配解决的就是这段「数据格式到算法假设」的落差把里程计数据集里的 bin、times、poses 串成一条能重复跑的链路让激光里程计真正吃到完整点云再用 evo 把估计轨迹和 ground-truth 拉进同一套坐标系里比。适合已经在机器上跑通过 LeGO-LOAM 的示例包、想拿公开数据验证自己那份代码到底行不行的从业者。2. 从 bin 到 PointCloud2KITTI 与 LeGO-LOAM 的三处接口错位2.1 为什么不能直接把 bin 当 ROS 点云喂进去KITTI 里程计数据集的每一帧是一个纯二进制文件float32四元组连续排列含义是x, y, z, reflectance坐标轴是激光雷达常见的 x 向前、y 向左、z 向上反射率归一化到 0 到 1 之间。它没有 ROS 消息头没有时间戳也没有 ring 字段这三个缺失刚好都踩在 LeGO-LOAM 的输入假设上。LeGO-LOAM 的前端在imageProjection.cpp里会把一帧点云投成N_SCAN × Horizon_SCAN的二维距离图像然后按行号做地面分割和点云聚类。行号有两条来路点云自带 ring 通道就直接读没有就按垂直角度反算成rowIdn (verticalAngle ang_bottom) / ang_res_y。KITTI 的 bin 没有 ring所以只能走第二条。默认参数是按 16 线雷达写的垂直视场上下各 15 度行号一旦算到 16 以上就被continue掉结果就是点云看着还在其实只有底部几行活下来。这是后面所有精度问题的总源头不是玄学是几行常数。2.2 数据目录、时间戳、标定链路上真正的三个输入odometry 数据集的目录结构很规整但要跑起来得先弄清楚每个文件在链路里干什么文件或目录内容在链路里的角色sequences/00/velodyne/*.bin每帧 float32 × 4点云源喂给算法sequences/00/times.txt每行一个相对秒数造header.stamp也是 evo 对齐的时间轴sequences/00/poses.txt每行 12 个数3×4 变换矩阵评估基准不是给算法用的sequences/00/calib.txtP0–P3 投影矩阵与 Tr决定 GT 位姿在哪个坐标系sequences/00/sequence.txt图像与点云的时间对应纯视觉才用得上跑雷达可以忽略times.txt里是相对第一帧的秒数不是 UTC也不是绝对 ROS 时间。这一点直接影响评估如果 GT 用 times估计轨迹却用rosbag录制时间或者系统墙钟evo 在时间戳匹配阶段就会大量丢帧最后算出来的 ATE 是残缺的。我一般做法是播放节点把header.stamp显式设成times.txt里的值同时打开/use_sim_time让整条链路只有一个时间来源。calib.txt里的Tr给的是相机 0 到别的相机的外参poses.txt里的位姿是相机 0 在世界系下的位姿也就是 x 向右、y 向下、z 向前。激光雷达系是 x 前、y 左、z 上。两者差一个固定的刚体变换评估时不处理等于拿两套坐标轴比距离误差表会难看到无法解释。常见做法是在 GT 转换脚本里乘一个旋转矩阵把 GT 从相机系搬到激光系或者反过来把估计轨迹搬到相机系只要两边一致就行。2.3 转 rosbag 还是直接写播放节点两种喂数据的方式都有人用。转 bag 的好处是一次转换多次复用评估时可以直接rosbag play缺点是要先把一百多 GB 的 bin 全转一遍磁盘和转换脚本的时间戳处理都得做对直接写播放节点的好处是不落中间文件改参数只要改代码缺点是要自己管节流和仿真时间。我一般选直接出版本节点理由是调参阶段会反复改N_SCAN、maxRange这些常量重新编译如果每次都重新转 bag迭代节奏会被磁盘 IO 拖住。播放节点本身很短用 numpy 读 bin手工拼PointCloud2的data字段比create_cloud逐点打包快一个数量级因为后者会走 Python 层的结构体序列化。3. 改四个常量再编译让 LeGO-LOAM 认得出 64 线点云3.1 工作空间、依赖与一次干净的编译先把 LeGO-LOAM 的源码目录原样放进src注意保留仓库自身的层级它内部还有一层同名目录才是 catkin 包递归查找package.xml能识别到就行mkdir -p ~/lego_ws/src cd ~/lego_ws/src # 把下载到的 LeGO-LOAM 源码目录整体拷进来保持仓库原始层级 cp -r /path/to/LeGO-LOAM . # 确认 catkin 能找到包正常输出应该指向 LeGO-LOAM 包目录 find . -name package.xml依赖分两块ROS 侧和 C 侧。PCL、Eigen、OpenCV 是编译期必须的gtsam 是后端位姿图优化用的LeGO-LOAM 用的是 4.0 系列的接口得单独装sudo apt-get install -y ros-noetic-pcl-ros ros-noetic-tf2-ros \ ros-noetic-eigen-conversions libpcl-dev libeigen3-dev libopencv-dev # gtsam 4.0.x 源码编译 cd ~/gtsam-4.0.x mkdir -p build cd build cmake .. -DCMAKE_BUILD_TYPERelease -DGTSAM_BUILD_EXAMPLES_ALWAYSOFF make -j4 sudo make install编译这一步有个血泪经验catkin_make默认不带优化跑 KITTI 这种单帧十二万点的数据Debug 版本的处理速度大概只有 Release 的十分之一表现出来的现象是「算法发布轨迹的频率比点云低很多」很容易被误判成代码逻辑有问题。加-DCMAKE_BUILD_TYPERelease之后cd ~/lego_ws catkin_make -DCMAKE_BUILD_TYPERelease -j4 source devel/setup.bash-j4按机器核数给内存小的机器8G 以下建议降到-j2PCL 的模板展开很吃内存并行度过高容易在链接阶段被 OOM killer 干掉报错信息看起来像是某个源文件编译失败实际是内存不够这个坑很浪费时间。3.2 utility.h 里的四个常量改错了全盘皆输所有编译期参数都集中在LeGO-LOAM/include/utility.h不需要去动算法主体。KITTI 对应的是 HDL-64E 这个级别的 64 线雷达垂直视场从 2 度到 -24.8 度左右这组值改完再编译// LeGO-LOAM/include/utility.h // 从 VLP-16 的默认值改成 KITTI HDL-64E 的量级 extern const int N_SCAN 64; // 线数投影图像的高度 extern const int Horizon_SCAN 2088; // 单圈水平点数投影图像的宽度 extern const float ang_res_y (2.0 24.9) / 63.0; // 约 0.427 度 extern const float ang_bottom 24.9; // 最低垂直角度的绝对值 extern const int groundScanInd 58; // 地面候选从第几行开始逐个说清楚为什么这么取。N_SCAN直接决定投影图像的 行数填 16 的时候rowIdn算出来 20 的点会被判越界丢掉KITTI 一帧里有一大半点落在 16 行以下所以不改的话点云是残的。Horizon_SCAN是宽度KITTI 每帧总量约 12 万点除以 64 线平均每线接近 1900 点但 HDL-64E 的水平分辨率本身不均匀某些方位会密到 2000 点以上取 2088360 度除以约 0.1728 度留出余量写小了会丢点写大了浪费内存这个值在 1800 到 2100 之间都能跑属于经验区间。ang_res_y和ang_bottom是一对前者由后者和最高角度折算而来公式就是垂直视场总跨度除以 63 个间隔。ang_bottom在代码里参与的是verticalAngle ang_bottom所以填的是绝对值不是带符号的负数负号留在夹角本身。groundScanInd是地面分割的起点行号0 度水平面对应的行号是24.9 / 0.427 ≈ 58所以地面候选从 58 行往下取比这个值大一点更保守能躲开车辆俯仰带来的误判。提示HDL-64E 的垂直角度分布不是严格等间距上半部分约 1/3 度一格下半部分约 1/2 度一格。用线性近似会让部分点被分到相邻行效果是点云略稀疏但拓扑完整跑里程计够用。想要更精确就得把 ring 号塞进 intensity 字段再改imageProjection.cpp的读取分支属于第二阶段的事。3.3 播放节点把 bin 逐帧变成 PointCloud2节点只做三件事按times.txt的节奏读 bin、截断过远的点、拼出带正确stamp和frame_id的消息#!/usr/bin/env python3 # kitti_bin_player.py逐帧读取 KITTI velodyne bin 并发布 PointCloud2 import os import numpy as np import rospy from std_msgs.msg import Header from sensor_msgs.msg import PointCloud2, PointField 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), ] class KittiPlayer: def __init__(self): seq_dir rospy.get_param(~seq_dir) # 序列根目录如 /data/kitti/sequences/00 self.max_range rospy.get_param(~max_range, 60.0) self.frame_id rospy.get_param(~frame_id, base_link) self.bin_dir os.path.join(seq_dir, velodyne) self.files sorted(os.listdir(self.bin_dir)) # times.txt 给的是相对起始的秒数作为唯一的仿真时间来源 self.times [float(x) for x in open(os.path.join(seq_dir, times.txt))] self.pub rospy.Publisher(/velodyne_points, PointCloud2, queue_size4) def build_msg(self, arr, stamp): msg PointCloud2() msg.header Header(stamprospy.Time.from_sec(stamp), frame_idself.frame_id) msg.height 1 msg.width arr.shape[0] msg.fields FIELDS msg.is_bigendian False msg.point_step 16 # 4 个 float32 msg.row_step 16 * arr.shape[0] msg.is_dense False msg.data arr.astype(np.float32).tobytes() return msg def run(self): n min(len(self.files), len(self.times)) for i in range(n): arr np.fromfile(os.path.join(self.bin_dir, self.files[i]), dtypenp.float32).reshape(-1, 4) # 截断远点KITTI 城区场景里 60 米外的点对里程计贡献小、耗时高 rng np.linalg.norm(arr[:, :3], axis1) arr arr[rng self.max_range] if rospy.is_shutdown(): break self.pub.publish(self.build_msg(arr, self.times[i])) rospy.Rate(10).sleep() # 与 KITTI 采集频率对齐 rospy.loginfo(sequence done: %d frames, n) if __name__ __main__: rospy.init_node(kitti_bin_player) KittiPlayer().run()逻辑上有几个点值得说明。data字段用手工.tobytes()而不是走sensor_msgs.point_cloud2.create_cloud因为后者是逐点序列化十二万点的一帧要几百毫秒直接拼字节数组是毫秒级。point_step设 16 是因为四个float32紧密排列没有填充字节row_step就是宽度乘 16这两处写错会导致 rviz 里点云错位成麻花。stamp用times.txt的值而不是rospy.Time.now()是为了让整条链路落在同一个相对时间轴上后面 evo 匹配才不会被时区或录制延迟坑。frame_id直接写base_link是个省事的做法。LeGO-LOAM 的imageProjection内部不做 tf 查询只用 launch 里的extrinsicTrans、extrinsicRot做外参补偿所以点云的frame_id主要影响 rviz 显示。写base_link之后rviz 里把固定帧也设成base_link就省掉了一条静态 TF 的配置。3.4 串起一条 launch看第一帧有没有活下来把播放节点和算法端放进同一个 launch算法端用仓库自带的run.launch原样 include避免改坏它的内部参数launch arg nameseq_dir default/data/kitti/sequences/00/ !-- 全程只用 times.txt 的相对时间必须开仿真时间 -- param name/use_sim_time valuetrue/ node pkgkitti_player typekitti_bin_player.py namekitti_bin_player outputscreen param nameseq_dir value$(arg seq_dir)/ param namemax_range value60.0/ param nameframe_id valuebase_link/ /node !-- 仓库自带的算法 launch参数保持默认外参用零值 -- include file$(find lego_loam)/launch/run.launch/ /launchuse_sim_time必须在启动任何节点之前就设成 true顺序反了会让节点先吃到一次墙钟时间之后rosbag record或 tf 里出现时间跳变。跑起来的验收标准有三个rostopic hz /velodyne_points稳定在 10Hz 左右rostopic hz /aft_mapped_to_init有输出哪怕慢一点rviz 里固定帧设成base_link后看/velodyne_points应该能清楚看到街道两侧的立面和地面而不是稀稀拉拉几条线。如果第三点看到的点云层数明显少于 64回到 3.2 检查常量别急着看算法。4. 用 evo 对轨迹把 odom 和 ground-truth 拉进同一套坐标系4.1 从 bag 里抽 odom顺手做时间戳去重评估要用的是后端优化后的位姿话题是/aft_mapped_to_init不是前端的/Odometry。录制方式很直接# 先起录制再起播放保证第一帧不丢 rosbag record -O /data/seq00_lego.bag /aft_mapped_to_init roslaunch kitti_player kitti_run.launch seq_dir:/data/kitti/sequences/00bag 转 TUM 的脚本要注意两件事一是时间戳可能有重复同一次优化迭代里补发evo 遇到非严格递增会直接报错退出二是字段顺序TUM 是timestamp tx ty tz qx qy qz qw四元数在最后#!/usr/bin/env python3 # bag2tum.py把 /aft_mapped_to_init 抽成 TUM 格式顺便去掉重复时间戳 import rosbag bag rosbag.Bag(/data/seq00_lego.bag) rows, seen [], set() for _, msg, _ in bag.read_messages(topics[/aft_mapped_to_init]): ts round(msg.header.stamp.to_sec(), 6) if ts in seen: # 同一时间戳只保留第一次保证严格递增 continue seen.add(ts) p, q msg.pose.pose.position, msg.pose.pose.orientation rows.append((ts, p.x, p.y, p.z, q.x, q.y, q.z, q.w)) rows.sort(keylambda r: r[0]) with open(/data/seq00_lego_tum.txt, w) as f: for r in rows: f.write(%.6f %.6f %.6f %.6f %.9f %.9f %.9f %.9f\n % r) bag.close()round(..., 6)是为了把浮点误差导致的两个「几乎相同」的时间戳归并掉六位小数在 10Hz 场景下已经足够区分相邻帧。四元数保留九位小数TUM 的解析对精度比较敏感位姿差异大的时候截断误差会被放大成假的漂移。4.2 GT 转 TUM外加一次坐标系搬家poses.txt每行 12 个数是 3×4 的变换位姿在相机 0 系下。把它转成 TUM 并在同一脚本里换到激光系公式是T_w_velo T_w_cam0 · T_cam0_velo那个固定旋转就是轴重排#!/usr/bin/env python3 # gt2tum.pyKITTI poses.txt times.txt 转 TUM并把位姿搬到激光雷达坐标系 import numpy as np from scipy.spatial.transform import Rotation as R # 相机系(右-下-前) 到 激光系(前-左-上) 的固定旋转 R_cam_velo np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]], dtypefloat) p a hrefhttps://download.csdn.net/download/weixin_42101237/18720557 stylecolor:#ec7500;font-size:14px; 本文还有配套的精品资源点击获取 /a img altmenu-r.4af5f7ec.gif srchttps://csdnimg.cn/release/wenkucmsfe/public/img/menu-r.4af5f7ec.gif stylewidth:16px;margin-left:4px;vertical-align:text-bottom;cursor:text; /p