简介这份代码包面向基于3D激光雷达做SLAM、需要为ICP或NDT匹配提供先验的C开发者聚焦校准激光雷达与IMU之间的外参姿态分量。其思路是将平移量通常置零重点求解旋转关系适合已具备ROS与点云匹配基础、希望补齐多传感器标定环节的读者参考。压缩包共7个文件约10KB包含2个cpp源码、1个头文件、1个launch启动配置、package.xml与CMakeLists.txt构建文件及README说明结构紧凑便于快速定位标定节点、参数配置与编译入口。已有5902人学习或下载说明该方向具有稳定的实践需求。包内围绕calib_exR_lidar2imu节点组织读者可据此理解外参标定程序的模块划分、启动方式与依赖准备并结合ndt_omp环境、rosbag录制的IMU和激光雷达数据开展实验获取从代码结构到标定流程的完整参考。1. 外参差 1 厘米点云重影一片这套 C 标定包解决什么3D 激光雷达和 IMU 用支架锁在一起两个坐标系之间的旋转平移就是外参。这个数靠卷尺和目测填融合建图时墙面会分成两层转弯处拖尾跑久了漂移。lidar_imu_calib是一套 C 实现从一段 rosbag 里自动估计 LiDAR 到 IMU 的外参把手工试凑变成可复现的求解流程。它适合做手持、背包、机器人平台多传感器融合的人尤其是已经能跑通雷达里程计、但被外参反复折磨的团队。标题里说的“外在性”落到工程里就是这 6 个自由度3 个旋转、3 个平移外加一个常被忽略的时间偏移。读完你至少知道bag 怎么录、两条位姿链怎么建、C 里怎么把 AXXB 解出来、结果怎么验。2. 手眼标定框架AXXB 怎么落到 LiDAR 与 IMU 的位姿对上2.1 为什么外参标定本质是手眼问题先把坐标系约定说死LiDAR 系记 LIMU 系记 I世界系记 W。待求外参 X 表示把 LiDAR 系下的点变到 IMU 系也就是 T_IL。任意时刻 k两个传感器对同一段运动的描述满足 T_WL_k T_WI_k · X含义是“雷达在世界里的位姿等于 IMU 位姿再串一个固定的杆臂变换”。这个等式在整段数据里都成立因为传感器是刚性连接的X 不随时间变。取相邻两个时刻 i 和 j把上面的等式左右两边各自做相对运动消掉世界系得到 T_WI_i⁻¹·T_WI_j X·T_WL_i⁻¹·T_WL_j·X⁻¹。令 A T_WI_i⁻¹·T_WI_jB T_WL_i⁻¹·T_WL_j就得到 A·X X·B。这就是手眼标定里最经典的 AXXB 形式A 是 IMU 在 i 到 j 之间的相对位姿B 是 LiDAR 在同一时间段里的相对位姿X 是待求外参。把旋转和平移拆开旋转部分满足 R_A·R_X R_X·R_B平移部分满足 (R_A − I)·t_X R_X·t_B − t_A。这里有个容易翻车的点A 和 B 必须是同一时间区间上的相对运动。IMU 通常 100 Hz 以上LiDAR 只有 10 Hz 左右不能拿“第 k 帧 IMU”直接对“第 k 帧点云”而要按 LiDAR 帧的时间戳去 IMU 轨迹上插值。很多标定结果飘不是求解器写错了而是这一步的时间基准就歪了。2.2 两条位姿链的构建与数据接口LiDAR 端要得到的是每一帧点云在某一参考系下的位姿 B_i。常见做法是用 NDT 或 GICP 做帧间配准串成里程计如果已经有 LOAM 系列的前端也可以直接拿它的里程计输出。这里的关键不是配准算法本身而是输出的位姿要和点云帧严格一一对应时间戳用点云的 header.stamp而不是 ROS 收到消息的墙上时间。IMU 端要得到 A_i。可以用预积分也可以先用角速度和加速度积分出姿态与位移再做相邻帧相对位姿。对手眼标定来说旋转链的精度比平移链更关键因为平移方程里 R_X 是乘上去的旋转一旦偏了平移会被放大。工程上常见的处理是IMU 端先用 Allan 方差标出零偏和噪声标定时把零偏作为已知量扣掉剩下的随机游走靠短时间窗口压住。两条链建好后需要一个统一的数据结构来对齐。我一般用下面这个最小结构时间戳统一用 double 秒旋转用四元数平移用 Vector3dstruct PoseStamped { double stamp; Eigen::Quaterniond q; // 旋转需归一化 Eigen::Vector3d t; // 平移单位米 }; // 在轨迹上按时间戳插值旋转用 slerp平移线性插值 bool lookupPose(const std::vectorPoseStamped traj, double stamp, PoseStamped out) { if (traj.empty() || stamp traj.front().stamp || stamp traj.back().stamp) return false; auto it std::lower_bound(traj.begin(), traj.end(), stamp, [](const PoseStamped p, double s) { return p.stamp s; }); if (it traj.begin()) { out *it; return true; } const PoseStamped p1 *(it - 1); const PoseStamped p0 *it; double dt p0.stamp - p1.stamp; double alpha (dt 1e-9) ? (stamp - p1.stamp) / dt : 0.0; out.stamp stamp; out.q p1.q.slerp(alpha, p0.q).normalized(); out.t p1.t alpha * (p0.t - p1.t); return true; }lookupPose里做了三件事越界检查、二分查找定位区间、按比例插值。alpha一定要做除零保护否则在仿真数据或重复时间戳的 bag 里会直接产生 NaN后面整个优化都会炸。slerp之后重新归一化是习惯动作四元数在多次插值后数值漂移很常见。对齐时还有一个隐藏参数插值容差。如果某个 LiDAR 帧的时间戳落在 IMU 轨迹两端之外或者最近邻 IMU 帧离它超过 20 ms我一般直接丢弃这一帧而不是硬插值。丢几帧对 200 组以上的方程没影响硬插值会把错误带进 A 矩阵。2.3 一个最小可跑的求解骨架旋转部分用四元数法最稳把 q_A ⊗ q_X q_X ⊗ q_B 写成 (L(q_A) − R(q_B))·q_X 0堆叠所有帧后取最小奇异值对应的向量。L 和 R 分别是四元数左乘、右乘矩阵展开后都是 4×4 的常数矩阵Eigen::Matrix4d QLeft(const Eigen::Quaterniond q) { Eigen::Matrix4d m; m q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), -q.z(), q.y(), q.y(), q.z(), q.w(), -q.x(), q.z(), -q.y(), q.x(), q.w(); return m; } Eigen::Matrix4d QRight(const Eigen::Quaterniond q) { Eigen::Matrix4d m; m q.w(), -q.x(), -q.y(), -q.z(), q.x(), q.w(), q.z(), -q.y(), q.y(), -q.z(), q.w(), q.x(), q.z(), q.y(), -q.x(), q.w(); return m; } Eigen::Quaterniond solveRotation( const std::vectorstd::pairEigen::Quaterniond, Eigen::Quaterniond pairs) { Eigen::Matrix4d M Eigen::Matrix4d::Zero(); for (const auto p : pairs) { Eigen::Matrix4d D QLeft(p.first) - QRight(p.second); M D.transpose() * D; // 构造零空间目标 } Eigen::SelfAdjointEigenSolverEigen::Matrix4d solver(M); Eigen::Vector4d v solver.eigenvectors().col(0); // 最小特征值对应向量 return Eigen::Quaterniond(v[0], v[1], v[2], v[3]).normalized(); }这段代码的逻辑是每一组观测贡献一个 4×4 的残差矩阵 D把所有 D 的平方和堆成 MM 的最小特征向量就是让总残差最小的 q_X。col(0)取的是最小特征值对应的列因为SelfAdjointEigenSolver按特征值升序排列。四元数的正负号不影响旋转最后统一归一化即可。旋转解出来后平移就是一个线性最小二乘问题。把每组方程 (R_A − I)·t_X R_X·t_B − t_A 按行堆起来得到 H·t_X g用colPivHouseholderQr()解。这里要检查 H 的条件数如果旋转激励不够R_A − I 接近奇异解出来的平移会对输入噪声极其敏感。我的习惯是条件数大于 1e3 就拒绝出结果回去补数据而不是硬着头皮用。3. 数据采集与预处理rosbag 里该有什么、怎么筛3.1 话题、频率与运动激励的硬指标标定质量的上限在录 bag 那一刻就定了后面求解器再花哨也救不回来。下面这张表是我自己录数据时对照的清单不是某个包的官方参数而是踩过几次坑之后总结的底线项目建议说明LiDAR 话题sensor_msgs/PointCloud210 Hz 左右确认frame_id是雷达本体不是经过 TF 变换后的系IMU 话题sensor_msgs/Imu100–200 Hz角速度和线加速度都要有协方差字段别全填 0录制时长2–3 分钟前后各留 10 秒静止段方便估零偏角速度幅值不超过量程的 70%超过会饱和饱和段必须切掉运动形式绕三个轴各来回转两次以上纯平移对旋转外参没有激励平移幅度0.5–2 米太小信噪比差太大点云配准容易丢实际录制时手持或背包平台先静止十几秒然后绕竖直轴左右各转一圈再绕另外两个轴各俯仰、横滚两次最后走一段 S 形。每个动作之间停半秒让点云配准有时间收敛。这套动作看起来笨但它保证了 A 和 B 在三个旋转方向上都有足够的分量不会退化成某个轴上的欠定问题。3.2 用脚本切分与检查 bag录完之后不要直接丢进标定节点先看 bag 的基本信息再把静止段和明显碰撞、急停的段切掉# 录包时只录必要话题避免把图像、GPS 全塞进来 rosbag record -O calib_raw.bag /velodyne_points /imu/data # 看话题、消息数、起止时间 rosbag info calib_raw.bag # 按时间窗裁剪单位是秒具体数值按 info 里的起止时间改 rosbag filter calib_raw.bag calib_cut.bag \ t.to_sec() 100.0 and t.to_sec() 220.0rosbag info重点看两件事两个话题的消息数比例是否接近频率比以及 bag 的总时长是否覆盖了完整动作。如果 IMU 消息数明显偏少先查驱动是不是在录包开始后才启动这种半截数据后面插值会大量丢帧。再用一段 Python 把时间戳拉出来看比肉眼翻rostopic echo靠谱得多import rosbag from collections import defaultdict bag rosbag.Bag(calib_cut.bag) stamps defaultdict(list) for topic, msg, _ in bag.read_messages(topics[/velodyne_points, /imu/data]): stamps[topic].append(msg.header.stamp.to_sec()) lidar sorted(stamps[/velodyne_points]) imu sorted(stamps[/imu/data]) print(lidar frames:, len(lidar), imu msgs:, len(imu)) # 雷达帧间隔正常应在 0.1 s 附近突变说明有丢帧 dts [round(b - a, 4) for a, b in zip(lidar, lidar[1:])] print(lidar dt sample:, dts[:10]) # IMU 时间戳必须严格递增倒序或重复会让插值直接出错 bad [(a, b) for a, b in zip(imu, imu[1:]) if b a] print(imu order issues:, len(bad))这段脚本不依赖任何标定包纯粹做数据体检。lidar dt里出现 0.2 秒以上的间隔说明中间丢了帧配准里程计会在那里断掉imu order issues不为零说明驱动或录包环节有时间戳回跳必须先修数据再谈标定。很多人把标定失败归咎于算法实际问题在 bag 里这种黑匣子式的排查顺序能省掉大量无效调参。3.3 点云去畸变与里程计输出LiDAR 一帧不是瞬时曝光而是转一圈扫出来的。平台在转点云就会畸变尤其是角速度大的那段墙面会变成弧面。常见做法是用 IMU 的角速度积分出帧内每个点的相对姿态把点云统一到帧起始时刻。此时外参还没标出来可以先用 CAD 或卷尺量一个粗略值顶着去畸变对外参的敏感度远低于配准本身。里程计输出建议单独存成一个文本文件每行timestamp qx qy qz qw tx ty tz不要依赖 ROS 话题回放。原因是标定过程可能要反复调参、换时间段每次回放 bag 都重新跑一遍配准很浪费时间。把位姿落盘之后标定节点只读文本迭代速度会快一个量级。NDT 的参数我一般从分辨率 1.0 米、最大迭代 30 次、步长 0.1 开始试。分辨率太小室外稀疏点云会配准失败太大帧间运动估计不准。判断配准有没有问题不看残差数值看轨迹有没有跳变把里程计轨迹画出来出现折线式的尖角基本就是某几帧配准跑飞了这几帧要在标定输入里剔掉。4. 求解与优化旋转、平移、时间偏移三个变量的处理顺序4.1 旋转先解、平移后解的理由AXXB 的旋转方程 R_A·R_X R_X·R_B 只含旋转和平移完全解耦平移方程 (R_A − I)·t_X R_X·t_B − t_A 里R_X 一旦已知t_X 就是线性的。这个结构决定了合理的求解顺序先解旋转再解平移。反过来的话平移的误差会污染旋转的初值非线性优化容易收敛到局部极小。时间偏移要放在旋转之前粗搜。IMU 和 LiDAR 的时间戳如果差 20 ms在快速旋转段这个偏差足以让 R_A 和 R_B 对不上旋转残差会明显抬高。我一般先固定外参初值在 [−0.1, 0.1] 秒范围内以 5 ms 步长扫一遍 dt每步重新插值 IMU 位姿、重算旋转残差取残差最小的一段作为 dt 初值。这一步不需要精确只要把量级找对后面联合优化才不会跑偏。4.2 Ceres 里的残差与参数块旋转初值有了之后把旋转和平移一起丢进 Ceres 做联合优化残差 6 维3 维旋转误差加 3 维平移误差。旋转误差用 R_A·R_X·R_Bᵀ·R_Xᵀ 偏离单位阵的程度来度量取反对称部分的 vee 映射数值上比直接取角度更平滑struct HandEyeCost { HandEyeCost(const Eigen::Matrix3d RA, const Eigen::Vector3d tA, const Eigen::Matrix3d RB, const Eigen::Vector3d tB) : RA_(RA), tA_(tA), RB_(RB), tB_(tB) {} template typename T bool operator()(const T* const q, const T* const t, T* residual) const { // 四元数转旋转矩阵q 顺序为 w,x,y,z T w q[0], x q[1], y q[2], z q[3]; Eigen::MatrixT, 3, 3 RX; RX 1 - 2*(y*y z*z), 2*(x*y - z*w), 2*(x*z y*w), 2*(x*y z*w), 1 - 2*(x*x z*z), 2*(y*z - x*w), 2*(x*z - y*w), 2*(y*z x*w), 1 - 2*(x*x y*y); Eigen::MatrixT, 3, 3 RA RA_.castT(); Eigen::MatrixT, 3, 3 RB RB_.castT(); Eigen::MatrixT, 3, 3 Rerr RA * RX * RB.transpose() * RX.transpose(); // 旋转残差取反对称矩阵的 vee residual[0] T(0.5) * (Rerr(2,1) - Rerr(1,2)); residual[1] T(0.5) * (Rerr(0,2) - Rerr(2,0)); residual[2] T(0.5) * (Rerr(1,0) - Rerr(0,1)); // 平移残差(RA - I) * tX - (RX * tB - tA) Eigen::MatrixT, 3, 1 tX(t[0], t[1], t[2]); Eigen::MatrixT, 3, 1 tA tA_.castT(); Eigen::MatrixT, 3, 1 tB tB_.castT(); Eigen::MatrixT, 3, 1 r (RA - Eigen::MatrixT,3,3::Identity()) * tX - (RX * tB - tA); residual[3] r(0); residual[4] r(1); residual[5] r(2); return true; } Eigen::Matrix3d RA_, RB_; Eigen::Vector3d tA_, tB_; };调用侧把四元数作为 4 维参数块挂上流形平移单独 3 维double q[4] {q_init.w(), q_init.x(), q_init.y(), q_init.z()}; double t[3] {t_init.x(), t_init.y(), t_init.z()}; ceres::Problem problem; for (const auto pair : pairs) { problem.AddResidualBlock( new ceres::AutoDiffCostFunctionHandEyeCost, 6, 4, 3( new HandEyeCost(pair.RA, pair.tA, pair.RB, pair.tB)), new ceres::HuberLoss(1.0), q, t); // Huber 压外点 } problem.SetManifold(q, new ceres::EigenQuaternionManifold()); ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_QR; options.max_num_iterations 100; options.function_tolerance 1e-10;HuberLoss(1.0)是给配准失败的帧留的后悔药残差大于阈值后权重下降不让一两帧坏数据把整体拉偏。SetManifold是较新的接口老版本 Ceres 对应的是SetParameterization两者语义一样换环境时注意编译报错。DENSE_QR适合方程数在几百组以内的场景超过两千组可以换SPARSE_NORMAL_CHOLESKY但手眼标定的数据量一般到不了那个级别。4.3 时间偏移的搜索与收敛判断时间偏移如果是常量可以把它作为第 8 个参数块丢进 Ceres但插值逻辑要写成可微形式工程量不小。更务实的做法是外层网格搜索、内层优化外参对每个候选 dt重新对齐 IMU 轨迹、解一次外参、记录最终残差取残差曲线的谷底。dt 的搜索范围不要开太大±0.1 秒足够覆盖大多数驱动的时间戳问题步长 5 ms 粗搜找到谷底后再用 1 ms 精搜一遍。收敛判断看三个量残差下降是否单调、旋转残差是否降到 0.5° 以内、多段数据分别标定后的外参离散度。如果残差曲线在某个 dt 附近出现平台而不是尖谷说明这段数据对时间偏移不敏感多半是运动太慢得回去补快速旋转段。如果优化迭代到上限还没收敛先别加迭代次数检查有没有帧的时间戳倒序或者四元数没归一化这两个原因占了我遇到的大半。5. 避坑与排查五条血泪记录5.1 旋转接近单位阵平移却乱飞现象是标定输出的 R_X 几乎就是单位阵t_X 每次跑都不一样换一段数据能差出十几厘米。原因通常不是求解器而是旋转激励不足数据里只有平移和轻微晃动R_A − I 接近奇异平移方程的条件数爆掉任何一点噪声都会被放大成米级误差。解决方式是回看 bag确认三个轴是不是都转到了如果确实只转了偏航就补录俯仰和横滚段。补完之后先看旋转解的条件数再决定要不要信平移结果。我现在的习惯是标定前先用角速度积分估一下总旋转量三个轴都超过 30° 才开跑省得白算一轮。5.2 时间戳看着同步实际差 20 ms现象是每次标定结果都在小幅漂移旋转残差对 dt 特别敏感手动把 IMU 时间戳整体挪 20 ms 后残差明显下降。原因是录包时混用了驱动时间戳和 ROS 接收时间有的驱动在header.stamp里填的是传感器内部时钟和主机时钟没对齐。排查方法是把两个话题的时间戳打到同一张图上看静止段的对齐情况更直接的是在 4.3 的 dt 搜索里看谷底位置谷底明显偏离 0 就说明有时间偏差。解决方式是在标定前统一时间基准用header.stamp而不是rosbag的接收时间并且确认驱动没有做额外的时间补偿。5.3 运动太温柔解出来的是噪声现象是标定能跑完残差也不大但把外参应用到新数据上点云该重影还是重影。原因是动作太慢角速度幅值只有几度每秒IMU 的信噪比很低A 矩阵本身就被噪声主导优化只是在拟合噪声。解决方式是提高动作幅度角速度拉到量程的一半左右但别超过 70%饱和段必须切掉。判断数据够不够“狠”可以看 IMU 角速度的峰值和方差峰值低于 30°/s 的数据我一般直接不用重新录比调参便宜。5.4 点云坐标系不是雷达本体现象是标定结果和卷尺量出来的值差一个固定旋转而且每次差得还不一样。原因是点云话题的frame_id经过了 TF 变换或者驱动作了安装角补偿标定节点拿到的不是雷达原始坐标系下的点。排查方法是rostopic echo看header.frame_id再和 URDF 里的链路对一遍确认中间没有多余的静态变换。解决方式是在进标定之前把点云转到雷达本体坐标系或者把外参定义明确成“话题坐标系到 IMU 系”别让两套定义混着用。这个坑最隐蔽因为数值上看不出错只有拿实物一比才发现对不上。5.5 优化收敛了换一段数据就废现象是训练段残差很小留出段一验旋转差 2° 以上。原因是过拟合了一段特定的运动尤其是那段数据里某个轴几乎没激励优化器用其他轴的观测把它“补”出来了但补的方向是错的。解决方式是录数据时就分成两段一段标定、一段验证验证段要包含不同方向的旋转标定完之后在验证段上重算 AXXB 残差旋转超过 1° 就说明泛化不行。另一个辅助手段是分时段各标一次看多次结果的离散度离散度比单次残差更能说明问题。6. 验证与进阶用重投影残差和闭环一致性判断能不能用标定跑完不等于能用我一般按下面这张表过一遍任何一项不过都不交付检查项做法我用的合格线留出段旋转残差用未参与标定的数据重算 AXXB小于 0.5°留出段平移残差同上小于 2 cm点云重投影把外参作用到点云叠到 IMU 预测位姿上墙面单层、无拖尾多段一致性不同时段各标一次比较外参旋转差小于 1°时间偏移稳定性换段数据重搜 dt谷底位置漂移小于 5 ms重投影验证最直观取一段转弯数据用标定外参把点云转到 IMU 系再按 IMU 积分位姿拼起来看墙面是不是一层。如果出现规律性的双层通常是旋转还差一点如果拖尾方向随时间变化多半是时间偏移没对齐。这个检查不需要额外工具RViz 里叠两帧就能看出来比盯着残差数字有用。还有一个容易被忽略的进阶用法把标定结果反过来当诊断工具。如果某段数据怎么调都标不准先别怀疑算法用当前外参把点云投到 IMU 系看残差随时间的分布。残差在某个时间段突然变大往往对应那段数据里的配准失败或者 IMU 饱和问题在数据而不在标定。这个思路帮我省过很多次无效调参。最后说个教训。早期我做标定跑出结果看着残差小就直接写进配置文件结果整机跑起来点云还是歪的。后来强制自己每次标完都走一遍“留出段 重投影 多段一致性”三项里至少两项过才允许落地。外参这个东西没有后悔药装到设备上再发现错拆机重标的时间成本远高于多花二十分钟验证。希望帮到你。本文还有配套的精品资源点击获取