KITTI数据集适配LeGO-LOAM：从跑通到跑准的实战拆解

简介这份资源是面向LiDAR SLAM研究与自动驾驶感知学习者的LEGO-LOAM适配版本针对Kitti数据集在数据读取、时间戳同步、特征提取与点云匹配等环节做了针对性修改便于在真实场景数据上复现激光雷达里程计与建图流程。压缩包共23个文件约26.74MB以cpp源码与h头文件为核心辅以launch启动配置、msg消息定义、rviz可视化配置、xml参数文件并附有md说明、pdf论文预印本及少量jpg、png、gif示意图整体结构接近完整可编译工程。目前已有648人学习下载适合具备一定ROS与SLAM基础、希望快速上手Kitti实验的读者。拿到后可直接对照源码理解算法改动点借助论文与说明文档梳理原理并基于现有配置开展定位精度与建图效果的验证和二次开发。1. 为适配 KITTI 数据集修改的 LeGO-LOAM从跑通到跑准的实战拆解手里有一份 KITTI 的 rosbag想直接丢进 LeGO-LOAM 跑一遍结果要么启动就崩要么建出来的图歪得没法看——这是很多做激光 SLAM 的人第一次接触「为适配 KITTI 数据集修改的 LeGO-LOAM」时最真实的场景。LeGO-LOAM 本身是为地面车辆设计的轻量级激光里程计与建图方案靠地面分割和聚类把特征点分得干净在 KITTI 这种城市道路数据上本来应该很合适。但原版代码的默认配置、话题名、外参、点云格式都是按作者自己的传感器标定的直接拿来跑 KITTI 大概率翻车。这个标题讲的就是把 LeGO-LOAM 的输入接口、参数和坐标系对齐到 KITTI 的 Velodyne HDL-64E 数据上让它能稳定输出轨迹和点云地图。适合已经会编译 ROS 包、手里有 KITTI raw data 或 odometry benchmark 数据、想拿 LeGO-LOAM 做基线对比或二次开发的人。2. KITTI 数据与 LeGO-LOAM 的接口差异先搞清楚哪里对不上2.1 KITTI 点云格式和 LeGO-LOAM 期望的输入KITTI raw data 里的 Velodyne 点云是二进制 bin 文件每条扫描线按x, y, z, intensity四个 float32 存储没有 ring 字段也没有时间戳字段。LeGO-LOAM 的imageProjection节点订阅的是sensor_msgs/PointCloud2内部按 Velodyne 的 ring 来划分扫描线默认 N_SCAN16、Horizon_SCAN1800。KITTI 用的是 HDL-64E64 线水平分辨率约 0.08° 到 0.35° 不等一圈大约 2083 个点每线取决于旋转速度。如果你直接把 KITTI 的 bin 转成 PointCloud2 而不补 ringLeGO-LOAM 的imageProjection会按角度硬算 ring算出来的线号和真实线号对不上地面分割就会把该留的点删掉或者把墙面点当成地面。常见做法是写一个转换脚本读 bin 文件按垂直角度反推 ring再填进 PointCloud2 的 ring 字段。KITTI 的 HDL-64E 垂直角分布不是均匀的上 32 线和下 32 线角度间隔不同所以反推 ring 的时候要用查找表不能简单线性除。我一般会先把 HDL-64E 的 64 个垂直角度从标定文件里读出来排序后做最近邻匹配。import numpy as np import struct # HDL-64E 垂直角度度从 KITTI 标定文件或 Velodyne 手册获取 VERT_ANGLES np.array([...]) # 64 个值升序排列 def bin_to_points(bin_path): points np.fromfile(bin_path, dtypenp.float32).reshape(-1, 4) x, y, z points[:, 0], points[:, 1], points[:, 2] # 计算每个点的垂直角度 vert_angle np.degrees(np.arctan2(z, np.sqrt(x**2 y**2))) # 最近邻匹配到 64 线 ring np.argmin(np.abs(vert_angle[:, None] - VERT_ANGLES[None, :]), axis1) return points, ring.astype(np.uint16)这段代码的关键是VERT_ANGLES必须准确。KITTI 的 HDL-64E 标定文件里给了每个激光头的垂直角度直接拿来用。如果手头没有标定文件可以用 Velodyne 官方 HDL-64E 的标称角度但会有 0.1° 到 0.3° 的偏差跑短距离问题不大跑长距离会累积。ring算完之后要作为 PointCloud2 的一个字段发出去LeGO-LOAM 的imageProjection里useCloudRing参数要设成 true否则它还是按角度硬算。2.2 话题名、坐标系和外参的对应关系LeGO-LOAM 默认订阅/velodyne_points发布/lio_sam/deskew/cloud_deskewed之类的中间话题。KITTI 的 rosbag 里话题名通常是/kitti/velo/pointcloud或者你自己转的时候定的名字。最省事的做法是在 launch 文件里把imageProjection的subscribe_topic改成你的话题名而不是去改代码。外参方面LeGO-LOAM 假设激光雷达坐标系是 x 向前、y 向左、z 向上KITTI 的 Velodyne 坐标系是 x 向前、y 向左、z 向上这一点是一致的所以外参矩阵基本是单位阵。但 KITTI 的相机和激光雷达之间有标定外参如果你要把点云投影到图像上做验证那个外参是另一回事不影响 LeGO-LOAM 本身。坐标系上容易踩的坑是 KITTI 的 ground truth 位姿。KITTI odometry benchmark 给的 poses 是 3x4 的变换矩阵从相机坐标系出发的。如果你要拿 LeGO-LOAM 的输出和 ground truth 对比得先把 LeGO-LOAM 的轨迹从雷达坐标系转到相机坐标系或者把 ground truth 转到雷达坐标系。我一般会在评估脚本里统一转到雷达坐标系因为 LeGO-LOAM 输出的是雷达位姿。2.3 参数文件里必须改的几项LeGO-LOAM 的utility.h里有一堆编译期常量改完要重新编译。KITTI 适配最关键的几个参数原版默认KITTI 适配值说明N_SCAN1664激光线数Horizon_SCAN18002083每圈水平点数按 KITTI 实际值ang_res_y2.00.4垂直角分辨率HDL-64E 约 0.4°ang_bottom15.024.8最下激光角度HDL-64E 约 -24.8°groundScanInd720地面扫描线索引64 线要调大useCloudRingfalsetrue用 ring 字段而不是硬算Horizon_SCAN如果设小了点云会被截断建图会缺一块设大了浪费内存。KITTI 的 HDL-64E 在 10Hz 旋转下每圈大约 2083 个点但不同序列可能略有差异可以先用rosbag info看一帧点云的实际点数再定。groundScanInd决定哪些线被当成地面候选16 线的时候 7 差不多64 线要调到 20 左右否则地面点太少groundRemoval会把非地面点误判。3. 把 KITTI bin 转成 LeGO-LOAM 能吃的 rosbag完整操作链3.1 转换脚本的编写与运行KITTI raw data 下载下来是 drive 文件夹里面有velodyne_points/data/*.bin和timestamps.txt。要转成 rosbag核心是读 bin、补 ring、填时间戳、写 PointCloud2。下面是一个可复现的 Python 脚本骨架依赖rosbag、sensor_msgs、numpy。import rosbag import rospy from sensor_msgs.msg import PointCloud2, PointField import sensor_msgs.point_cloud2 as pc2 import numpy as np import os def make_pointcloud2(points, ring, stamp): msg PointCloud2() msg.header.stamp stamp msg.header.frame_id velodyne msg.height 1 msg.width points.shape[0] msg.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), PointField(ring, 16, PointField.UINT16, 1), ] msg.is_bigendian False msg.point_step 18 # 44442 msg.row_step msg.point_step * msg.width msg.is_dense True # 按字段打包 buf [] for i in range(points.shape[0]): buf.append(struct.pack(ffffH, points[i,0], points[i,1], points[i,2], points[i,3], ring[i])) msg.data b.join(buf) return msg def convert_drive(drive_path, bag_path): bag rosbag.Bag(bag_path, w) bin_dir os.path.join(drive_path, velodyne_points, data) times [line.strip() for line in open(os.path.join(drive_path, velodyne_points, timestamps.txt))] for i, bin_file in enumerate(sorted(os.listdir(bin_dir))): if not bin_file.endswith(.bin): continue points np.fromfile(os.path.join(bin_dir, bin_file), dtypenp.float32).reshape(-1, 4) ring compute_ring(points) # 用 2.1 的最近邻方法 t rospy.Time.from_sec(float(times[i])) msg make_pointcloud2(points, ring, t) bag.write(/kitti/velo/pointcloud, msg, t) bag.close()point_step是 18 字节因为 x/y/z/intensity 各 4 字节ring 是 uint16 占 2 字节。struct.pack的格式字符串ffffH对应这五个字段。时间戳从timestamps.txt读KITTI 的时间戳是 UTC 秒数直接转成rospy.Time就行。写 bag 的时候话题名用/kitti/velo/pointcloud后面 launch 文件里对应改。3.2 launch 文件的关键修改LeGO-LOAM 的run.launch里要改三处imageProjection的订阅话题、featureAssociation的订阅话题、以及mapOptimization的保存路径。订阅话题改成/kitti/velo/pointcloud保存路径改成你有写权限的目录。另外imageProjection的useCloudRing参数在utility.h里改完要catkin_make重新编译。launch node pkglego_loam typeimageProjection nameimageProjection outputscreen param namesubscribe_topic value/kitti/velo/pointcloud/ /node node pkglego_loam typefeatureAssociation namefeatureAssociation outputscreen param namesubscribe_topic value/lio_sam/deskew/cloud_deskewed/ /node node pkglego_loam typemapOptimization namemapOptimization outputscreen param namesave_path value/home/user/kitti_map// /node /launchfeatureAssociation订阅的是imageProjection输出的去畸变点云话题名不用改除非你在imageProjection里改了发布名。mapOptimization的save_path要提前建好文件夹否则保存点云的时候会静默失败这个坑我踩过好几次。3.3 跑通第一帧的验证方法启动roslaunch lego_loam run.launch之后先别急着看建图效果用rostopic hz /kitti/velo/pointcloud确认点云频率是不是 10Hz。然后用rviz加PointCloud2显示/lio_sam/deskew/cloud_deskewed看地面分割后的点云是不是只剩地面和少量非地面点。如果地面点几乎没了说明groundScanInd设小了如果非地面点里混了大量地面点说明groundScanInd设大了。调这个参数的时候一次改 2 到 3别一次跳太多。另一个验证点是mapOptimization输出的/lio_sam/mapping/odometry用rostopic echo看位姿的 z 值是不是在 0 附近小幅波动。如果 z 值一直往上飘说明地面分割没把地面找对或者外参有旋转。KITTI 的 HDL-64E 安装是水平的外参旋转基本是单位阵但如果你的 bag 里 frame_id 和 LeGO-LOAM 期望的不一致TF 树会出问题z 就会飘。4. 避坑与排查KITTI 适配 LeGO-LOAM 的 5 个血泪教训4.1 现象启动后点云不动rviz 里只有一帧原因imageProjection的subscribe_topic没改对或者 bag 里的话题名和 launch 里不一致。KITTI 转出来的 bag 话题名如果是/kitti/velo/pointcloudlaunch 里还是/velodyne_points节点就收不到数据。解决rostopic list看实际话题名rostopic echo确认有数据再改 launch。4.2 现象建图轨迹整体旋转了 90 度原因KITTI 的 Velodyne 坐标系和 LeGO-LOAM 期望的坐标系在 y 轴方向有差异。KITTI 的 Velodyne 是 x 向前、y 向左、z 向上LeGO-LOAM 也是这个约定但如果你的转换脚本里把 y 和 z 搞反了或者 bag 的 frame_id 对应的 TF 有旋转就会整体转。解决检查转换脚本里points[:, 1]和points[:, 2]有没有写反检查 TF 树里velodyne到base_link的变换。4.3 现象地面分割后地面点大量丢失原因groundScanInd设小了。16 线的时候 7 够用64 线的时候地面扫描线索引要到 20 左右。另外ang_bottom如果还是 15.0最下面的线会被截掉。解决把groundScanInd调到 20ang_bottom调到 24.8重新编译。4.4 现象跑长序列后轨迹漂移严重原因KITTI 的 HDL-64E 点云密度比 16 线高很多featureAssociation里的特征提取阈值没调导致提取的特征点太多或太少。原版edgeThreshold和surfThreshold是按 16 线调的64 线要适当调大。解决把edgeThreshold从 0.1 调到 0.2surfThreshold从 0.1 调到 0.15观察特征点数量。4.5 现象保存的点云地图是空的原因mapOptimization的save_path目录不存在或者没有写权限。LeGO-LOAM 保存点云的时候如果目录不存在不会报错直接静默失败。解决提前mkdir -p建好目录确认当前用户有写权限。5. 进阶用 KITTI ground truth 定量评估 LeGO-LOAM 轨迹跑通只是第一步真正要判断适配得好不好得拿 KITTI 的 ground truth 做定量评估。KITTI odometry benchmark 的 poses 文件是 3x4 矩阵每行一个位姿从相机坐标系出发。评估的时候要把 LeGO-LOAM 输出的雷达位姿和 ground truth 对齐到同一坐标系然后算绝对轨迹误差ATE和相对位姿误差RPE。我一般用 evo 这个工具先把 LeGO-LOAM 的/lio_sam/mapping/odometry录成 bag 或者存成 TUM 格式再把 KITTI 的 poses 转成 TUM 格式。转换的时候注意 KITTI 的 poses 是相机坐标系要左乘一个雷达到相机的外参矩阵。这个外参在 KITTI 的 calib 文件里有通常是R|t的形式。import numpy as np # KITTI 相机到雷达的外参从 calib 文件读 T_cam_velo np.array([...]) # 4x4 T_velo_cam np.linalg.inv(T_cam_velo) def kitti_poses_to_tum(poses_path, tum_path): poses np.loadtxt(poses_path).reshape(-1, 3, 4) with open(tum_path, w) as f: for i, pose in enumerate(poses): T np.eye(4) T[:3, :4] pose T_velo T_velo_cam T T_cam_velo # 转到雷达坐标系 t T_velo[:3, 3] q rotation_matrix_to_quaternion(T_velo[:3, :3]) f.write(f{i*0.1:.6f} {t[0]:.6f} {t[1]:.6f} {t[2]:.6f} f{q[0]:.6f} {q[1]:.6f} {q[2]:.6f} {q[3]:.6f}\n)时间戳按 10Hz 算每帧间隔 0.1 秒。四元数转换用scipy.spatial.transform.Rotation就行不用自己写。转完之后用evo_ape tum ground_truth.txt lego_loam.txt -va --plot看 ATEKITTI 00 序列上调好的 LeGO-LOAM 大概能到 1% 到 2% 的漂移比原版 16 线配置好不少但比 LOAM 原版还是差一点因为 LeGO-LOAM 的地面分割在 64 线上会丢掉一些地面特征。评估的时候有个细节LeGO-LOAM 输出的第一帧位姿是单位阵ground truth 的第一帧也是单位阵但两者的时间戳起点可能差几帧。用 evo 的时候加--t_max_diff 0.02做时间对齐不然会对不上。另外 KITTI 的 00 序列有闭环LeGO-LOAM 没有闭环检测所以 ATE 会随着距离累积这是算法本身的限制不是适配的问题。我自己的习惯是每次改完参数先跑 00 序列的前 500 帧看 ATE 有没有下降再跑全长。这样迭代快不用每次等完整序列跑完。调参的时候一次只改一个改完记录 ATE不然出了问题不知道是哪个参数导致的。希望帮到你。本文还有配套的精品资源点击获取