1. VINS-Fusion 是什么为什么值得折腾先把话说在前面VINS-Fusion 是目前多传感器融合定位领域里最适合拿来练手也最适合做二次开发的一套开源方案。它出自港科大的空中机器人实验室核心思路是把视觉、IMU、GPS 三种传感器数据揉在一起输出一段平滑、低漂移、全局一致的运动轨迹。我最早接触它是做机器人平台定位后来在无人车上也用它跑过一长段真实道路整体感受是代码结构清晰、文档算全、坑也确实不少。如果你正准备做 GPS/IMU/视觉融合或者想用 KITTI 数据集验证自己写的算法这篇内容大概率能帮你省下几周踩坑时间。先说清楚这套系统能解决什么问题。单独的视觉里程计在光照变化、快速旋转、纯纹理缺失场景下特别容易跑飞单独的 IMU 又存在明显的积分漂移几秒钟不矫正姿态就飘得没法看GPS 虽然全局无漂移但在城市峡谷、树荫、隧道里信号差甚至完全丢失而且输出频率低。VINS-Fusion 的做法是把三者放到一个紧耦合优化框架里让视觉提供局部几何约束IMU 提供帧间运动和重力方向GPS 提供全局位置修正最终得到既平滑又没有累计漂移的轨迹。这套设计思路在学术界叫多传感器融合状态估计放到工程上就是一个非常实用的定位基座。谁适合参考这篇内容如果你正在做自动驾驶、移动机器人、AR/VR 定位或者无人机的视觉导航尤其是已经跑通了 ORB-SLAM、LOAM 这类单传感器方案、想往多传感器融合方向深入的同学这篇笔记值得花时间看完。我尽量把从环境搭建、数据集准备到融合调参的全过程都记录下来包括那些文档里不会写的细节。2. 环境准备与编译避坑2.1 依赖项安装最容易翻车的一步VINS-Fusion 的依赖不复杂ROS、Ceres Solver、OpenCV、Eigen。但不复杂和不容易翻车是两回事我在这步见过太多人卡住十有八九是版本没对齐。先说 ROS。我当时用的是 Ubuntu 18.04 ROS Melodic后来在 20.04 ROS Noetic 上也完整跑过一遍都能正常工作。Melodic 上编译最顺利很多老坑已经被前人填完了遇到问题搜一下就有答案。Noetic 上要注意一点OpenCV 默认是 4.x而 VINS-Fusion 早期代码对 OpenCV 4 的兼容性有点问题主要是CV_FM_LMEDS、CV_RGB2GRAY这些常量被改名了。如果你用 Noetic建议直接拉最新版代码新版已经做了兼容处理。再强调一下 Ceres Solver。VINS-Fusion 的后端优化完全依赖 Ceres这个库的版本选择很讲究。我实测下来Ceres 1.14.0 配合 Eigen 3.3.x 最稳妥太新的 Ceres比如 2.x在编译时可能会因为接口变化报一些莫名其妙的错误。如果你不想折腾直接按我下面这套顺序装# 安装 Eigen sudo apt install libeigen3-dev # 编译 Ceres 1.14.0 git clone https://github.com/ceres-solver/ceres-solver.git cd ceres-solver git checkout 1.14.0 mkdir build cd build cmake .. make -j4 sudo make install这里有个小细节Ceres 编译依赖的glog、gflags、suitesparse一定要提前装好否则 cmake 会卡在依赖检测上。Ubuntu 下直接一行命令搞定sudo apt install libgoogle-glog-dev libgflags-dev libatlas-base-dev libsuitesparse-dev2.2 编译 VINS-Fusion 本体过程记录依赖装好之后编译 VINS-Fusion 本体就轻松了。把代码克隆到你的工作空间 src 目录下cd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd ~/catkin_ws catkin_make如果一切顺利你会看到vins_fusion、camera_models、global_fusion、loop_fusion这些包逐个编译通过。但我赌你大概率会碰到几个问题这里提前排雷。第一个高频报错是找不到opencv头文件。这通常是因为系统里同时存在多个 OpenCV 版本CMake 找错了目标。解决办法是在CMakeLists.txt里显式指定版本或者直接把 OpenCV 的 include 路径加到环境变量里。更省事的做法是装好libopencv-dev后用默认路径让 cmake 自己找。第二个坑是编译过程中vins_fusion包报 C 标准相关错误。新版代码默认用了 C14 的特性但部分老旧依赖头文件按 C11 标准写的混编时容易冲突。如果遇到可以在CMakeLists.txt里检查set(CMAKE_CXX_STANDARD 14)是否被注释掉取消注释即可。第三个坑是loop_fusion依赖的 DBoW2 库。这个库在部分版本下编译会报Vocabulary相关错误原因是 OpenCV 接口变化。新版 VINS-Fusion 已经内嵌了修改过的 DBoW2一般不会触发这个坑但如果你拉到的是旧版本分支记得先处理。我个人的建议是直接用 master 分支省心。注意如果你用 Docker 跑这套环境记得把--nethost加上否则后面用 rviz 可视化、播放 bag 包时容易碰到网络通信问题明明节点都起来了rviz 里就是看不到轨迹。3. KITTI 数据集下载、解压、格式化3.1 先搞懂 KITTI 数据到底长什么样KITTI 是视觉 SLAM 领域最经典的数据集之一由德国卡尔斯鲁厄理工学院和丰田美国研究院联合发布采集车在市区、乡村、高速公路上行驶车上装了双目相机、激光雷达、GPS/IMU 组合导航系统。VINS-Fusion 支持的 KITTI 数据主要是 odometry 部分也就是00到10这 11 个序列每个序列都提供了左右目图像和对应的 GPS/IMU 数据。下载下来之后你会看到这样的目录结构KITTI/ ├── dataset/ │ ├── sequences/ │ │ ├── 00/ │ │ │ ├── image_0/ # 左目灰度图 │ │ │ ├── image_1/ # 右目灰度图 │ │ │ ├── calib.txt # 相机内参和外参 │ │ │ └── times.txt # 每帧图像的时间戳 │ └── poses/ │ └── 00.txt # 真值轨迹可选有一点容易被新手忽略KITTI 的 GPS/IMU 数据不在sequences目录里面而是在 raw data 部分文件名带_oxts后缀。VINS-Fusion 官方脚本直接下载的是 odometry 数据里面并没有打包 GPS 信息。所以如果你想跑 GPS 融合实验需要额外去 raw data 里下载对应的_oxts数据或者直接用我后面讲到的脚本生成带 GPS 的 bag 包。3.2 用 KITTI 跑 VINS-Fusion选择哪种模式VINS-Fusion 对 KITTI 的支持主要有两种模式双目 IMU 模式以及双目 GPS 融合模式。第一种用于验证视觉惯性里程计本身的精度第二种用于验证全局融合后的轨迹一致性。如果你只是想快速看到效果建议先跑双目 IMU 模式确认整个链路通了再上 GPS。原因很简单GPS 数据预处理涉及坐标系转换和时间戳对齐中间多好几个环节一旦出问题你很难判断是 VINS-Fusion 本身的问题还是数据预处理的问题。先跑通视觉惯性部分相当于把系统最核心的定位骨架验证了再加 GPS 就是锦上添花。我在实际跑的时候遇到一个重要问题KITTI 的times.txt时间戳单位是秒而且是十进制浮点数比如1.000000e01。VINS-Fusion 读取时会把这些时间戳直接当作 ROS 时间戳用但 ROS bag 里的时间戳是以秒为单位的 nanosecond 精度。如果你直接用官方 launch 文件跑时间戳是没有问题的因为官方脚本内部做了转换。但如果你自己写 Python 脚本生成 bag这块特别容易出错后面会展开讲。3.3 实测跑通 KITTI 00 序列的完整过程我用 00 序列做过大量测试给大家一个完整的操作流程参考。第一步把下载好的 KITTI 数据放到约定目录。假设你放在~/dataset/KITTI/odometry/dataset/sequences那么目录结构是~/dataset/KITTI/odometry/dataset/sequences/00/ ├── image_0/ ├── image_1/ ├── calib.txt └── times.txt第二步直接使用 VINS-Fusion 提供的 KITTI 配置和 launch 文件。官方帮你写好了kitti_odom.launchroslaunch vins_fusion kitti_odom.launch默认情况下它会读取~/dataset/KITTI/odometry/dataset/sequences/00这个路径然后开始发布/vins_fusion/odometry和/vins_fusion/path等话题同时打开 rviz 显示轨迹。如果你不想改路径就把 00 序列放到它默认读取的位置或者修改 launch 文件里的参数。提示官方提供的 rviz 配置文件路径在vins_fusion/rviz/rviz_show.rviz如果启动后 rviz 是空白的八成是 launch 文件里config参数没有正确指向这个文件。第三步用 bag 包方式回放。VINS-Fusion 官方提供了一个 Python 脚本kitti2bag.py可以把 KITTI 数据转成 bag 包。这个脚本在vins_fusion/data目录下。使用方式大致是python kitti2bag.py -t 00 -o /tmp/kitti_00.bag ~/dataset/KITTI/odometry执行后会生成一个包含左右目图像、IMU、GPS 的 ROS bag 包。然后先启动 VINS-Fusion再用rosbag play回放roslaunch vins_fusion kitti_odom.launch rosbag play /tmp/kitti_00.bag用 bag 包的好处是你可以反复回放同一份数据方便调试、调参、对比不同参数下的定位效果。我后期做参数对比基本全靠这个方式一遍遍回放一边看 rviz 轨迹变化。4. GPS/IMU/视觉融合原理与实操4.1 GPS 在 VINS-Fusion 里到底怎么起作用很多人在跑 VINS-Fusion 的 GPS 融合时有个误区以为 VINS-Fusion 是把 GPS 原始经纬度直接加到优化框架里然后把视觉轨迹往 GPS 上靠。实际上它的做法更聪明。VINS-Fusion 的 GPS 融合节点在global_fusion包里它接收两类输入一类是/vins_fusion/odometry来自视觉惯性里程计模块的高频局部位姿大概 20~30Hz另一类是/gps话题类型为sensor_msgs/NavSatFix也就是标准的 GPS 经纬度消息。GPS 消息进来之后global_fusion节点会用 GeographicLib 把经纬度转换成局部地固坐标系下的 XYZ 坐标然后把这些 GPS 位姿约束和视觉惯性里程计的位姿一起放进因子图里做位姿图优化。每一步优化的核心思想是无 GPS 信号时系统完全依赖视觉惯性里程计保证局部平滑和短时精度有 GPS 信号时GPS 作为绝对位置约束把长时间积累的漂移拉回来。这个思路相当于 GPS 是全局坐标锚点视觉惯性是局部运动生成器两者通过因子图交换信息。正是这种设计让系统能在 GPS 信号差的地方不崩有信号的地方不飘。4.2 IMU 标定别直接拿出厂参数用IMU 是 VINS-Fusion 里最娇气的传感器。视觉给的是相对运动IMU 给的是绝对的角速度和加速度两者融合需要很精确的 IMU 内参。如果你用 KITTI 的 IMU 数据它自带的标定参数已经比较准了直接用官方配置跑没问题。但如果你换了自己的硬件平台比如我之前用 Intel RealSense D435i就踩了大坑。D435i 内置 IMU 的出厂参数是可以用的但精度不够支撑长时间融合大概跑几十秒之后 yaw 角的漂移就很明显。解决办法是用imu_utils做 Allan 方差标定流程分三步先静止采集至少两小时的 IMU 数据然后跑imu_utils得到噪声密度和随机游走最后把标定结果填进 VINS-Fusion 的配置文件。我做的过程中发现一个关键点采集数据时设备要放在稳定的平台上并且远离散热风扇、电机这些振动源。IMU 标定的本质是测量噪声的统计特性如果环境里有额外振动标定出来的噪声密度会偏大整个系统对加速度计和陀螺仪的信任度就会下降最终表现是轨迹整体精度变差。如果你的硬件没有提供标定好的 IMU 内外参至少要做到两点一是仔细阅读芯片手册确认量程二是用静止数据估计噪声参数。直接拿默认参数去跑融合很多时候不是稍微漂一点的问题而是初始化就失败系统一直处于waiting for good IMU data状态。4.3 视觉惯性联合初始化最容易失败的环节VINS-Fusion 的初始化机制值得单独说一下。系统启动后并不会立刻输出里程计而是要等到视觉和 IMU 的运动充分激励之后才完成联合初始化。什么叫充分激励就是传感器需要经历足够的加速和旋转让系统能同时估计出重力方向、尺度因子和初始速度。所以在测试的时候不要一启动就让机器人原地不动也不要只做匀速直线运动。最好是一开始就做一个包含加速、减速、转弯、稍微倾斜的动作几秒钟到十几秒即可。这个动作的目的就是让 IMU 的加速度计和陀螺仪信号里出现足够的特征让优化问题可解。我遇到过不少朋友说VINS-Fusion 跑不起来一直初始化失败结果一看启动后的前几秒数据平台基本没动。原地静止三五秒内没法初始化是正常的但如果动了十几秒还是不行就要检查 IMU 数据率和时间戳了。VINS-Fusion 对 IMU 频率有最低要求一般至少 100Hz 才能获得较好的初始化效果。如果 IMU 只有 50Hz建议先内插到 100Hz否则视觉和 IMU 的残差匹配会很吃力。5. 调参与误差优化别被定位漂移搞崩溃5.1 关键参数解读按经验排序VINS-Fusion 的配置集中在 yaml 文件里。每个参数都有它的意义但我建议按优先级调整不要一上来全改一遍。我整理了一张参考表按影响程度排序表格VINS-Fusion 核心参数影响排序参数名作用影响程度经验取值imu_topicIMU 数据话题名极高与驱动发布一致image_topic图像话题名极高左右目分别配置max_imu_accumulationIMU 数据缓存长度高默认值即可max_features_num每帧提取特征点数高150~300keyframe_parallax关键帧选择视差阈值高默认 15 像素acc_n/gyr_n加速度计/陀螺仪噪声密度高由 IMU 标定获得acc_w/gyr_w加速度计/陀螺仪随机游走高由 IMU 标定获得estimate_extrinsic是否在线估计相机-IMU外参中第一遍设 1标定后设 0solver_type优化求解器选择低默认即可优先处理acc_n/gyr_n/acc_w/gyr_w这几个噪声参数。如果你没有标定条件至少根据自己的 IMU 芯片手册填一个合理范围。以 D435i 为例加速度计噪声密度大约0.02 m/s^2/sqrt(Hz)量级陀螺仪约0.002 rad/s/sqrt(Hz)。填错了会导致系统过度信任 IMU漂移会非常明显。keyframe_parallax也很关键它决定多少像素的视差才触发新的关键帧。值设小了会频繁插入关键帧计算负担大设大了关键帧稀疏局部精度下降。在 KITTI 数据集上15 像素是个稳妥起点。如果你在室内小场景可以把阈值调低到 10让关键帧更密。5.2 常见误差来源逐个排查融合定位出问题第一件事不是调参而是分清误差来源。我总结了一套排查顺序先看视觉本身。把话题/vins_fusion/feature_img可视化出来看看特征点是否均匀分布在图像上有没有大片的无特征区域。KITTI 数据在多数场景下光照稳定、纹理丰富特征点分布比较理想。但如果你在做真实道路测试遇到白墙、天空占比大的画面特征点数量会骤降定位精度直接受影响。再看 IMU 信号。用rqt_plot订阅 IMU 原始话题检查角速度和线速度是否有明显的跳变或缺失。IMU 数据最常出现的问题是时间戳抖动导致 VINS-Fusion 在前后端时间对齐时产生误差。如果 IMU 话题数据率不稳定可以在驱动端做平滑或采用更高精度的同步机制。最后看时间同步。VINS-Fusion 里视觉和 IMU 是通过时间戳对齐的如果你的驱动给图像和 IMU 打的时间戳不同步融合效果会非常差。我做 D435i 实验时发现ROS 里的d435i_camera驱动默认可以对 IMU 和图像做硬件同步但不同 ROS 版本下同步精度不同。最稳妥的办法是查看话题时间戳确保左右目图像和 IMU 时间戳差值在毫秒量级。5.3 GPS 融合调参思路先局部后全局GPS 融合阶段的调参思路和纯视觉惯性不一样。纯视觉惯性定位关心的是短时轨迹平滑度GPS 融合关心的是整体轨迹是否和真实路线贴合。我建议分两步验证。第一步先关闭 GPS单纯跑视觉惯性里程计把轨迹保存下来观察长距离行驶后累计漂移有多少。这样你能知道系统在没有全局约束时的极限精度。第二步打开 GPS 融合保证 GPS 信号源正确、话题发布频率不低于 5Hz再看融合后的轨迹是否被拉回真值附近。需要注意的是GPS 融合算法的默认配置适合城市道路的 GPS 精度如果 GPS 信号受到多路径效应影响跳动很大可以考虑在global_fusion的配置里增大 GPS 噪声协方差让系统对 GPS 的信任度降低一些避免轨迹被带偏。我在 KITTI 00 序列上做对比时纯视觉惯性模式跑完 3.7km累计漂移大概 20 多米加上 GPS 融合后终点误差基本在 1 米以内。这个提升非常直观也是 GPS 融合价值的最好证明。6. 问题排查与避坑清单6.1 编译期问题速查这一步的问题前面已经拆解过一部分这里整理成速查表方便你对照排查。表格编译期常见问题现象原因解决方案fatal error: opencv2/opencv.hpp: No such file or directoryOpenCV 未安装或路径不对安装libopencv-dev或指定OpenCV_DIRundefined reference to cv::xxx编译时链接的 OpenCV 版本不统一统一系统 OpenCV 版本重新编译全部依赖Ceres Solver not foundCeres 未安装或版本过新安装 Ceres 1.14.0DBoW2 compile error拉到了旧版本代码使用 master 分支catkin_make 过程中内存不足多线程编译占满内存用make -j2降低并发数6.2 运行期问题排查实录编译通过只是刚开始运行中才是真正的重头戏。这里写几个我实际踩过且反复出现的坑。第一坑启动后 rviz 看不到轨迹。这个问题的根源通常是话题名不一致。VINS-Fusion 发布轨迹的话题一般是/vins_fusion/path但如果 rviz 固定订阅的是/path就什么都看不到。解决方法是先rostopic list确认话题名再在 rviz 里手动修改订阅主题。第二坑跑 KITTI bag 时图像速度飞快轨迹直接飞出去。这是因为 bag 包里的消息时间戳和系统时间不一致或者rosbag play没有加--clock参数。VINS-Fusion 依赖模拟时间做缓存和同步正确的回放方式是rosparam set /use_sim_time true rosbag play --clock /tmp/kitti_00.bag注意use_sim_time要在启动 VINS-Fusion 之前设置否则节点启动时读取的仍然是系统时间后面肯定会出问题。第三坑GPS 话题有数据但没有发布融合结果。我遇到过 GPS 一直在线但global_fusion节点的输出 path 完全没变化的情况。排查后发现是 GPS 数据的时间戳和视觉里程计时间戳差距太大导致 GPS 约束在优化里一直被当作异常值剔除。解决方法是检查两路消息时间戳是否在同一单位下必要时手动修正 GPS 话题的时间戳。6.3 避坑速查清单最后给一份我整理出来的避坑清单内容不多每一条都值得打印出来贴显示器边上装依赖时先装ceres-solver 1.14.0不要直接apt install libceres-dev版本太新了反而容易编译失败。运行 KITTI 官方 launch 前先确认路径存在不要把数据放到带空格的路径下ROS 对路径中的空格处理容易出问题。用 bag 回放时必须设置use_sim_time否则 VINS-Fusion 内部的时间戳对齐逻辑会全部乱掉。改完 yaml 参数后必须重启节点生效VINS-Fusion 没有热加载机制别以为改完配置就能自动重新读取。打开 rviz 时建议加载官方的rviz_show.rviz手动添加话题容易遗漏odometry、path、feature_img等关键显示项。记录轨迹前先把文件保存路径配置好避免实验结束才发现数据没有写出来。做真实设备实验前先用标定工具确认 IMU 噪声参数不要直接沿用别人项目的数值传感器个体差异比你想象的大得多。最后一条也是最重要的每次只修改一个参数跑完一整个序列对比效果再改下一个。多参数一起调出了问题根本不知道是谁引起的。我个人的体会是VINS-Fusion 这套系统非常有意思它把视觉、IMU、GPS 之间的互补关系演绎得比较典型吃透它之后再去看 RTK 融合、激光雷达与 IMU 标定、车辆运动学约束这些进阶方案思路会顺畅很多。KITTI 数据集又是很好的实验场既有真值做参考又能覆盖多种场景。技术方向上后续可以继续做的是把 RTK 的厘米级定位引入因子图替换普通 GPS 单点定位或者给系统加上回环检测模块让它在重访场景下再次收敛。这些方向我做了一部分之后再开篇文章单独聊。最后再分享一个小技巧在跑完一段序列后用evo工具做轨迹精度评估它可以直接读取 TUM 或 KITTI 格式的轨迹文件输出 RPE、APE、RMSE 等指标。把 VINS-Fusion 的轨迹导出成 TUM 格式之后再用evo和真值对比你会更直观地看到每次调整参数带来的精度提升这种数据驱动的调参方式比肉眼在 rviz 里判断可靠得多。