简介面向激光SLAM学习与KITTI数据评测需求这份kitti-lego-loam资源提供经修改的LeGO-LOAM代码可在KITTI数据集上直接运行并评估轨迹适合不熟悉ROS与LOAM、希望快速复现激光里程计实验的学生和工程师。压缩包共35个文件、约26.84MB以5个cpp和3个头文件承载核心算法2个launch与1个rviz配置负责启动和可视化另有README、PDF说明、msg消息定义及txt/xml等工程文件并配有少量图片、动图辅助理解目录结构与运行效果。当前已有6361人学习下载。使用后可获得KITTI地面格式轨迹并能借助EVO-eval套件对照真值完成精度评估代码同时给出GTSAM等依赖提示便于搭建实验环境、复现LeGO-LOAM流程、定位编译运行中的常见问题。1. 把 KITTI 的 velodyne 点云直接喂进 LeGO-LOAM跑出来的轨迹和 ground-truth 一叠ATE 动不动几十米——多数时候不是算法趴窝是数据链路在骗你kitti-lego-loam 这套东西解决的就是这条链路KITTI 数据怎么变成 LeGO-LOAM 认的点云、launch 里那几个参数在 KITTI 上该改成什么、跑完的轨迹怎么和 ground-truth 用 evo 对齐打分。适合已经能编过 ROS 工作空间、想拿公开数据集验证激光里程计精度的人新手照着步骤能跑出第一张 ATE 表熟手能对着参数表和坐标变换看边界。整套流程里最玄学的不是建图是格式对齐——点云的 ring 字段、逐点时间、bag 的时间基准、输出坐标系哪一环没对上最后的数字都没有意义。先把这层捅破后面 evo 报出来的 RMSE 才值得信。2. 数据落盘与格式对齐从 KITTI bin 到 ROS bag 的转换链路KITTI odometry 的一帧点云是一段裸二进制每点 4 个 float32x、y、z、intensity强度归一化到 0 到 1 之间。它没有 ring线号也没有逐点相对时间。而 LeGO-LOAM 的 imageProjection 恰恰依赖这两样东西做球面投影和分簇没有 ring相邻扫描线的关系就断了没有相对时间去畸变那段就只能瞎猜。这一章把「补齐字段、打 bag、转 GT」三件事拆开做。2.1 KITTI odometry 目录里哪些文件是硬需求先把目录看明白别一上来就写转换脚本。odometry 数据集里跟激光里程计相关的文件其实就那么几个缺哪个、后果是什么心里得有个表。路径内容缺了会怎样sequences/00/velodyne/*.bin每帧 4×float32 的点云没有数据可播直接卡死在这一步sequences/00/times.txt每帧时间戳秒浮点bag 的时间戳只能自己造evo 配对时全是错位sequences/00/calib.txt相机与雷达的标定矩阵纯激光模式可暂缺做点云投影或多传感器融合时必须有poses/00.txt3×4 位姿矩阵每行 12 个数没有 ground-truth只能看地图形态没法定量sequences/00/image_0、image_1灰度双目图LeGO-LOAM 纯激光不用可以整目录忽略有两个细节容易翻车。第一只有 00 到 10 这 11 个序列带 ground-truth11 到 21 是纯测试集别拿 11 号去算 ATE 然后怀疑人生。第二times.txt的时间往往是从 0 开始的相对秒直接塞进 ROS 时间会得到 stamp 为 0 的消息部分节点对零时间戳的处理并不友好实操里习惯加一个固定偏移比如 1.0 秒再发。2.2 用脚本把 bin 点云补齐 ring 与相对时间后打成 rosbag转换脚本的核心是三件事读 bin、算 ring、算逐点时间然后打包成sensor_msgs/PointCloud2。不要用现成的通用转换包一把梭字段顺序和 datatype 一旦不对rviz 里看着有点LeGO-LOAM 里就是一堆散点。#!/usr/bin/env python # -*- coding: utf-8 -*- import os import glob import numpy as np import rosbag from sensor_msgs.msg import PointCloud2, PointField N_SCAN 64 ANG_BOTTOM 24.8 # 最低线与水平面的夹角度HDL-64E 约 2 ~ -24.8 ANG_RES_Y 26.8 / (N_SCAN - 1) LINE_DURATION 0.1 # 一圈 10Hz转一圈 0.1 秒 def read_bin(path): # KITTI 每点 4 个 float32x y z intensity return np.fromfile(path, dtypenp.float32).reshape(-1, 4) def build_fields(): return [ 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), PointField(time, 18, PointField.FLOAT32, 1), ] def build_cloud(scan, stamp): x, y, z scan[:, 0], scan[:, 1], scan[:, 2] # 1) 俯仰角量化成线号clip 兜住边界避免越界 pitch np.degrees(np.arctan2(z, np.sqrt(x * x y * y))) ring np.clip(((ANG_BOTTOM - pitch) / ANG_RES_Y).astype(np.int32), 0, N_SCAN - 1) # 2) 逐点相对时间用方位角线性插值转一圈 0.1s az np.arctan2(y, x) t ((az np.pi) / (2 * np.pi) * LINE_DURATION).astype(np.float32) # 3) 按 x y z intensity ring time 顺序拼成结构化数组packed 后逐点 memcpy arr np.zeros(len(scan), dtype[ (x, np.float32), (y, np.float32), (z, np.float32), (intensity, np.float32), (ring, np.uint16), (time, np.float32)]) arr[x], arr[y], arr[z] x, y, z arr[intensity] scan[:, 3] arr[ring], arr[time] ring.astype(np.uint16), t msg PointCloud2() msg.header.stamp stamp msg.header.frame_id velodyne msg.height, msg.width 1, len(scan) msg.fields build_fields() msg.is_bigendian False msg.point_step 22 msg.row_step 22 * len(scan) msg.is_dense True msg.data arr.tobytes() return msg def main(seq_dir, bag_path, time_offset1.0): with rosbag.Bag(bag_path, w) as bag: times np.loadtxt(os.path.join(seq_dir, times.txt)) bins sorted(glob.glob(os.path.join(seq_dir, velodyne, *.bin))) for i, b in enumerate(bins): stamp times[i] time_offset msg build_cloud(read_bin(b), rospy_time(stamp)) bag.write(/velodyne_points, msg, msg.header.stamp)逻辑上分三层。第一层read_bin只负责把二进制摊平成 N×4 的数组reshape(-1, 4)里的 4 是硬约定KITTI 没有别的格式。第二层算字段ring 用俯仰角反推而不是查表HDL-64E 的垂直角分辨率约 26.8/63 度把俯仰角代入这个线性关系取整就能得到 0 到 63 的线号。这个近似在 KITTI 上足够用因为它的点云本身是理想化的。逐点时间用的是方位角线性插值本质是在假设一圈匀速旋转——KITTI 没给真实时间这是业界常见的近似够拿来跑通、不够拿来发表。第三层的几个参数值得单独说。point_step 22是 4 个 float32 加 1 个 uint16 加 1 个 float32字节数必须严格等于字段偏移量的总和写错一个字节 rviz 会直接花屏。time_offset就是我上面说的那个偏移默认给 1.0 秒。frame_id用velodyne只是习惯最终要和 launch 里的 TF 对上。2.3 ground-truth 位姿转成 evo 能直接读的两类格式KITTI 的poses/00.txt每行是 3×4 位姿矩阵的 12 个数行主序。evo 支持直接读这种格式evo_ape kitti但如果要做时间重采样或和别的轨迹插值转成 TUM 的timestamp tx ty tz qx qy qz qw更方便。import numpy as np from scipy.spatial.transform import Rotation as Rot def kitti_to_tum(pose_file, time_file, out_file, t_offset1.0): poses np.loadtxt(pose_file).reshape(-1, 3, 4) times np.loadtxt(time_file) with open(out_file, w) as f: for i in range(len(poses)): T poses[i] t T[:, 3] q Rot.from_matrix(T[:, :3]).as_quat() # 返回顺序正好是 x y z w f.write(%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n % (times[i] t_offset, t[0], t[1], t[2], q[0], q[1], q[2], q[3]))注意Rot.as_quat()返回的是(x, y, z, w)和 TUM 要求的顺序一致这一点很多人会下意识按(w, x, y, z)写反结果 evo 报出来的误差大得离谱却查不出原因。t_offset必须和点云那边用同一个值否则按时间关联时整条轨迹会错位。如果不想转 TUM直接用 kitti 格式也行代价是 evo 只能按行号配对帧数必须严格相等——这个坑在第五章展开。3. LeGO-LOAM 的四个参数块地面分割、聚类、特征、回环在 KITTI 上各该动哪里LeGO-LOAM 相对原始 LOAM 的改动集中在两处先用地面分割把点云分成地面和非地面再对非地面做聚类最后在簇内提特征。这套设计在结构化道路场景收益很大而 KITTI 恰好就是典型的城市与公路场景。但默认参数是照着 16 线雷达和室内外混合场景调的搬到 64 线的 KITTI 上好几个地方必须改否则要么地面提不出来要么分簇碎成一地。3.1 N_SCAN、Horizon_SCAN 与 groundScanInd球面投影的分箱基础这三个参数都在utility.h里是整条流水线的地基。N_SCAN是线数KITTI 是 HDL-64E填 64。ang_res_y是垂直角分辨率用26.8/(N_SCAN-1)算。ang_bottom是最低线相对水平面的角度HDL-64E 取 24.8。这三个基本没得商量。真正需要现场量的是Horizon_SCAN也就是水平方向一圈切多少个箱。切少了同一列里会挤进方位角差很远的点投影图糊成一片切多了大部分箱是空的特征提取时邻域搜索大量落空。KITTI 每圈的实际点数在 2000 上下浮动具体值跟场景有关常见做法是先统计再定import numpy as np scan np.fromfile(000000.bin, dtypenp.float32).reshape(-1, 4) az np.degrees(np.arctan2(scan[:, 1], scan[:, 0])) print(一帧点数:, len(scan)) print(0.2 度分箱后唯一列数:, len(np.unique(np.round(az / 0.2))))跑出来如果唯一列数接近 2000Horizon_SCAN就别用默认的 1800往上提到 2080 或 2250。这不是玄学投影时列索引超出会被回绕回绕就意味着两个不同方向的点被塞进同一个箱地面分割的连通性直接崩掉表现出来是地面被切成碎片、建图飘。宁大勿小是这个参数唯一的原则。groundScanInd决定哪几条线被当成地面。KITTI 的车顶安装高度和俯仰角决定了地面点主要落在下面 18 到 22 条线上我一般先填 20然后在 rviz 里看/ground_cloud话题地面点如果一直延伸到远处的墙面说明取大了如果近处地面都缺一块说明取小了往下调两条线再看。参数位置KITTI 取值调整依据N_SCANutility.h64雷达线数固定Horizon_SCANutility.h2080 ~ 2250每圈实际点数宁大勿小ang_res_yutility.h26.8/(N_SCAN-1)由线数和垂直视场推导ang_bottomutility.h24.8最低线俯角groundScanIndutility.h18 ~ 22看/ground_cloud调useCloudRingutility.htrue点云带 ring 字段时必须开3.2 地面分割与聚类两个阈值决定了特征点的质量分割逻辑在imageProjection.cpp里。同一列相邻两点如果它们之间的连线跟水平面的夹角小于某个阈值就判为同一类地面否则断开。聚类则是在非地面点上做基于距离的连通域生长相邻点距离超过阈值就分到不同簇。这两个阈值在源码里是写死的常量KITTI 上一般不用动但有两件事要确认。第一去畸变要不要开。KITTI odometry 的点云在发布时已经做过一定程度的运动补偿而 LeGO-LOAM 默认会拿每点的time字段再做一遍。开与不开的差别在直道上几乎看不出来在转弯和加减速段能差出几十厘米。我一般的做法是跑两遍同一段序列比 ATE取小的那个别凭感觉拍板。第二聚类的输出在featureAssociation里被用来限制特征提取的范围每个簇里只保留曲率最大的若干个边缘点和若干个平面点簇太小就直接跳过。KITTI 的远处点很稀一个簇可能只有三五个点阈值稍微一严就被整段丢掉表现出来是远处墙面没有特征、位姿在长直道上慢慢漂。这时候与其去改聚类阈值不如先把Horizon_SCAN调对很多问题会自己消失。3.3 回环检测与 GTSAM 后端车载高速场景要放宽搜索半径回环部分在mapOptimization.cpp核心是历史关键帧的搜索。几个参数在 KITTI 上需要重新掂量参数含义常用取值KITTI 上的考量surroundingkeyframeAddingDistThreshold新增关键帧的位移阈值米1.0车速约 10m/s1 米大约十帧一次合理surroundingkeyframeAddingAngleThreshold新增关键帧的角度阈值弧度0.2直道上几乎不触发转弯时主导surroundingKeyframeSearchRadius局部地图的搜索半径米50场景大时可以提到 80historyKeyframeSearchRadius回环候选的半径米15车载场景建议 20 ~ 25historyKeyframeSearchTimeDiff回环候选的最小时间差秒30防止把相邻帧错认成回环historyKeyframeFitnessScoreICP 配准的接受阈值0.3点云稀疏时可放宽到 0.5historyKeyframeSearchNum每次搜索的历史帧数25越大越慢先别动回环半径 15 米在室内或者园区里够用放到 KITTI 上就偏保守车开得快几秒钟就出去几十米回到同一地点的两次经过在时间上隔了很久空间上却可能差二十多米。把半径提到 20 到 25配合时间差阈值回环触发率会明显上来。但historyKeyframeFitnessScore别放松过头0.5 以上就容易把长得像的路口错配成回环GTSAM 优化完之后整张图会拧一下这种错误在地图上看不出来在 ATE 上是一根尖刺。3.4 launch 文件与话题重映射KITTI 上要砍掉 IMU 这条支路原版的启动文件会把 IMU 预积分节点一起拉起来它在等/imu/data。KITTI 没有 IMU节点起不来就发不出对应的 TF下游的transformFusion一直等表现出来是 odom 完全不动或者偶尔跳一下。纯激光跑 KITTI启动文件只留四个节点launch param name/use_sim_time valuetrue/ node pkglego_loam typeimageProjection nameimageProjection outputscreen/ node pkglego_loam typefeatureAssociation namefeatureAssociation outputscreen/ node pkglego_loam typemapOptimization namemapOptimization outputscreen/ node pkglego_loam typetransformFusion nametransformFusion outputscreen/ !-- bag 里的话题名不是 /velodyne_points 时在这里改 -- remap from/velodyne_points to/velodyne_points/ /launch/use_sim_time必须为 true原因是 bag 回放用的是录制时的时间基准节点如果还按墙钟走TF 的查询会一直报外推失败。这一条几乎是所有离线回放问题的源头先确认它再去查别的。remap那一行看着像废话但它的存在是提醒你bag 里的话题名、launch 里订阅的话题名、rviz 里加的点云话题三处必须一致转换脚本里写的是/velodyne_points那就以它为准。4. 跑通与量化从 rosbag play 到 evo 出 ATE/RPE 的完整命令链数据和参数都对齐之后剩下的就是把流程跑起来。这一段最容易出的问题不是算法是节奏LeGO-LOAM 单帧处理时间在普通工作站上可能超过 100 毫秒而 KITTI 是 10Hz 数据按原速播一定会丢帧。丢帧之后轨迹行数和 GT 对不上后面所有数字都白算。4.1 启动顺序与回放节奏先起 launch再放 bag两个终端分开别图省事写一个脚本里。launch 起来之后在 rviz 里确认 TF 树里map、camera_init、velodyne都在再放数据。# 终端一启动纯激光节点 source devel/setup.bash roslaunch lego_loam kitti_run.launch # 终端二以半速回放并发布仿真时钟 rosbag play kitti_00.bag -r 0.5 --clock --pause--clock配合/use_sim_time把 bag 的时间发布成/clock所有节点跟着它走。--pause让 bag 先停在第一帧这时候去 rviz 里看一眼点云是不是成圈的线状分布、有没有明显的飞点、/ground_cloud是不是贴着地面。确认没问题再按空格开始。-r 0.5是半速先保证不丢帧跑通之后再试 1.0看处理延迟还能不能跟上。4.2 订阅里程计话题并落盘成 TUM 轨迹LeGO-LOAM 的位姿发布在/aft_mapped_to_init类型是nav_msgs/Odometry坐标系是camera_init。用 rosbag record 记下来再离线转是一种办法但更省事的是直接写个订阅节点落盘时间戳和位姿一起写省掉二次转换。#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy from nav_msgs.msg import Odometry out open(lego_loam_00_cam.txt, w) def cb(msg): t msg.header.stamp.to_sec() p msg.pose.pose.position q msg.pose.pose.orientation out.write(%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n % (t, p.x, p.y, p.z, q.x, q.y, q.z, q.w)) out.flush() # 中途崩了也不至于一条都没留下 rospy.init_node(traj_saver) rospy.Subscriber(/aft_mapped_to_init, Odometry, cb, queue_size50) rospy.spin()flush()这一行是血泪经验跑长序列时节点偶尔因为内存或段错误挂掉没有 flush 的话缓冲区里的位姿全丢白跑半小时。queue_size给 50 是留点余量订阅端处理不过来的话消息会被丢弃轨迹出现时间跳变后面按时间关联就对不上了。写完之后检查一下行数和poses/00.txt的行数比一比差太多就说明中间丢帧了回去把-r降下来重跑。4.3 把 camera 系位姿转回 KITTI 系不转这一步ATE 永远差好几倍/aft_mapped_to_init在camera_init坐标系下轴向是 x 向右、y 向下、z 向前。KITTI 的 ground-truth 是 x 向前、y 向左、z 向上。这两套系之间差一个固定的旋转不是简单换个轴名必须用矩阵转。import numpy as np from scipy.spatial.transform import Rotation as Rot # kitti 系基向量在 camera 系下的表示按列拼起来 R_ck np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]], dtypefloat) def convert(src, dst): rows np.loadtxt(src) with open(dst, w) as f: for r in rows: t, p, q r[0], r[1:4], r[4:8] T np.eye(4) T[:3, :3] Rot.from_quat(q).as_matrix() T[:3, 3] p # 相似变换T_kitti R^T * T_cam * R T_k R_ck.T T R_ck q_k Rot.from_matrix(T_k[:3, :3]).as_quat() f.write(%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n % (t, T_k[0, 3], T_k[1, 3], T_k[2, 3], q_k[0], q_k[1], q_k[2], q_k[3])) convert(lego_loam_00_cam.txt, lego_loam_00_kitti.txt)R_ck的三列分别是 KITTI 的前、左、上三个方向在 camera 系里的坐标前对应 camera 的 z左对应负 x上对应负 y。相似变换写成R^T T R而不是R T是因为位姿是作用在坐标系上的左右两边都要换基。漏掉右边那个 R的后果很隐蔽位置看着大致对姿态全错ATE 的平移部分可能还行一到旋转相关的指标就离谱。转完之后拿前几帧对一下车应该沿一个轴单调前进不是斜着走。4.4 evo 的两条命令与三个必须搞清的对齐选项# ATE绝对位姿误差看整体轨迹和真值差多远 evo_ape kitti gt/00.txt est/lego_loam_00_kitti.txt \ -a --plot --save_results res_00_ape.zip # RPE相对位姿误差按每 10 米一段算局部位移漂移 evo_rpe kitti gt/00.txt est/lego_loam_00_kitti.txt \ -a --delta 10 --delta_unit m --plot-a是--align用 Umeyama 算法做 SE3 对齐只估旋转和平移不动尺度。这是评估激光里程计时应该用的选项因为同一台雷达、同一套坐标系尺度本来就该一致。千万不要顺手加--correct_scale它会额外估一个尺度因子把尺度误差吸收进对齐里ATE 数字立刻好看一大截但那是自欺欺人真实系统里的尺度问题被掩盖了。--delta 10 --delta_unit m是 RPE 的窗口按每 10 米位移算一段相对误差这个指标比 ATE 更能反映「走得越远漂得越多」。--save_results存的 zip 后面可以用evo_res重新出表做多组参数对比时不用重跑。另外 kitti 格式的轨迹是按行号配对的两张表的行数必须相等这是最容易被忽略的前置条件——下一章第一条就是这个。5. 避坑与排查五个把 ATE 拉爆的典型问题按现象、原因、解决拆开跑通一次不难难的是数字不对劲的时候知道往哪查。下面这几条都是同一类问题的不同表现数据本身没错错的是数据进入算法之前的形态。每条都按「现象 → 原因 → 解决」写方便对照自己遇到的症状。5.1 点云在 rviz 里成形但特征点碎成渣节点偶尔直接崩现象是/laser_cloud_sharp和/laser_cloud_flat两个话题上的点稀稀拉拉、没有沿结构分布跑久一点featureAssociation会段错误退出。原因基本锁定在useCloudRing上这个开关打开时代码会去读点云里的ring字段做相邻线判断而 bin 转 bag 时如果没有补这个字段读到的就是未定义内存。转 bag 脚本里字段顺序、point_step、datatype三者中任何一个写错都会走到这个结果。解决分两步先用rostopic echo /velodyne_points/fields确认ring和time都在offset和datatype对得上确认无误后如果还崩临时把useCloudRing设成false跑一遍能跑通就说明问题确实在字段上回头修脚本别就这么放着——关掉 ring 之后相邻线关系丢失精度会掉一截。5.2 前端几十米正常越走越弯ATE 到几十米量级现象是轨迹开头和 GT 基本重合走出一两百米之后整体向一侧弯最后回到起点时闭合误差巨大。原因通常是两个叠加一是Horizon_SCAN偏小每圈点被压缩后分箱回绕导致远处点的方位被算错位姿解算时被一点点带偏二是没做 4.3 里的坐标变换两套轴向混着比误差随距离线性增长。排查顺序是先看坐标把估计轨迹和 GT 画在同一张俯视图上如果形状相似只是整体旋转了那是轴向问题几十行代码就能修好如果形状本身就变形回到Horizon_SCAN用 3.1 里的统计脚本量一遍每圈点数把值提上去重跑。5.3 完整 launch 起得来但 odom 完全不动或者间歇性乱跳现象是 rviz 里点云正常刷新/aft_mapped_to_init却一直是同一个值或者偶尔跳一下又停住。原因几乎都是 IMU 预积分节点在等一个永远不会来的/imu/data它不发 TF下游的transformFusion拿不到变换就卡住。KITTI 是纯激光数据没有 IMU这条支路必须砍掉。解决就是用 3.4 里那个四节点的 launch把imuPreintegration从启动文件里删干净。删完之后如果 odom 还是不动去看rosnode list里mapOptimization是否还活着它挂掉的话通常伴随GTSAM的断言失败那是另一个问题回去看 3.3 里的回环阈值是不是放得太松。5.4 evo 报两张轨迹长度不一致直接拒绝计算现象是evo_ape报trajectories have different number of poses或者算出来的结果莫名其妙。原因是 kitti 格式按行号配对LeGO-LOAM 在某些帧点太少、解不出位姿时会跳过输出估计轨迹比 GT 少几行。解决有两条路一是按时间最近邻重采样到同一时间轴再比二是直接截断到较短的一侧。前者更严谨后者更快。我一般在保存轨迹的节点里就把queue_size和 flush 做好尽量不丢帧同时跑完先wc -l对一下行数行数不等就别急着跑 evo先查丢在哪一段。5.5 ATE 只有零点几米RPE 随距离爆炸现象是 ATE 数字漂亮得不像话一看 RPE 曲线每 10 米段的误差随里程稳步上升。原因八成是 evo 命令里加了--correct_scale或者用了某种带尺度估计的对齐方式把尺度误差吸收到对齐变换里了。纯激光、同传感器本文还有配套的精品资源点击获取