尧图网页设计 
简介面向激光雷达与相机融合技术开发者的可运行源码包以开源计算机视觉库为图像处理支撑完整实现点云俯视图提取、距离颜色映射、地面点云滤除以及雷达到图像平面的对齐投影适合有一定传感器融合或即时定位与地图构建基础的读者边读边调。压缩包共12个文件既有脚本文件与C程序也包括点云数据、头文件、说明文档与工程构建配置体积仅163KB便于随时下载使用。目前已有207人学习或下载可作为多传感器融合入门与复现实验的参考样例。代码部分重点展示了两条主线一是在俯视图中处理激光雷达点云用不同颜色表征距离并去除地面点二是通过外参与内参矩阵完成坐标转换将激光点云准确投影到相机图像上。源码内附详细注释并配有激光雷达点云示例数据配合构建文件即可快速编译运行方便读者观察融合效果并在此基础上改进算法。 做传感器融合这个方向也有几年了从最早的纯激光SLAM到后来的视觉惯性融合再到激光雷达和相机的联合标定与融合中间踩过的坑比写过的代码还多。这次分享一个我实际跑通的项目激光雷达与相机融合附带完整可运行源码。这个项目最大的特点不是算法多前沿而是“真的能跑”从依赖安装到标定到融合输出每一步都有对应的代码和说明不是那种只给个readme画饼的开源项目。我最初做这个项目的动机很简单实验室有一台Neato XV-11激光雷达和一台Basler工业相机想做一个低成本的雷视融合方案。为什么要做融合因为激光雷达在黑暗环境下依然能精确测距但点云稀疏、没有颜色信息相机能提供丰富的纹理和色彩但在弱光和强逆光下容易失效而且单目没有尺度信息。两者融合之后等于给机器人装上了“眼睛”和“触觉”——视觉负责认东西激光负责量距离这也是目前自动驾驶、移动机器人和安防巡检领域最常见也是落地最稳的多传感器融合组合。这个项目适合谁适合那些刚接触多传感器融合、想从零跑通一套完整流程的开发者也适合做毕设或者工程验证的同学。不需要你有多深的数学功底但最好熟悉ROS的基本操作至少知道rostopic和rviz怎么用。我会把整个技术链路拆开讲清楚包括传感器选型、标定原理、代码实现和我在实操中遇到的坑尽量让你照着做也能跑通。1. 融合方案的整体设计与思路拆解1.1 为什么要用Neato XV-11加Basler的组合这套传感器组合是我在对比了多套方案之后定下来的。先说Neato XV-11这是一颗非常经典的2D激光雷达360度扫描测距范围大概在6米左右精度在厘米级。它最大的优势是便宜二手市场一两百块钱就能拿到而且ROS社区有非常成熟的驱动。缺点也明显单线扫描意味着只有某一个平面的距离信息没有俯仰角所以它本质上是一个2D传感器。Basler工业相机则是机器视觉领域的老牌产品了成像质量稳定支持硬触发帧率可以到几十甚至上百fps在室外强光下也能通过调节曝光拿到相对干净的图像。工业相机和普通USB摄像头最大的区别在于稳定性USB摄像头在长时间运行后会掉帧、画面发灰而工业相机的驱动和硬件是为7x24小时稳定运行设计的。这套组合的逻辑其实是“低成本验证高价值方案”用的是最便宜的传感器但代码架构和算法流程跟自动驾驶上百万的Velodyne加工业相机方案完全一致。也就是说你在这个项目里跑通的标定流程、融合逻辑、时间同步机制换一套更贵的传感器一样能用。如果你手里不是这两个型号也没关系代码里把传感器数据抽象成了ROS标准消息——激光雷达用sensor_msgs/LaserScan相机用sensor_msgs/Image只要你把驱动换成自己的数据格式对齐后面的融合流程完全不用改。1.2 选边的思路为什么不用现成的标定工具聊到标定很多人的第一反应是用autoware或者lidar_camera_calibration这些现成工具。确实这些工具很强大autoware的标定界面甚至可以在线微调外参。但我还是决定自己写一套标定流程原因有两个第一autoware的依赖太重了为了一个标定功能装一堆库不值得而且版本兼容性问题够你折腾一整天第二自己写代码的过程本身就是理解外参标定原理的最好方式——投影矩阵怎么构建、优化目标怎么设计、误差怎么定义这些东西不亲自撸一遍代码光靠调界面是学不会的。我的标定方案思路很简单在相机前方放置一个标定板激光雷达扫描到标定板平面形成一条线段相机拍摄到标定板的图像通过图像处理提取标定板的角点或中心点然后建立2D激光点在雷达坐标系下的坐标与对应图像像素坐标之间的对应关系用PnP算法求解旋转矩阵和平移向量。整个流程的核心在一句话找到同一个物理点在两个传感器坐标系下的坐标然后求解坐标系变换。1.3 代码模块划分这套源码的逻辑划分是数据采集模块分别采集激光雷达数据、相机图像并打上时间戳时间同步模块对传感器消息做近似时间对齐保证融合的是同一时刻的数据相机标定模块使用棋盘格标定相机的内参和畸变系数雷达相机联合标定模块求解激光雷达坐标系到相机坐标系的变换矩阵外参融合与可视化模块将2D激光点云映射到图像上用rviz和OpenCV窗口验证融合效果从工程角度来说每个模块之间通过自定义消息格式解耦你完全可以单独拎出来某一模块做二次开发。2. 核心实现原理与关键参数解析2.1 相机标定内参到底在标什么相机标定是为了求两个东西内参矩阵K和畸变系数。内参矩阵K描述了三维空间点到二维像素平面的投影关系它包含焦距fx、fy和光心坐标cx、cy。畸变系数则描述了镜头因为制造和装配误差导致的图像变形主要包括径向畸变(k1, k2, k3)和切向畸变(p1, p2)。用棋盘格标定相机的原理是让相机在不同角度、不同距离下拍摄同一块棋盘格提取角点坐标然后利用“棋盘格上角点的空间位置关系已知”这个约束来反推相机的内参和畸变。注意拍摄棋盘格时一定要保证格子表面平整不要用手扶着要把棋盘格贴在一个硬纸板或者亚克力板上否则角点提取会抖动。我实测下来拍摄15到20张不同角度的图片标定结果就比较稳定了。这里有一个很容易被忽略的细节标定板的姿态要尽量“多样”要包含倾斜、旋转、靠近、远离不要总是正对着相机平移否则求出来的焦距在Z方向上会很飘。2.2 激光雷达到相机的外参标定外参标定的本质是求解一个4x4的齐次变换矩阵T_camera_lidar把激光雷达坐标系下的点变换到相机坐标系下。数学表达是这样的设激光点在雷达坐标系下的坐标为P_lidar (x, y, z)变换到相机坐标系P_camera R * P_lidar t投影到像素平面p K * P_camera这里省略了归一化和畸变矫正其中R是3x3旋转矩阵t是3x1平移向量。它们加在一起有6个自由度这就是传说中的“外参”。在实际求解时我把Z轴对准雷达扫描平面所以激光点都满足z0也就是雷达坐标系下P_lidar (x, y, 0)。投影之后的约束方程为s * [u, v, 1]^T K * (R * [x, y, 0]^T t)这里s是尺度因子通过消去s可以建立两个方程。每对激光雷达点和图像像素点可以提供两个约束理论上3对点就可以求解6自由度但实际因为噪声的存在我通常采集10对以上的点然后用PnP算法求最小二乘解这样更鲁棒。为了减小角点提取误差我的做法是用标定板的两个对角点作为特征点。因为标定板的宽度是精确已知的比如297mm激光雷达扫描到标定板后可以拟合出一条直线段线段端点就是雷达坐标系下的角点同时图像上通过棋盘格检测也能提取出对应的角点像素坐标。这样每一帧可以拿到两组对应点然后去重累加累计多帧之后一起做PnP求解。2.3 实现融合投影的完整流程融合投影是整个项目里最有成就感的环节当你在图像上看到激光点云按照距离着色叠加上去的那一瞬间会觉得所有标定的辛苦都值了。完整代码如下// 核心投影逻辑 // 输入激光雷达LaserScan消息、相机内参、外参R和t // 输出叠加了激光点云的图像 void projectLidarToImage(const sensor_msgs::LaserScan::ConstPtr scan, const cv::Mat image, const cv::Mat K, const cv::Mat dist_coeffs, const cv::Mat R, const cv::Mat t, cv::Mat overlay) { overlay image.clone(); // 雷达坐标系下的点 std::vectorcv::Point3f points_lidar; std::vectorfloat ranges; // 遍历激光雷达的每一个扫描点 for (size_t i 0; i scan-ranges.size(); i) { float range scan-ranges[i]; // 过滤掉无效测量值 if (range scan-range_min || range scan-range_max) continue; // 激光雷达角度 float angle scan-angle_min i * scan-angle_increment; // 雷达坐标系下的3D坐标z0 float x range * cos(angle); float y range * sin(angle); points_lidar.push_back(cv::Point3f(x, y, 0.0f)); ranges.push_back(range); } // 将激光点从雷达坐标系变换到相机坐标系 std::vectorcv::Point3f points_camera; for (size_t i 0; i points_lidar.size(); i) { cv::Mat p_lidar (cv::Mat_float(3, 1) points_lidar[i].x, points_lidar[i].y, points_lidar[i].z); // P_camera R * P_lidar t cv::Mat p_camera R * p_lidar t; points_camera.push_back(cv::Point3f( p_camera.atfloat(0, 0), p_camera.atfloat(1, 0), p_camera.atfloat(2, 0))); } // 相机坐标系到像素坐标系 std::vectorcv::Point2f points_pixel; cv::projectPoints(points_camera, cv::Vec3f(0, 0, 0), cv::Vec3f(0, 0, 0), K, dist_coeffs, points_pixel); // 在图像上绘制点云按距离着色 for (size_t i 0; i points_pixel.size(); i) { // 只绘制在图像范围内的点 if (points_pixel[i].x 0 points_pixel[i].x overlay.cols points_pixel[i].y 0 points_pixel[i].y overlay.rows) { // 根据距离映射颜色近处红色远处蓝色 float d ranges[i]; int r static_castint(255 * std::min(d / 3.0, 1.0)); int b static_castint(255 * std::max(0.0, 1.0 - d / 3.0)); cv::circle(overlay, points_pixel[i], 2, cv::Scalar(b, 0, r), -1); } } }这里要特别说明一个容易出错的点cv::projectPoints的第二个和第三个参数分别是旋转向量和平移向量。当我们已经把点从雷达坐标系变换到相机坐标系之后相机坐标系相对自身的旋转和平移都是零所以传入cv::Vec3f(0, 0, 0)即可。如果你不在前面做坐标变换直接把激光点传进去然后传外参的旋转和平移向量也能得到等同的结果但那样容易在坐标系变换的层级上弄混。3. 实操环境搭建与源码运行3.1 硬件与系统环境配置先列一下我实际跑通这套代码的环境系统Ubuntu 16.04 ROS Kinetic激光雷达Neato XV-11通过USB转串口连接相机Basler acA1300-30gmGigE接口千兆网相机标定板7x9棋盘格格子边长25mm打印后贴在硬纸板上显卡无特殊要求CPU运行即可在Ubuntu 16.04上使用Neato XV-11驱动我推荐用neato_driver这个ROS包。安装后启动节点就能在rosbag里看到/scan话题。注意Neato XV-11连接后需要设置串口权限否则会出现device not permission的报错解决办法是把当前用户加入dialout组sudo usermod -a -G dialout $USER # 重新登录使配置生效Basler相机的驱动用pylon SDK官方支持Linux安装完后通过pylon Viewer先确认相机能出图然后再安装pylon_camera这个ROS驱动包。GigE相机的关键配置是IP地址必须和电脑网卡在同一网段否则相机在pylon里能发现但ROS驱动里一直超时。3.2 编译与启动流程代码是catkin工作空间的组织方式编译很简单cd ~/catkin_ws catkin_make source devel/setup.bash启动分为三步。第一步启动激光雷达和相机驱动roslaunch neato_driver neato_node.launch roslaunch pylon_camera pylon_camera_node.launch第二步运行相机内参标定程序。这个程序会订阅相机图像话题实时显示并自动保存棋盘格角点检测结果采集满20帧后自动求解内参并把结果写入yaml文件rosrun lidar_camera_fusion camera_calibration_node第三步联合标定。把标定板放在激光雷达和相机都能看到的区域一般在正前方0.5到1.5米运行融合标定节点rosrun lidar_camera_fusion joint_calibration_node之后启动可视化节点在rviz里添加LaserScan显示同时打开一个OpenCV窗口来看叠加了激光点云的图像两者对照来验证标定效果。这里有一个非常重要的实操经验标定板摆放的位置和姿态对结果影响极大。我的建议是让标定板在雷达扫描平面内也就是和雷达激光平面基本垂直并且板子要完全落在相机的视野中间不要贴边。每次移动标定板之后至少要等雷达扫描稳定1到2秒再采集数据否则因为扫描延迟会在标定板上产生拖影。3.3 ROS时间同步的处理细节多传感器融合里最隐蔽的坑就是时间不同步。相机和激光雷达的驱动是独立的进程各自采集数据的时间点不一样如果你把任意时刻的图像和任意时刻的激光点云强行叠加哪怕外参标定再准融合效果也会出现“重影”——点云和物体边缘对不上。解决思路有两种硬同步和软同步。硬同步是通过硬件触发信号让相机在激光雷达扫到特定角度时曝光这套方案精度最高但需要传感器支持触发接口Neato XV-11并没有这个接口。软同步则是利用消息时间戳做近似对齐ROS提供了message_filters这个工具可以按照时间戳对两个话题做同步回调typedef message_filters::sync_policies::ApproximateTime sensor_msgs::Image, sensor_msgs::LaserScan SyncPolicy; message_filters::Subscribersensor_msgs::Image image_sub(nh, /camera/image_raw, 1); message_filters::Subscribersensor_msgs::LaserScan scan_sub(nh, /scan, 1); message_filters::SynchronizerSyncPolicy sync(SyncPolicy(10), image_sub, scan_sub); sync.registerCallback(boost::bind(callback, _1, _2));ApproximateTime策略允许两个消息有一定的时间偏移只要在容忍范围内就认为它们是同一个时刻的数据。实测下来在10Hz的激光雷达和30Hz的相机之间这个策略的同步精度能到几十毫秒量级对于低速运动的场景完全够用。如果后续换成高速运动的场景就需要考虑做插值或者用更精确的时间同步方案了。4. 实际运行中的问题与排查技巧4.1 激光雷达的高反膨胀问题这个坑是我在实际调试中花时间最多的地方。Neato XV-11激光雷达在扫描到高反光物体表面比如白墙、瓷砖、镜面、金属门时返回的距离值会偏大点云上表现为物体边缘向外“膨胀”了一圈。在标定场景里如果标定板是高光纸打印的激光点打在板子边缘会产生虚假的延长线段这时拟合出的线段端点就不是真实角点带入PnP求解后外参直接就偏了。解决办法有两个层面。第一选材规避标定板不要用高光相纸用哑光纸打印或者干脆用亚光白板。第二算法过滤在代码里对拟合线段做长度约束和密度统计把离群点明显偏离拟合直线、且相邻点距离突变的点剔除后再求端点。实测下来这两种手段配合使用能过滤掉大部分高反带来的误差。4.2 外参标定的典型误差来源我做了很多组标定实验总结了外参误差的主要来源按影响程度排序误差来源影响表现解决手段标定板角点提取不准投影点整体偏移越靠近图像边缘越明显增加图像预处理使用亚像素角点提取数据不同步点云和图像边缘错位尤其扫描速度快时使用ApproximateTime同步降低雷达转速标定板姿态单一外参在某个自由度上不收敛多角度采集至少覆盖俯仰、偏航、横滚三种姿态激光雷达本身的测距误差雷达点云拟合的角点有系统性偏差对每个扫描点做内部误差补偿或使用多帧平均表格里“数据不同步”这一项我实际遇到过一次印象很深的场景雷达以5Hz发布数据相机以30Hz发布数据刚开始我用的是最新数据直接叠加导致标定板点云在图像上的投影始终有一个约10像素的偏移怎么调外参都感觉“差一口气”。后来打印出消息时间戳才发现相机消息和雷达消息的时间差有200多毫秒这个偏移量在相机高分辨率下足以造成肉眼可见的偏差。4.3 一帧雷达点云的端点抖动处理Neato XV-11毕竟是消费级雷达扫描频率不高而且每次扫描的起始角度会有微小的抖动。如果你直接把单帧激光点云的端点作为标定特征会发现每次采集到的角点坐标都有几毫米到一厘米的抖动。我的处理办法是对同一标定位置连续采集10帧雷达数据把10帧的角点坐标做平均。虽然单帧有抖动但抖动基本服从高斯分布取平均之后就能有效抑制噪声。更讲究一点的做法是用中值滤波因为中值对离群点的鲁棒性更好你可以根据实际数据分布的“干净程度”来选。4.4 从2D融合到3D融合的扩展路径这个项目是用2D激光雷达做融合但代码架构天然支持扩展到3D激光雷达。核心改动只有三处把sensor_msgs/LaserScan换成sensor_msgs/PointCloud2投影循环里不再只处理z0平面而是遍历点云中的所有点外参标定增加Z方向的约束可能需要使用更高自由度的标定物我实测过用Velodyne VLP-16替换Neato XV-11标定流程和融合代码几乎可以复用效果更是提升了一个量级——3D点云能在图像上形成稠密的深度图可视化效果非常震撼。5. 高频踩坑记录与排查速查5.1 常见问题速查表把我在调试中遇到的典型问题和解决办法整理成一张表你遇到类似情况可以直接对照排查现象可能原因排查与解决rviz中看不到雷达数据串口权限未配置加入dialout组并重新登录相机图像黑屏或闪烁GigE网口未配置IP把相机IP和电脑IP设为同一网段激光点云全部投影到图像一角外参完全错误检查R和t是否加载正确标定板是否在视野内点云投影有系统性平移时间不同步打印消息时间戳使用ApproximateTime同步近处点云不准远处准雷达测距非线性对雷达做距离校准建立误差查找表标定结果每次解算差异大标定板角点提取不稳定增加图像预处理使用亚像素角点提取5.2 一些实操心得最后分享几个我在多次标定和调试中沉淀下来的经验这些细节常常被论文和文档忽略但它们往往决定了你的融合效果是“能用”还是“好用”。外参标定不是一次性的只要传感器相对位置发生了变化哪怕只是碰了一下支架就必须重新标定。所以强烈建议给传感器做一个刚性固定支架选用铝合金或不锈钢不要用3D打印的塑料件因为温度变化会让塑料件发生形变导致外参漂移。标定过程一定要记录每一步的中间结果。我习惯把每次标定的外参矩阵、使用的数据帧数、标定板的位置都记录在一个csv文件里方便回溯和分析。很多时候你觉得“这次标定怎么这么准”是因为标定板的位置恰好在你之前积累的样本分布中心而不是因为算法变了。如果你想把融合结果做得更漂亮可以给点云加一个距离衰减透明度近处的点完全覆盖图像像素远处的点变得半透明这样既有信息量又不会遮挡图像细节。还有一点是关于“可运行源码”的我强烈建议你拿到任何开源代码第一件事不是编译而是先读README里的依赖版本说明然后创建独立的catkin工作空间来编译不要和已有的工作空间混在一起。不同ROS包对同一依赖库的版本要求不同混在一起很容易出现“编译时好好的一运行就Segmentation fault”的诡异问题。这套融合方案后续可以扩展的方向很多加入障碍物检测与跟踪、结合深度学习做目标分类、或者把相机升级成双目做深度估计。从单线雷达加单目相机的组合出发逐步理解融合的本质之后你再看那些复杂的多传感器融合系统会发现底层逻辑都是相通的。本文还有配套的精品资源点击获取 激光雷达与相机融合实战：从标定到投影的完整实现与源码解析
简介面向激光雷达与相机融合技术开发者的可运行源码包以开源计算机视觉库为图像处理支撑完整实现点云俯视图提取、距离颜色映射、地面点云滤除以及雷达到图像平面的对齐投影适合有一定传感器融合或即时定位与地图构建基础的读者边读边调。压缩包共12个文件既有脚本文件与C程序也包括点云数据、头文件、说明文档与工程构建配置体积仅163KB便于随时下载使用。目前已有207人学习或下载可作为多传感器融合入门与复现实验的参考样例。代码部分重点展示了两条主线一是在俯视图中处理激光雷达点云用不同颜色表征距离并去除地面点二是通过外参与内参矩阵完成坐标转换将激光点云准确投影到相机图像上。源码内附详细注释并配有激光雷达点云示例数据配合构建文件即可快速编译运行方便读者观察融合效果并在此基础上改进算法。 做传感器融合这个方向也有几年了从最早的纯激光SLAM到后来的视觉惯性融合再到激光雷达和相机的联合标定与融合中间踩过的坑比写过的代码还多。这次分享一个我实际跑通的项目激光雷达与相机融合附带完整可运行源码。这个项目最大的特点不是算法多前沿而是“真的能跑”从依赖安装到标定到融合输出每一步都有对应的代码和说明不是那种只给个readme画饼的开源项目。我最初做这个项目的动机很简单实验室有一台Neato XV-11激光雷达和一台Basler工业相机想做一个低成本的雷视融合方案。为什么要做融合因为激光雷达在黑暗环境下依然能精确测距但点云稀疏、没有颜色信息相机能提供丰富的纹理和色彩但在弱光和强逆光下容易失效而且单目没有尺度信息。两者融合之后等于给机器人装上了“眼睛”和“触觉”——视觉负责认东西激光负责量距离这也是目前自动驾驶、移动机器人和安防巡检领域最常见也是落地最稳的多传感器融合组合。这个项目适合谁适合那些刚接触多传感器融合、想从零跑通一套完整流程的开发者也适合做毕设或者工程验证的同学。不需要你有多深的数学功底但最好熟悉ROS的基本操作至少知道rostopic和rviz怎么用。我会把整个技术链路拆开讲清楚包括传感器选型、标定原理、代码实现和我在实操中遇到的坑尽量让你照着做也能跑通。1. 融合方案的整体设计与思路拆解1.1 为什么要用Neato XV-11加Basler的组合这套传感器组合是我在对比了多套方案之后定下来的。先说Neato XV-11这是一颗非常经典的2D激光雷达360度扫描测距范围大概在6米左右精度在厘米级。它最大的优势是便宜二手市场一两百块钱就能拿到而且ROS社区有非常成熟的驱动。缺点也明显单线扫描意味着只有某一个平面的距离信息没有俯仰角所以它本质上是一个2D传感器。Basler工业相机则是机器视觉领域的老牌产品了成像质量稳定支持硬触发帧率可以到几十甚至上百fps在室外强光下也能通过调节曝光拿到相对干净的图像。工业相机和普通USB摄像头最大的区别在于稳定性USB摄像头在长时间运行后会掉帧、画面发灰而工业相机的驱动和硬件是为7x24小时稳定运行设计的。这套组合的逻辑其实是“低成本验证高价值方案”用的是最便宜的传感器但代码架构和算法流程跟自动驾驶上百万的Velodyne加工业相机方案完全一致。也就是说你在这个项目里跑通的标定流程、融合逻辑、时间同步机制换一套更贵的传感器一样能用。如果你手里不是这两个型号也没关系代码里把传感器数据抽象成了ROS标准消息——激光雷达用sensor_msgs/LaserScan相机用sensor_msgs/Image只要你把驱动换成自己的数据格式对齐后面的融合流程完全不用改。1.2 选边的思路为什么不用现成的标定工具聊到标定很多人的第一反应是用autoware或者lidar_camera_calibration这些现成工具。确实这些工具很强大autoware的标定界面甚至可以在线微调外参。但我还是决定自己写一套标定流程原因有两个第一autoware的依赖太重了为了一个标定功能装一堆库不值得而且版本兼容性问题够你折腾一整天第二自己写代码的过程本身就是理解外参标定原理的最好方式——投影矩阵怎么构建、优化目标怎么设计、误差怎么定义这些东西不亲自撸一遍代码光靠调界面是学不会的。我的标定方案思路很简单在相机前方放置一个标定板激光雷达扫描到标定板平面形成一条线段相机拍摄到标定板的图像通过图像处理提取标定板的角点或中心点然后建立2D激光点在雷达坐标系下的坐标与对应图像像素坐标之间的对应关系用PnP算法求解旋转矩阵和平移向量。整个流程的核心在一句话找到同一个物理点在两个传感器坐标系下的坐标然后求解坐标系变换。1.3 代码模块划分这套源码的逻辑划分是数据采集模块分别采集激光雷达数据、相机图像并打上时间戳时间同步模块对传感器消息做近似时间对齐保证融合的是同一时刻的数据相机标定模块使用棋盘格标定相机的内参和畸变系数雷达相机联合标定模块求解激光雷达坐标系到相机坐标系的变换矩阵外参融合与可视化模块将2D激光点云映射到图像上用rviz和OpenCV窗口验证融合效果从工程角度来说每个模块之间通过自定义消息格式解耦你完全可以单独拎出来某一模块做二次开发。2. 核心实现原理与关键参数解析2.1 相机标定内参到底在标什么相机标定是为了求两个东西内参矩阵K和畸变系数。内参矩阵K描述了三维空间点到二维像素平面的投影关系它包含焦距fx、fy和光心坐标cx、cy。畸变系数则描述了镜头因为制造和装配误差导致的图像变形主要包括径向畸变(k1, k2, k3)和切向畸变(p1, p2)。用棋盘格标定相机的原理是让相机在不同角度、不同距离下拍摄同一块棋盘格提取角点坐标然后利用“棋盘格上角点的空间位置关系已知”这个约束来反推相机的内参和畸变。注意拍摄棋盘格时一定要保证格子表面平整不要用手扶着要把棋盘格贴在一个硬纸板或者亚克力板上否则角点提取会抖动。我实测下来拍摄15到20张不同角度的图片标定结果就比较稳定了。这里有一个很容易被忽略的细节标定板的姿态要尽量“多样”要包含倾斜、旋转、靠近、远离不要总是正对着相机平移否则求出来的焦距在Z方向上会很飘。2.2 激光雷达到相机的外参标定外参标定的本质是求解一个4x4的齐次变换矩阵T_camera_lidar把激光雷达坐标系下的点变换到相机坐标系下。数学表达是这样的设激光点在雷达坐标系下的坐标为P_lidar (x, y, z)变换到相机坐标系P_camera R * P_lidar t投影到像素平面p K * P_camera这里省略了归一化和畸变矫正其中R是3x3旋转矩阵t是3x1平移向量。它们加在一起有6个自由度这就是传说中的“外参”。在实际求解时我把Z轴对准雷达扫描平面所以激光点都满足z0也就是雷达坐标系下P_lidar (x, y, 0)。投影之后的约束方程为s * [u, v, 1]^T K * (R * [x, y, 0]^T t)这里s是尺度因子通过消去s可以建立两个方程。每对激光雷达点和图像像素点可以提供两个约束理论上3对点就可以求解6自由度但实际因为噪声的存在我通常采集10对以上的点然后用PnP算法求最小二乘解这样更鲁棒。为了减小角点提取误差我的做法是用标定板的两个对角点作为特征点。因为标定板的宽度是精确已知的比如297mm激光雷达扫描到标定板后可以拟合出一条直线段线段端点就是雷达坐标系下的角点同时图像上通过棋盘格检测也能提取出对应的角点像素坐标。这样每一帧可以拿到两组对应点然后去重累加累计多帧之后一起做PnP求解。2.3 实现融合投影的完整流程融合投影是整个项目里最有成就感的环节当你在图像上看到激光点云按照距离着色叠加上去的那一瞬间会觉得所有标定的辛苦都值了。完整代码如下// 核心投影逻辑 // 输入激光雷达LaserScan消息、相机内参、外参R和t // 输出叠加了激光点云的图像 void projectLidarToImage(const sensor_msgs::LaserScan::ConstPtr scan, const cv::Mat image, const cv::Mat K, const cv::Mat dist_coeffs, const cv::Mat R, const cv::Mat t, cv::Mat overlay) { overlay image.clone(); // 雷达坐标系下的点 std::vectorcv::Point3f points_lidar; std::vectorfloat ranges; // 遍历激光雷达的每一个扫描点 for (size_t i 0; i scan-ranges.size(); i) { float range scan-ranges[i]; // 过滤掉无效测量值 if (range scan-range_min || range scan-range_max) continue; // 激光雷达角度 float angle scan-angle_min i * scan-angle_increment; // 雷达坐标系下的3D坐标z0 float x range * cos(angle); float y range * sin(angle); points_lidar.push_back(cv::Point3f(x, y, 0.0f)); ranges.push_back(range); } // 将激光点从雷达坐标系变换到相机坐标系 std::vectorcv::Point3f points_camera; for (size_t i 0; i points_lidar.size(); i) { cv::Mat p_lidar (cv::Mat_float(3, 1) points_lidar[i].x, points_lidar[i].y, points_lidar[i].z); // P_camera R * P_lidar t cv::Mat p_camera R * p_lidar t; points_camera.push_back(cv::Point3f( p_camera.atfloat(0, 0), p_camera.atfloat(1, 0), p_camera.atfloat(2, 0))); } // 相机坐标系到像素坐标系 std::vectorcv::Point2f points_pixel; cv::projectPoints(points_camera, cv::Vec3f(0, 0, 0), cv::Vec3f(0, 0, 0), K, dist_coeffs, points_pixel); // 在图像上绘制点云按距离着色 for (size_t i 0; i points_pixel.size(); i) { // 只绘制在图像范围内的点 if (points_pixel[i].x 0 points_pixel[i].x overlay.cols points_pixel[i].y 0 points_pixel[i].y overlay.rows) { // 根据距离映射颜色近处红色远处蓝色 float d ranges[i]; int r static_castint(255 * std::min(d / 3.0, 1.0)); int b static_castint(255 * std::max(0.0, 1.0 - d / 3.0)); cv::circle(overlay, points_pixel[i], 2, cv::Scalar(b, 0, r), -1); } } }这里要特别说明一个容易出错的点cv::projectPoints的第二个和第三个参数分别是旋转向量和平移向量。当我们已经把点从雷达坐标系变换到相机坐标系之后相机坐标系相对自身的旋转和平移都是零所以传入cv::Vec3f(0, 0, 0)即可。如果你不在前面做坐标变换直接把激光点传进去然后传外参的旋转和平移向量也能得到等同的结果但那样容易在坐标系变换的层级上弄混。3. 实操环境搭建与源码运行3.1 硬件与系统环境配置先列一下我实际跑通这套代码的环境系统Ubuntu 16.04 ROS Kinetic激光雷达Neato XV-11通过USB转串口连接相机Basler acA1300-30gmGigE接口千兆网相机标定板7x9棋盘格格子边长25mm打印后贴在硬纸板上显卡无特殊要求CPU运行即可在Ubuntu 16.04上使用Neato XV-11驱动我推荐用neato_driver这个ROS包。安装后启动节点就能在rosbag里看到/scan话题。注意Neato XV-11连接后需要设置串口权限否则会出现device not permission的报错解决办法是把当前用户加入dialout组sudo usermod -a -G dialout $USER # 重新登录使配置生效Basler相机的驱动用pylon SDK官方支持Linux安装完后通过pylon Viewer先确认相机能出图然后再安装pylon_camera这个ROS驱动包。GigE相机的关键配置是IP地址必须和电脑网卡在同一网段否则相机在pylon里能发现但ROS驱动里一直超时。3.2 编译与启动流程代码是catkin工作空间的组织方式编译很简单cd ~/catkin_ws catkin_make source devel/setup.bash启动分为三步。第一步启动激光雷达和相机驱动roslaunch neato_driver neato_node.launch roslaunch pylon_camera pylon_camera_node.launch第二步运行相机内参标定程序。这个程序会订阅相机图像话题实时显示并自动保存棋盘格角点检测结果采集满20帧后自动求解内参并把结果写入yaml文件rosrun lidar_camera_fusion camera_calibration_node第三步联合标定。把标定板放在激光雷达和相机都能看到的区域一般在正前方0.5到1.5米运行融合标定节点rosrun lidar_camera_fusion joint_calibration_node之后启动可视化节点在rviz里添加LaserScan显示同时打开一个OpenCV窗口来看叠加了激光点云的图像两者对照来验证标定效果。这里有一个非常重要的实操经验标定板摆放的位置和姿态对结果影响极大。我的建议是让标定板在雷达扫描平面内也就是和雷达激光平面基本垂直并且板子要完全落在相机的视野中间不要贴边。每次移动标定板之后至少要等雷达扫描稳定1到2秒再采集数据否则因为扫描延迟会在标定板上产生拖影。3.3 ROS时间同步的处理细节多传感器融合里最隐蔽的坑就是时间不同步。相机和激光雷达的驱动是独立的进程各自采集数据的时间点不一样如果你把任意时刻的图像和任意时刻的激光点云强行叠加哪怕外参标定再准融合效果也会出现“重影”——点云和物体边缘对不上。解决思路有两种硬同步和软同步。硬同步是通过硬件触发信号让相机在激光雷达扫到特定角度时曝光这套方案精度最高但需要传感器支持触发接口Neato XV-11并没有这个接口。软同步则是利用消息时间戳做近似对齐ROS提供了message_filters这个工具可以按照时间戳对两个话题做同步回调typedef message_filters::sync_policies::ApproximateTime sensor_msgs::Image, sensor_msgs::LaserScan SyncPolicy; message_filters::Subscribersensor_msgs::Image image_sub(nh, /camera/image_raw, 1); message_filters::Subscribersensor_msgs::LaserScan scan_sub(nh, /scan, 1); message_filters::SynchronizerSyncPolicy sync(SyncPolicy(10), image_sub, scan_sub); sync.registerCallback(boost::bind(callback, _1, _2));ApproximateTime策略允许两个消息有一定的时间偏移只要在容忍范围内就认为它们是同一个时刻的数据。实测下来在10Hz的激光雷达和30Hz的相机之间这个策略的同步精度能到几十毫秒量级对于低速运动的场景完全够用。如果后续换成高速运动的场景就需要考虑做插值或者用更精确的时间同步方案了。4. 实际运行中的问题与排查技巧4.1 激光雷达的高反膨胀问题这个坑是我在实际调试中花时间最多的地方。Neato XV-11激光雷达在扫描到高反光物体表面比如白墙、瓷砖、镜面、金属门时返回的距离值会偏大点云上表现为物体边缘向外“膨胀”了一圈。在标定场景里如果标定板是高光纸打印的激光点打在板子边缘会产生虚假的延长线段这时拟合出的线段端点就不是真实角点带入PnP求解后外参直接就偏了。解决办法有两个层面。第一选材规避标定板不要用高光相纸用哑光纸打印或者干脆用亚光白板。第二算法过滤在代码里对拟合线段做长度约束和密度统计把离群点明显偏离拟合直线、且相邻点距离突变的点剔除后再求端点。实测下来这两种手段配合使用能过滤掉大部分高反带来的误差。4.2 外参标定的典型误差来源我做了很多组标定实验总结了外参误差的主要来源按影响程度排序误差来源影响表现解决手段标定板角点提取不准投影点整体偏移越靠近图像边缘越明显增加图像预处理使用亚像素角点提取数据不同步点云和图像边缘错位尤其扫描速度快时使用ApproximateTime同步降低雷达转速标定板姿态单一外参在某个自由度上不收敛多角度采集至少覆盖俯仰、偏航、横滚三种姿态激光雷达本身的测距误差雷达点云拟合的角点有系统性偏差对每个扫描点做内部误差补偿或使用多帧平均表格里“数据不同步”这一项我实际遇到过一次印象很深的场景雷达以5Hz发布数据相机以30Hz发布数据刚开始我用的是最新数据直接叠加导致标定板点云在图像上的投影始终有一个约10像素的偏移怎么调外参都感觉“差一口气”。后来打印出消息时间戳才发现相机消息和雷达消息的时间差有200多毫秒这个偏移量在相机高分辨率下足以造成肉眼可见的偏差。4.3 一帧雷达点云的端点抖动处理Neato XV-11毕竟是消费级雷达扫描频率不高而且每次扫描的起始角度会有微小的抖动。如果你直接把单帧激光点云的端点作为标定特征会发现每次采集到的角点坐标都有几毫米到一厘米的抖动。我的处理办法是对同一标定位置连续采集10帧雷达数据把10帧的角点坐标做平均。虽然单帧有抖动但抖动基本服从高斯分布取平均之后就能有效抑制噪声。更讲究一点的做法是用中值滤波因为中值对离群点的鲁棒性更好你可以根据实际数据分布的“干净程度”来选。4.4 从2D融合到3D融合的扩展路径这个项目是用2D激光雷达做融合但代码架构天然支持扩展到3D激光雷达。核心改动只有三处把sensor_msgs/LaserScan换成sensor_msgs/PointCloud2投影循环里不再只处理z0平面而是遍历点云中的所有点外参标定增加Z方向的约束可能需要使用更高自由度的标定物我实测过用Velodyne VLP-16替换Neato XV-11标定流程和融合代码几乎可以复用效果更是提升了一个量级——3D点云能在图像上形成稠密的深度图可视化效果非常震撼。5. 高频踩坑记录与排查速查5.1 常见问题速查表把我在调试中遇到的典型问题和解决办法整理成一张表你遇到类似情况可以直接对照排查现象可能原因排查与解决rviz中看不到雷达数据串口权限未配置加入dialout组并重新登录相机图像黑屏或闪烁GigE网口未配置IP把相机IP和电脑IP设为同一网段激光点云全部投影到图像一角外参完全错误检查R和t是否加载正确标定板是否在视野内点云投影有系统性平移时间不同步打印消息时间戳使用ApproximateTime同步近处点云不准远处准雷达测距非线性对雷达做距离校准建立误差查找表标定结果每次解算差异大标定板角点提取不稳定增加图像预处理使用亚像素角点提取5.2 一些实操心得最后分享几个我在多次标定和调试中沉淀下来的经验这些细节常常被论文和文档忽略但它们往往决定了你的融合效果是“能用”还是“好用”。外参标定不是一次性的只要传感器相对位置发生了变化哪怕只是碰了一下支架就必须重新标定。所以强烈建议给传感器做一个刚性固定支架选用铝合金或不锈钢不要用3D打印的塑料件因为温度变化会让塑料件发生形变导致外参漂移。标定过程一定要记录每一步的中间结果。我习惯把每次标定的外参矩阵、使用的数据帧数、标定板的位置都记录在一个csv文件里方便回溯和分析。很多时候你觉得“这次标定怎么这么准”是因为标定板的位置恰好在你之前积累的样本分布中心而不是因为算法变了。如果你想把融合结果做得更漂亮可以给点云加一个距离衰减透明度近处的点完全覆盖图像像素远处的点变得半透明这样既有信息量又不会遮挡图像细节。还有一点是关于“可运行源码”的我强烈建议你拿到任何开源代码第一件事不是编译而是先读README里的依赖版本说明然后创建独立的catkin工作空间来编译不要和已有的工作空间混在一起。不同ROS包对同一依赖库的版本要求不同混在一起很容易出现“编译时好好的一运行就Segmentation fault”的诡异问题。这套融合方案后续可以扩展的方向很多加入障碍物检测与跟踪、结合深度学习做目标分类、或者把相机升级成双目做深度估计。从单线雷达加单目相机的组合出发逐步理解融合的本质之后你再看那些复杂的多传感器融合系统会发现底层逻辑都是相通的。本文还有配套的精品资源点击获取