FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry做SLAM和机器人感知的同行应该一眼就明白这个标题的分量既快又直接还是LiDAR-Inertial-Visual三传感器融合的里程计。FAST-LIVO2是香港大学MARS Lab在FAST-LIVO基础上进一步迭代出来的开源方案。它没有走“先提特征、再匹配”的老路而是直接用原始激光点云和图像像素的观测值做配准配合IMU进行紧耦合的状态估计。这意味着它在弱纹理、快速旋转、光照变化大的场景下依然能稳定输出高频率位姿特别适合无人机、手持扫描仪、机器人底盘这类对实时性要求很高的设备。如果你准备复现或者把这个方案迁移到自己的传感器上我的建议是先别急着git clone然后catkin build。直接法虽然原理上优雅但对环境配置和外参标定非常敏感。很多人在编译阶段卡住要么是Visual C运行库缺失要么是Livox驱动版本对不上也有很多人编译通过后跑官方数据集没问题一换到自己数据就发散问题多半出在LiDAR-IMU外参标定上。这篇文章我会从算法原理拆起接着给出完整的复现流程、环境配置手记和排坑清单希望能让你的FAST-LIVO2少走弯路。1. FAST-LIVO2核心思路与设计拆解1.1 为什么需要“直接法”的多传感器里程计先得理解传统方案的问题。纯LiDAR里程计在几何结构退化的地方特别容易翻车比如一条笔直的长走廊激光雷达在前进方向上的约束很弱位置估计会慢慢飘。纯视觉里程计又怕光照剧烈变化和运动模糊昏暗环境几乎不可用。所以LiDAR-Inertial-Visual融合成了主流思路但怎么融是个大学问。早期很多方案是特征级融合从图像里提ORB、SuperPoint这类点特征从点云里提平面、边缘特征然后找特征对应关系再丢进优化器。问题在于特征提取本身有延迟和精度损失在纹理稀疏或者几何重复的场景里特征点的数量和质量都不稳定。FAST-LIVO2的思路是直接拿原始像素灰度和原始点云几何作为测量值去和全局地图做配准。所谓“direct”指的就是这个。省掉特征提取这一步一方面减少了计算开销另一方面保留了尽可能多的信息尤其是图像在低纹理区域的微弱亮度梯度也能参与约束。1.2 系统架构从FAST-LIVO到FAST-LIVO2FAST-LIVO是第一代已经把“LiDAR-惯性-视觉里程计”跑通了但它在体素地图更新和视觉信息利用上还有优化空间。FAST-LIVO2主要做了三件事一是把体素地图的更新改成增量式只更新当前视野附近的部分节省大量计算二是采用了自适应分辨率的体素划分让地图既保留细节又不至于膨胀得太大三是对视觉残差的权重和鲁棒核做了更细的调整让光照变化和动态物体的干扰被抑制得更彻底。从模块上看整套系统还是经典的紧耦合框架IMU负责高频运动预测LiDAR点云和图像帧在预测的位姿初值下分别与局部地图做配准求出残差后输入到误差状态迭代卡尔曼滤波器ESIKF中输出最终的6DoF位姿。整个流程不维护传统意义上的关键帧和回环图状态更新是增量式的这也是它能跑到几十赫兹到上百赫兹的原因。1.3 自适应体素地图与增量更新FAST-LIVO2的体素地图设计是关键。在点云配准中每个点都要在已有地图里找对应平面或对应体素如果地图太大最近邻搜索会非常耗时。FAST-LIVO2把地图用体素建索引每个体素内保存拟合出来的平面参数和光度信息。分辨率高的地方体素小几何平坦或纹理单一的地方体素大这是“自适应”的核心。增量更新的意思是新来的点只影响它所在的那几个体素系统没必要重新构建全局地图。实测下来在Livox Avia这样的小视场雷达上CPU占用比第一代明显下降但精度没有损失。这个设计对性能的提升非常直观你不需要一台顶配电脑也能跑实时。如果你是做嵌入式或机载部署的这两点直接决定了方案能不能落地。1.4 适用场景与落地价值FAST-LIVO2适合哪些场景第一是高速运动和剧烈旋转。无人机快速转向时图像容易模糊纯视觉会失效但LiDAR和IMU还能顶住。第二是弱纹理、光照变化大的室内外环境。它用的光度误差让昏暗走廊里微弱的纹理也能参与估计比特征法强很多。第三是要求低延迟和高输出频率的实时系统比如机器人避障和控制闭环。你可以把它当作一个稳定输出的里程计前端后端再接回环检测和全局优化。反过来如果场景是长时间大范围建图、需要闭环修正的SLAMFAST-LIVO2本身不带回环需要额外接模块。这一点在选型时要心里有数。2. 核心原理与关键实现细节2.1 激光点云的投影残差模型FAST-LIVO2在激光部分使用点到平面的残差。具体地每个新扫描点通过当前预测位姿变换到地图坐标系然后在地图的体素中找到最近的局部平面计算该点到平面的距离作为残差。这个残差反映了位姿估计的错误程度位姿越准点越贴合地图中的平面距离越小。平面拟合在每个体素里维护点云的最新统计信息使用增量式协方差估计不需要每个点都重新拟合。这意味着当体素收到新点时只需要更新几个矩阵就能快速得到平面法向量和重心。这段逻辑在手写实现时容易忽略的是体素内的点不能太多也不能太少太多会导致旧点拖慢响应太少会导致平面拟合不稳定。FAST-LIVO2用一定的降采样策略控制每个体素内的点数量工程实现上比理论公式更磨人。2.2 图像的光度误差模型视觉部分FAST-LIVO2不使用特征点而是用像素的光度值。对于图像帧中一个关键像素通过预测位姿把它投影到全局地图中地图中对应的体素存储了参考灰度值两者之间的亮度差就是光度残差。优化时让这个残差最小化就相当于通过灰度匹配把当前帧和地图对齐。这个思路很像直接法视觉里程计但不一样的是地图是LiDAR和视觉信息共同维护的深度由LiDAR提供光度由相机提供。所以即使某张图像曝光不一致只要大多数像素的灰度关系稳定系统依然能收敛。不过必须提醒光度误差要求相机响应基本一致如果相机有自动曝光且变化剧烈残差会出现虚假波动建议把相机设为固定曝光或者使用HDR模式。2.3 紧耦合的误差状态迭代卡尔曼滤波多传感器融合的核心是把IMU、LiDAR、视觉三类信息放在同一个状态向量里。FAST-LIVO2使用误差状态迭代卡尔曼滤波ESIKF状态量包括位置、速度、姿态、陀螺零偏、加速度计零偏以及可能需要的外参修正项。IMU用于时间更新预测下一时刻状态和协方差LiDAR和视觉残差作为量测通过迭代更新修正预测状态。很多人会把ESIKF和传统的扩展卡尔曼滤波搞混。简单说ESIKF在每次量测更新内部再做多轮迭代让线性化点尽量接近真实值精度上比一次线性化的EKF好很多计算量又比全因子图优化小。FAST-LIVO2能在低功耗硬件上跑到几十赫兹正是靠这个设计。2.4 为什么直接法能赢直接法最大的优势是信息利用率高。一个100万像素的图像即使只有几千个像素有有效梯度直接法也能让它们全部参与约束而特征法可能只挑出几百个特征点另外的信息全扔掉。类似地点云中的每个点都可以直接配准不需要先判断它是不是角点、边缘点。第二个优势是避免了匹配错误。特征匹配经常有外点需要RANSAC等剔除直接法用光度误差和几何距离作为连续值度量配合鲁棒核函数天然能削弱动态物体和传感器噪声的影响。当然直接法也有代价对位姿初值敏感如果IMU预测的初值太差残差便会掉进局部极小值对传感器标定要求高内参、外参、时间同步有一处不准系统的冗余观测会互相打架。这也是后面讲复现时为什么把标定和编译环境单独拿出来说的原因。3. 复现前的环境准备从Linux到Windows的依赖清单3.1 Ubuntu ROS环境绝大多数SLAM项目官方支持环境是Ubuntu 18.04/20.04 ROS Melodic/Noetic。FAST-LIVO2推荐用Ubuntu 20.04 ROS Noetic因为对Livox ROS驱动和PCL的支持最稳定。如果你是ARM平台比如Jetson也可以用JetPack对应的Ubuntu版本但编译时要留意Eigen版本和CUDA的搭配。建立工作空间时建议把源码放在独立的catkin工作空间里避免和系统里的其他ROS包冲突。我习惯这样建mkdir -p ~/fastlivo2_ws/src cd ~/fastlivo2_ws catkin_init_workspace然后把FAST-LIVO2源码clone到src目录。依赖库包括Livox SDK、livox_ros_driver2、PCL、Eigen3、Ceres、OpenCV、Sophus等。Ceres版本太老会出现编译错误建议用1.14.0以上。OpenCV如果系统里有4.5没问题只要ROS的vision_opencv版本对得上。3.2 Windows下的编译环境Visual Studio相关还有些人非要在Windows下复现比如没有Linux机器或者想用Windows上现成的传感器SDK。这条路不是官方主推但确实能跑通。核心坑集中在Visual Studio版本和运行库上。FAST-LIVO2的代码是C14/17风格在Windows下建议使用Visual Studio 2022社区版免费或Visual Studio 2019安装时务必勾选“使用C的桌面开发”工作负载并且装好Windows 10/11 SDK。只装了VS Code是不行的编译需要MSVC编译器和Windows SDK。装完以后还要确认系统里安装了最新的Microsoft Visual C Redistributable特别是2015-2022合集版本。很多跑官方数据集时exe启动就报“VCRUNTIME140.dll缺失”或“0xc000007b错误”十有八九是Redistributable没装或者版本不一致。在Windows下还有几个附带问题Livox SDK在Windows上需要从Livox官网单独下载对应的Windows版本PCL和Eigen用vcpkg或者预编译包安装Ceres在Windows上编译比较麻烦建议直接使用vcpkg安装并设定与VS相同的架构x64。还有一点如果CMake配置时找不到OpenCV多半是因为没有设置OpenCV_DIR环境变量。整个环境搭下来比Linux麻烦不少所以我一般建议新手先用UbuntuWindows只作为你确实需要把系统跑在Windows上的备选项。3.3 LiDAR-IMU外参标定准备FAST-LIVO2对LiDAR-IMU外参的精度非常敏感。每个传感器在安装时物理位置不可能和设计图完全一致必须通过标定获得LiDAR坐标系与IMU坐标系之间的旋转和平移。港大MARS Lab开源了配套的标定工具一般是先录制一段包含丰富几何结构的rosbag然后离线运行标定程序输出外参文件。录标定数据时要注意尽量让设备做多个姿态的变化包括俯仰、翻滚、偏航场景中要有清晰的平面和角点不要在大面积无纹理的白墙或空旷场地录制。录制时间建议2-5分钟IMU和LiDAR一定要固定牢固不能有松动。标定完成后把外参填到FAST-LIVO2的配置文件中。很多人忽略的一个细节是标定得到的时间偏移也必须填进去如果时间同步不准后面的融合会剧烈震荡。3.4 相机内参与时间同步除了LiDAR-IMU外参相机内参也需要标定。FAST-LIVO2使用图像像素的光度信息因此需要相机内参焦距、主点、畸变系数来把3D点投影到像素平面。推荐用Kalibr或OpenCV棋盘格标定标定时要覆盖不同角度和距离。时间同步上相机的触发时间戳与LiDAR和IMU的时基要统一最好使用硬件同步或者至少用软件同步校准偏移。这些准备工作看着琐碎但对最终效果是决定性的。我见过太多人把时间跳过结果跑数据集时位姿持续漂移还以为是算法问题。4. 编译运行与常见错误排查4.1 从源码到可执行文件的完整流程在Ubuntu下最标准的做法是使用ROS的catkin工具cd ~/fastlivo2_ws/src git clone https://github.com/hku-mars/FAST-LIVO2.git cd .. rosdep install --from-paths src --ignore-src -r -y catkin build如果前面依赖都装好了这一步会一次性通过。但有几处容易卡Livox驱动版本FAST-LIVO2默认使用livox_ros_driver2需要先编译安装livox_ros_driver2再编译FAST-LIVO2。Sophus模板老代码需要fmt和Sophus如果编译报找不到Sophus需要安装libSophus或者将源码放到thirdparty。PCL版本冲突建议使用系统自带的PCL1.10Ubuntu20.04默认不要自己编译PCL除非你有特殊需求。编译完成后运行节点source devel/setup.bash roslaunch fast_livo2 fast_livo2_mid.launch这里“mid”指Livox Mid系列雷达如果你用Avia需要改成对应的launch文件。启动后再播放官方数据集rosbag play your_bag.bag如果一切正常RVIZ里会显示彩色的点云地图和相机图像同时终端输出位姿和频率信息。4.2 官方数据集与自采数据验证官方提供了多个场景的rosbag包含室内、室外、长廊、旋转等案例。第一次跑通建议先用官方bag因为它对应的配置文件和传感器参数已经调好能验证你的编译环境没问题。之后再用自采数据。自采数据时需要把配置文件的传感器参数改成自己的LiDAR系到IMU系的外参、相机到IMU的变换、相机内参、IMU噪声参数。这些参数如果缺一个系统可能直接发散。改完参数后可以先录制一段2分钟的静态数据观察输出的速度和位置是不是基本不变再录一段缓慢移动的数据看地图是否清晰。4.3 编译与运行错误速查表这里我把常见问题列出来方便你对照排查。错误现象常见原因解决方法找不到livox_ros_driver2未提前编译Livox驱动在工作空间下先单独编译livox_ros_driver2fatal error: sophus/se3.hppSophus未安装或版本不对安装模板版Sophus或使用仓库thirdpartyCeres版本不兼容Ceres 1.12以下API缺失用源码头安装Ceres 1.14运行时报段错误点云回调或地图初始化未等待检查传感器话题名称是否匹配以及是否等待rviz初始化初始化时间过长LiDAR和IMU时间戳不同步检查时间偏移重新标定同步地图闪烁或位姿漂移外参不准或相机曝光变化大重新标定外参固定相机曝光这些坑都不是FAST-LIVO2独有的而是所有多传感器融合SLAM共通的。遇到错误先别急着改代码检查依赖、话题、参数三件套80%的问题都能解决。4.4 性能评估与参数调优调参时重点关注三个地方。第一是体素分辨率太大细节丢失太小计算量爆炸通常设置在0.2-0.5米之间。第二是视觉残差权重如果相机图像质量好可以调大光度残差的权重如果曝光不稳定减小权重甚至暂时关闭视觉。第三是IMU噪声参数可以从小型IMU的数据手册里查或者用Allan方差分析得到。评估里程计性能可以用evo工具对比轨迹与真值。如果只有估计轨迹没有真值可以观察地图重投影误差和位姿是否连续平滑。FAST-LIVO2的正常表现应该是在快速旋转后角度误差不明显在弱纹理走廊中不严重漂移。如果你跑下来发现角度发散很快建议先检查外参旋转部分如果位置漂移多半和外参平移以及加速度计零偏有关。5. 避坑指南与真实使用心得5.1 Visual Studio与运行库的复盘前面提到Windows编译我再补充一个亲身踩过的坑。有一次我在Windows上编译FAST-LIVO2用的是VS2019编译一切正常但运行时程序启动就退出没有任何提示。查了半天最后发现是系统里只有VC 2013的运行库没有2015-2022的Redistributable。更隐蔽的是有些软件会安装旧版MSVCP140.dll新程序调用新函数时就会崩溃。解决方法是去官网下载最新的Microsoft Visual C Redistributable合集x64和x86都装上然后重启。这类问题在Linux上完全不会遇到所以我强烈建议复现SLAM项目优先用Linux。5.2 标定数据与曝光设置是成败关键如果让我说FAST-LIVO2复现里最重要的环节排第一的不是编译而是标定。外参差1厘米在10米外的误差就能放大到几十厘米。特别是在雷达和相机不在同一位置时外参错一点点图像投影到点云地图上就会出现明显的重影。我后来养成一个习惯每次更换传感器安装位置后都会重新标定并录制一段标定bag留档。千万不要把上一次的配置直接拿来用。相机曝光方面建议把自动曝光关了固定到一个合适值否则过隧道或进出窗户时光度残差会发生突变系统短暂跳动。如果你实在要用自动曝光可以在配置里调低视觉权重来缓解。5.3 硬件与实时性优化建议FAST-LIVO2其实对硬件要求不算高我用一台i5-12400的机器就能跑到60Hz以上。但要注意Livox雷达的数据频率本身不高MID-70的频率是10HzAvia是10Hz点云处理在单线程内完成。如果你用其他雷达比如机械雷达可能需要适配驱动并调整去畸变逻辑。CPU占用大头通常在建图和配准降低体素分辨率可以明显减少CPU占用。如果你在嵌入式设备上跑可以关掉RVIZ可视化用在线录制bag方式减少负载。同时使用ROS的realtime内核和设置CPU频率为performance模式能让时间延迟更稳定。5.4 向自建系统迁移的几点提醒把FAST-LIVO2集成到自己的机器人系统时要注意几个工程细节。首先是话题设计它内部订阅imu/data、点云话题和图像话题话题名称必须与你的驱动一致否则系统一直等数据。其次是坐标系TF树要设置好world到body的关系由算法维护不要自己乱发TF免得冲突。第三如果你需要回环检测FAST-LIVO2自己不带可以在后端接FAST_LIO_SLAM或者基于扫描匹配的闭环模块。不过我的经验是对于大多数局部里程计需求比如无人机悬停控制、地面机器人导航、手持设备建图FAST-LIVO2单独跑已经足够稳定了。最后再分享一个我自己的使用习惯在跑FAST-LIVO2之前我会先用静态数据验证传感器的内参和外参是否可靠再跑一段缓慢的手持数据看地图是否完整。这套流程虽然多花十分钟但能帮我在后期节省数小时的调试时间。希望你也能顺利跑通然后享受把三路传感器融合在一个位姿里的那种愉悦感。